From ca09872a849c182a1e310887c8b02cf02b7921bc Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 7 Dec 2023 09:33:53 +0100 Subject: [PATCH 01/34] move creation of stateinterfaces to systeminterface --- .../include/hardware_interface/handle.hpp | 14 ++ .../hardware_interface/hardware_info.hpp | 21 +++ .../hardware_interface/system_interface.hpp | 167 +++++++++++++++++- 3 files changed, 200 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index dc536e51be..a5e6003516 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,6 +17,7 @@ #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" namespace hardware_interface @@ -114,6 +115,13 @@ class ReadWriteHandle : public ReadOnlyHandle class StateInterface : public ReadOnlyHandle { public: + explicit StateInterface( + const InterfaceDescription & interface_description, double * value_ptr = nullptr) + : ReadOnlyHandle( + interface_description.prefix_name, interface_description.interface_info.name, value_ptr) + { + } + StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; @@ -124,6 +132,12 @@ class StateInterface : public ReadOnlyHandle class CommandInterface : public ReadWriteHandle { public: + explicit CommandInterface( + const InterfaceDescription & interface_description, double * value_ptr = nullptr) + : ReadWriteHandle( + interface_description.prefix_name, interface_description.interface_info.name, value_ptr) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 2bd2099e69..350a116751 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -126,6 +126,27 @@ struct TransmissionInfo std::unordered_map parameters; }; +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), interface_info(interface_info_in) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + InterfaceInfo interface_info; + + std::string get_name() const { return prefix_name + interface_info.name; } +}; + /// This structure stores information about hardware defined in a robot's URDF. struct HardwareInfo { diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f9173fb754..67e6db9608 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -21,6 +21,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -118,9 +119,102 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) { + info_ = hardware_info; + add_state_interface_descriptions(); + joint_pos_states_.resize( + state_interfaces_pos_descr_.size(), std::numeric_limits::quiet_NaN()); + joint_vel_states_.resize( + state_interfaces_vel_descr_.size(), std::numeric_limits::quiet_NaN()); + joint_acc_states_.resize( + state_interfaces_acc_descr_.size(), std::numeric_limits::quiet_NaN()); + joint_eff_states_.resize( + state_interfaces_eff_descr_.size(), std::numeric_limits::quiet_NaN()); + + add_command_interface_descriptions(); + joint_pos_commands_.resize( + command_interfaces_pos_descr_.size(), std::numeric_limits::quiet_NaN()); + joint_vel_commands_.resize( + command_interfaces_vel_descr_.size(), std::numeric_limits::quiet_NaN()); + joint_acc_commands_.resize( + command_interfaces_acc_descr_.size(), std::numeric_limits::quiet_NaN()); + joint_eff_commands_.resize( + command_interfaces_eff_descr_.size(), std::numeric_limits::quiet_NaN()); + return CallbackReturn::SUCCESS; }; + virtual void add_state_interface_descriptions() + { + for (const auto & joint : info_.joints) + { + for (const auto & state_interface : joint.state_interfaces) + { + if (state_interface.name == hardware_interface::HW_IF_POSITION) + { + state_interfaces_pos_descr_.emplace_back( + InterfaceDescription(joint.name, state_interface)); + } + else if (state_interface.name == hardware_interface::HW_IF_VELOCITY) + { + state_interfaces_vel_descr_.emplace_back( + InterfaceDescription(joint.name, state_interface)); + } + else if (state_interface.name == hardware_interface::HW_IF_ACCELERATION) + { + state_interfaces_acc_descr_.emplace_back( + InterfaceDescription(joint.name, state_interface)); + } + else if (state_interface.name == hardware_interface::HW_IF_EFFORT) + { + state_interfaces_eff_descr_.emplace_back( + InterfaceDescription(joint.name, state_interface)); + } + else + { + throw std::runtime_error( + "Creation of InterfaceDescription failed.The provided joint type of the state " + "interface is unknow."); + } + } + } + } + + virtual void add_command_interface_descriptions() + { + for (const auto & joint : info_.joints) + { + for (const auto & command_interface : joint.command_interfaces) + { + if (command_interface.name == hardware_interface::HW_IF_POSITION) + { + command_interfaces_pos_descr_.emplace_back( + InterfaceDescription(joint.name, command_interface)); + } + else if (command_interface.name == hardware_interface::HW_IF_VELOCITY) + { + command_interfaces_vel_descr_.emplace_back( + InterfaceDescription(joint.name, command_interface)); + } + else if (command_interface.name == hardware_interface::HW_IF_ACCELERATION) + { + command_interfaces_acc_descr_.emplace_back( + InterfaceDescription(joint.name, command_interface)); + } + else if (command_interface.name == hardware_interface::HW_IF_EFFORT) + { + command_interfaces_eff_descr_.emplace_back( + InterfaceDescription(joint.name, command_interface)); + } + else + { + throw std::runtime_error( + "Creation of InterfaceDescription failed.The provided joint type of the command " + "interface is unknow."); + } + } + } + } + /// Exports all state interfaces for this hardware interface. /** * The state interfaces have to be created and transferred according @@ -130,7 +224,33 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_state_interfaces() + { + std::vector state_interfaces; + + for (size_t i = 0; i < state_interfaces_pos_descr_.size(); ++i) + { + state_interfaces.emplace_back( + StateInterface(state_interfaces_pos_descr_[i], &joint_pos_states_[i])); + } + for (size_t i = 0; i < state_interfaces_vel_descr_.size(); ++i) + { + state_interfaces.emplace_back( + StateInterface(state_interfaces_vel_descr_[i], &joint_vel_states_[i])); + } + for (size_t i = 0; i < state_interfaces_acc_descr_.size(); ++i) + { + state_interfaces.emplace_back( + StateInterface(state_interfaces_acc_descr_[i], &joint_acc_states_[i])); + } + for (size_t i = 0; i < state_interfaces_eff_descr_.size(); ++i) + { + state_interfaces.emplace_back( + StateInterface(state_interfaces_eff_descr_[i], &joint_eff_states_[i])); + } + + return state_interfaces; + } /// Exports all command interfaces for this hardware interface. /** @@ -141,7 +261,33 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of command interfaces */ - virtual std::vector export_command_interfaces() = 0; + virtual std::vector export_command_interfaces() + { + std::vector command_interfaces; + + for (size_t i = 0; i < command_interfaces_pos_descr_.size(); ++i) + { + command_interfaces.emplace_back( + CommandInterface(command_interfaces_pos_descr_[i], &joint_pos_commands_[i])); + } + for (size_t i = 0; i < command_interfaces_vel_descr_.size(); ++i) + { + command_interfaces.emplace_back( + CommandInterface(command_interfaces_vel_descr_[i], &joint_vel_commands_[i])); + } + for (size_t i = 0; i < command_interfaces_acc_descr_.size(); ++i) + { + command_interfaces.emplace_back( + CommandInterface(command_interfaces_acc_descr_[i], &joint_acc_commands_[i])); + } + for (size_t i = 0; i < command_interfaces_eff_descr_.size(); ++i) + { + command_interfaces.emplace_back( + CommandInterface(command_interfaces_eff_descr_[i], &joint_eff_commands_[i])); + } + + return command_interfaces; + } /// Prepare for a new command interface switch. /** @@ -243,6 +389,23 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; + std::vector state_interfaces_pos_descr_; + std::vector state_interfaces_vel_descr_; + std::vector state_interfaces_acc_descr_; + std::vector state_interfaces_eff_descr_; + std::vector command_interfaces_pos_descr_; + std::vector command_interfaces_vel_descr_; + std::vector command_interfaces_acc_descr_; + std::vector command_interfaces_eff_descr_; + + std::vector joint_pos_states_; + std::vector joint_vel_states_; + std::vector joint_acc_states_; + std::vector joint_eff_states_; + std::vector joint_pos_commands_; + std::vector joint_vel_commands_; + std::vector joint_acc_commands_; + std::vector joint_eff_commands_; rclcpp_lifecycle::State lifecycle_state_; private: From c936484e724d4c282731d805a31f72e9487f3350 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 8 Dec 2023 10:46:02 +0100 Subject: [PATCH 02/34] store description in state_interface and provide functions for setting/gettting --- .../hardware_interface/hardware_info.hpp | 1 + .../hardware_interface/system_interface.hpp | 225 ++++++++---------- 2 files changed, 103 insertions(+), 123 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 350a116751..0ab0f520e5 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -145,6 +145,7 @@ struct InterfaceDescription InterfaceInfo interface_info; std::string get_name() const { return prefix_name + interface_info.name; } + std::string get_type() const { return interface_info.name; } }; /// This structure stores information about hardware defined in a robot's URDF. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 67e6db9608..aaffdb2253 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,6 +15,8 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include +#include #include #include @@ -121,25 +123,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { info_ = hardware_info; add_state_interface_descriptions(); - joint_pos_states_.resize( - state_interfaces_pos_descr_.size(), std::numeric_limits::quiet_NaN()); - joint_vel_states_.resize( - state_interfaces_vel_descr_.size(), std::numeric_limits::quiet_NaN()); - joint_acc_states_.resize( - state_interfaces_acc_descr_.size(), std::numeric_limits::quiet_NaN()); - joint_eff_states_.resize( - state_interfaces_eff_descr_.size(), std::numeric_limits::quiet_NaN()); - add_command_interface_descriptions(); - joint_pos_commands_.resize( - command_interfaces_pos_descr_.size(), std::numeric_limits::quiet_NaN()); - joint_vel_commands_.resize( - command_interfaces_vel_descr_.size(), std::numeric_limits::quiet_NaN()); - joint_acc_commands_.resize( - command_interfaces_acc_descr_.size(), std::numeric_limits::quiet_NaN()); - joint_eff_commands_.resize( - command_interfaces_eff_descr_.size(), std::numeric_limits::quiet_NaN()); - return CallbackReturn::SUCCESS; }; @@ -149,32 +133,23 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { for (const auto & state_interface : joint.state_interfaces) { - if (state_interface.name == hardware_interface::HW_IF_POSITION) - { - state_interfaces_pos_descr_.emplace_back( - InterfaceDescription(joint.name, state_interface)); - } - else if (state_interface.name == hardware_interface::HW_IF_VELOCITY) - { - state_interfaces_vel_descr_.emplace_back( - InterfaceDescription(joint.name, state_interface)); - } - else if (state_interface.name == hardware_interface::HW_IF_ACCELERATION) - { - state_interfaces_acc_descr_.emplace_back( - InterfaceDescription(joint.name, state_interface)); - } - else if (state_interface.name == hardware_interface::HW_IF_EFFORT) - { - state_interfaces_eff_descr_.emplace_back( - InterfaceDescription(joint.name, state_interface)); - } - else - { - throw std::runtime_error( - "Creation of InterfaceDescription failed.The provided joint type of the state " - "interface is unknow."); - } + joint_states_descr_.emplace_back(InterfaceDescription(joint.name, state_interface)); + } + } + + for (const auto & sensor : info_.sensors) + { + for (const auto & state_interface : sensor.state_interfaces) + { + sensor_states_descr_.emplace_back(InterfaceDescription(sensor.name, state_interface)); + } + } + + for (const auto & gpio : info_.gpios) + { + for (const auto & state_interface : gpio.state_interfaces) + { + gpio_states_descr_.emplace_back(InterfaceDescription(gpio.name, state_interface)); } } } @@ -185,32 +160,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { for (const auto & command_interface : joint.command_interfaces) { - if (command_interface.name == hardware_interface::HW_IF_POSITION) - { - command_interfaces_pos_descr_.emplace_back( - InterfaceDescription(joint.name, command_interface)); - } - else if (command_interface.name == hardware_interface::HW_IF_VELOCITY) - { - command_interfaces_vel_descr_.emplace_back( - InterfaceDescription(joint.name, command_interface)); - } - else if (command_interface.name == hardware_interface::HW_IF_ACCELERATION) - { - command_interfaces_acc_descr_.emplace_back( - InterfaceDescription(joint.name, command_interface)); - } - else if (command_interface.name == hardware_interface::HW_IF_EFFORT) - { - command_interfaces_eff_descr_.emplace_back( - InterfaceDescription(joint.name, command_interface)); - } - else - { - throw std::runtime_error( - "Creation of InterfaceDescription failed.The provided joint type of the command " - "interface is unknow."); - } + joint_commands_descr_.emplace_back(InterfaceDescription(joint.name, command_interface)); + } + } + for (const auto & gpio : info_.gpios) + { + for (const auto & command_interface : gpio.command_interfaces) + { + gpio_commands_descr_.emplace_back(InterfaceDescription(gpio.name, command_interface)); } } } @@ -227,26 +184,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_state_interfaces() { std::vector state_interfaces; + state_interfaces.reserve(joint_states_descr_.size()); - for (size_t i = 0; i < state_interfaces_pos_descr_.size(); ++i) + for (const auto & descr : joint_states_descr_) { - state_interfaces.emplace_back( - StateInterface(state_interfaces_pos_descr_[i], &joint_pos_states_[i])); - } - for (size_t i = 0; i < state_interfaces_vel_descr_.size(); ++i) - { - state_interfaces.emplace_back( - StateInterface(state_interfaces_vel_descr_[i], &joint_vel_states_[i])); - } - for (size_t i = 0; i < state_interfaces_acc_descr_.size(); ++i) - { - state_interfaces.emplace_back( - StateInterface(state_interfaces_acc_descr_[i], &joint_acc_states_[i])); - } - for (size_t i = 0; i < state_interfaces_eff_descr_.size(); ++i) - { - state_interfaces.emplace_back( - StateInterface(state_interfaces_eff_descr_[i], &joint_eff_states_[i])); + joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); } return state_interfaces; @@ -264,26 +207,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_command_interfaces() { std::vector command_interfaces; + command_interfaces.reserve(joint_commands_descr_.size()); - for (size_t i = 0; i < command_interfaces_pos_descr_.size(); ++i) - { - command_interfaces.emplace_back( - CommandInterface(command_interfaces_pos_descr_[i], &joint_pos_commands_[i])); - } - for (size_t i = 0; i < command_interfaces_vel_descr_.size(); ++i) - { - command_interfaces.emplace_back( - CommandInterface(command_interfaces_vel_descr_[i], &joint_vel_commands_[i])); - } - for (size_t i = 0; i < command_interfaces_acc_descr_.size(); ++i) - { - command_interfaces.emplace_back( - CommandInterface(command_interfaces_acc_descr_[i], &joint_acc_commands_[i])); - } - for (size_t i = 0; i < command_interfaces_eff_descr_.size(); ++i) + for (const auto & descr : joint_commands_descr_) { - command_interfaces.emplace_back( - CommandInterface(command_interfaces_eff_descr_[i], &joint_eff_commands_[i])); + joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); } return command_interfaces; @@ -375,6 +304,56 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + double joint_state_get_value(const InterfaceDescription & interface_descr) const + { + return joint_states_.at(interface_descr.get_name()); + } + + void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value) + { + joint_states_[interface_descr.get_name()] = value; + } + + double joint_command_get_value(const InterfaceDescription & interface_descr) const + { + return joint_commands_.at(interface_descr.get_name()); + } + + void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value) + { + joint_commands_[interface_descr.get_name()] = value; + } + + double sensor_state_get_value(const InterfaceDescription & interface_descr) const + { + return sensor_states_.at(interface_descr.get_name()); + } + + void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value) + { + sensor_states_[interface_descr.get_name()] = value; + } + + double gpio_state_get_value(const InterfaceDescription & interface_descr) const + { + return gpio_states_.at(interface_descr.get_name()); + } + + void gpio_state_set_value(const InterfaceDescription & interface_descr, const double & value) + { + gpio_states_[interface_descr.get_name()] = value; + } + + double gpio_command_get_value(const InterfaceDescription & interface_descr) const + { + return gpio_commands_.at(interface_descr.get_name()); + } + + void gpio_commands_set_value(const InterfaceDescription & interface_descr, const double & value) + { + gpio_commands_[interface_descr.get_name()] = value; + } + protected: /// Get the logger of the SystemInterface. /** @@ -389,23 +368,23 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; - std::vector state_interfaces_pos_descr_; - std::vector state_interfaces_vel_descr_; - std::vector state_interfaces_acc_descr_; - std::vector state_interfaces_eff_descr_; - std::vector command_interfaces_pos_descr_; - std::vector command_interfaces_vel_descr_; - std::vector command_interfaces_acc_descr_; - std::vector command_interfaces_eff_descr_; - - std::vector joint_pos_states_; - std::vector joint_vel_states_; - std::vector joint_acc_states_; - std::vector joint_eff_states_; - std::vector joint_pos_commands_; - std::vector joint_vel_commands_; - std::vector joint_acc_commands_; - std::vector joint_eff_commands_; + std::vector joint_states_descr_; + std::vector joint_commands_descr_; + + std::vector sensor_states_descr_; + + std::vector gpio_states_descr_; + std::vector gpio_commands_descr_; + +private: + std::map joint_states_; + std::map joint_commands_; + + std::map sensor_states_; + + std::map gpio_states_; + std::map gpio_commands_; + rclcpp_lifecycle::State lifecycle_state_; private: From b7f8a2488998674ea32d42566121059fe2554067 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 10:20:10 +0100 Subject: [PATCH 03/34] return correct name for InterfaceDescription --- hardware_interface/include/hardware_interface/hardware_info.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 0ab0f520e5..92c6e3efa4 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -144,7 +144,7 @@ struct InterfaceDescription InterfaceInfo interface_info; - std::string get_name() const { return prefix_name + interface_info.name; } + std::string get_name() const { return prefix_name + "/" + interface_info.name; } std::string get_type() const { return interface_info.name; } }; From d7fe50305f8a3a8ea41fdc89409c53c32e25bffa Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 11:40:37 +0100 Subject: [PATCH 04/34] move parsing of interface description to component parser --- .../hardware_interface/component_parser.hpp | 48 ++++++++++ .../hardware_interface/hardware_info.hpp | 11 ++- .../hardware_interface/system_interface.hpp | 65 ++++--------- hardware_interface/src/component_parser.cpp | 92 +++++++++++++++++++ 4 files changed, 169 insertions(+), 47 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 38ca0cf89d..838363f011 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -32,5 +32,53 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's SommandInterfaces for the joints + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_joint_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's SommandInterfaces for the sensors + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_sensor_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's SommandInterfaces for the gpios + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_gpio_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's CommandInterfaces for the joints + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_joint_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +/** + * \param[in] hw_info the hardware description + * \return vector filled with information about robot's CommandInterfaces for the gpios + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_gpio_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info); + +HARDWARE_INTERFACE_PUBLIC +bool parse_bool(const std::string & bool_string); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 92c6e3efa4..8e71282e21 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -142,10 +142,19 @@ struct InterfaceDescription */ std::string prefix_name; + /** + * What type of component is exported: joint, sensor or gpio + */ + std::string component_type; + + /** + * Information about the Interface type (position, velocity,...) as well as limits and so on. + */ InterfaceInfo interface_info; std::string get_name() const { return prefix_name + "/" + interface_info.name; } - std::string get_type() const { return interface_info.name; } + + std::string get_interface_type() const { return interface_info.name; } }; /// This structure stores information about hardware defined in a robot's URDF. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index aaffdb2253..625fe6da58 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,11 +15,13 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include #include #include #include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -129,47 +131,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual void add_state_interface_descriptions() { - for (const auto & joint : info_.joints) - { - for (const auto & state_interface : joint.state_interfaces) - { - joint_states_descr_.emplace_back(InterfaceDescription(joint.name, state_interface)); - } - } - - for (const auto & sensor : info_.sensors) - { - for (const auto & state_interface : sensor.state_interfaces) - { - sensor_states_descr_.emplace_back(InterfaceDescription(sensor.name, state_interface)); - } - } - - for (const auto & gpio : info_.gpios) - { - for (const auto & state_interface : gpio.state_interfaces) - { - gpio_states_descr_.emplace_back(InterfaceDescription(gpio.name, state_interface)); - } - } + joint_states_descriptions_ = parse_joint_state_interface_descriptions_from_hardware_info(info_); + gpio_states_descriptions_ = parse_gpio_state_interface_descriptions_from_hardware_info(info_); + sensor_states_descriptions_ = + parse_sensor_state_interface_descriptions_from_hardware_info(info_); } virtual void add_command_interface_descriptions() { - for (const auto & joint : info_.joints) - { - for (const auto & command_interface : joint.command_interfaces) - { - joint_commands_descr_.emplace_back(InterfaceDescription(joint.name, command_interface)); - } - } - for (const auto & gpio : info_.gpios) - { - for (const auto & command_interface : gpio.command_interfaces) - { - gpio_commands_descr_.emplace_back(InterfaceDescription(gpio.name, command_interface)); - } - } + joint_commands_descriptions_ = + parse_joint_command_interface_descriptions_from_hardware_info(info_); + gpio_commands_descriptions_ = + parse_gpio_command_interface_descriptions_from_hardware_info(info_); } /// Exports all state interfaces for this hardware interface. @@ -184,9 +157,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_state_interfaces() { std::vector state_interfaces; - state_interfaces.reserve(joint_states_descr_.size()); + state_interfaces.reserve(joint_states_descriptions_.size()); - for (const auto & descr : joint_states_descr_) + for (const auto & descr : joint_states_descriptions_) { joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); @@ -207,9 +180,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_command_interfaces() { std::vector command_interfaces; - command_interfaces.reserve(joint_commands_descr_.size()); + command_interfaces.reserve(joint_commands_descriptions_.size()); - for (const auto & descr : joint_commands_descr_) + for (const auto & descr : joint_commands_descriptions_) { joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); @@ -368,13 +341,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; - std::vector joint_states_descr_; - std::vector joint_commands_descr_; + std::vector joint_states_descriptions_; + std::vector joint_commands_descriptions_; - std::vector sensor_states_descr_; + std::vector sensor_states_descriptions_; - std::vector gpio_states_descr_; - std::vector gpio_commands_descr_; + std::vector gpio_states_descriptions_; + std::vector gpio_commands_descriptions_; private: std::map joint_states_; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 42432dda8d..f037a4a345 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,4 +905,96 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} + +std::vector parse_joint_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector joint_state_interface_descriptions; + joint_state_interface_descriptions.reserve(hw_info.joints.size()); + + for (const auto & joint : hw_info.joints) + { + for (const auto & state_interface : joint.state_interfaces) + { + joint_state_interface_descriptions.emplace_back( + InterfaceDescription(joint.name, state_interface)); + } + } + return joint_state_interface_descriptions; +} + +std::vector parse_sensor_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector sensor_state_interface_descriptions; + sensor_state_interface_descriptions.reserve(hw_info.sensors.size()); + + for (const auto & sensor : hw_info.sensors) + { + for (const auto & state_interface : sensor.state_interfaces) + { + sensor_state_interface_descriptions.emplace_back( + InterfaceDescription(sensor.name, state_interface)); + } + } + return sensor_state_interface_descriptions; +} + +std::vector parse_gpio_state_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector gpio_state_interface_descriptions; + gpio_state_interface_descriptions.reserve(hw_info.gpios.size()); + + for (const auto & gpio : hw_info.gpios) + { + for (const auto & state_interface : gpio.state_interfaces) + { + gpio_state_interface_descriptions.emplace_back( + InterfaceDescription(gpio.name, state_interface)); + } + } + return gpio_state_interface_descriptions; +} + +std::vector parse_joint_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector + gpio_state_intejoint_command_interface_descriptionsrface_descriptions; + gpio_state_intejoint_command_interface_descriptionsrface_descriptions.reserve( + hw_info.joints.size()); + + for (const auto & joint : hw_info.joints) + { + for (const auto & command_interface : joint.command_interfaces) + { + gpio_state_intejoint_command_interface_descriptionsrface_descriptions.emplace_back( + InterfaceDescription(joint.name, command_interface)); + } + } + return gpio_state_intejoint_command_interface_descriptionsrface_descriptions; +} + +std::vector parse_gpio_command_interface_descriptions_from_hardware_info( + const HardwareInfo & hw_info) +{ + std::vector gpio_command_interface_descriptions; + gpio_command_interface_descriptions.reserve(hw_info.gpios.size()); + + for (const auto & gpio : hw_info.gpios) + { + for (const auto & command_interface : gpio.command_interfaces) + { + gpio_command_interface_descriptions.emplace_back( + InterfaceDescription(gpio.name, command_interface)); + } + } + return gpio_command_interface_descriptions; +} + } // namespace hardware_interface From fc663d45cc9f52aa8c1502d17b1b694c7696bc3d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 12 Dec 2023 13:06:58 +0100 Subject: [PATCH 05/34] adjusted actuator- and sensor_interface as well --- .../hardware_interface/actuator_interface.hpp | 91 ++++++++++++++++++- .../hardware_interface/sensor_interface.hpp | 51 ++++++++++- .../hardware_interface/system_interface.hpp | 62 ++++++++++--- 3 files changed, 190 insertions(+), 14 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index b637068fe3..07968f59f3 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,9 +15,13 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ +#include +#include +#include #include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -117,11 +121,39 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) { + info_ = hardware_info; + import_state_interface_descriptions(info_); + import_command_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and store them. + */ + virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + joint_states_descriptions_ = + parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); + } + + /** + * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and store them. + */ + virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) + { + joint_commands_descriptions_ = + parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); + } + /// Exports all state interfaces for this hardware interface. /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * If overwritten: * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -129,10 +161,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_state_interfaces() + { + std::vector state_interfaces; + state_interfaces.reserve(joint_states_descriptions_.size()); + + for (const auto & descr : joint_states_descriptions_) + { + joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); + } + + return state_interfaces; + } /// Exports all command interfaces for this hardware interface. /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * * The command interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -140,7 +188,19 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of command interfaces */ - virtual std::vector export_command_interfaces() = 0; + virtual std::vector export_command_interfaces() + { + std::vector command_interfaces; + command_interfaces.reserve(joint_commands_descriptions_.size()); + + for (const auto & descr : joint_commands_descriptions_) + { + joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); + } + + return command_interfaces; + } /// Prepare for a new command interface switch. /** @@ -228,6 +288,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + double joint_state_get_value(const InterfaceDescription & interface_descr) const + { + return joint_states_.at(interface_descr.get_name()); + } + + void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value) + { + joint_states_[interface_descr.get_name()] = value; + } + + double joint_command_get_value(const InterfaceDescription & interface_descr) const + { + return joint_commands_.at(interface_descr.get_name()); + } + + void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value) + { + joint_commands_[interface_descr.get_name()] = value; + } + protected: /// Get the logger of the ActuatorInterface. /** @@ -242,6 +322,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; + std::vector joint_states_descriptions_; + std::vector joint_commands_descriptions_; + +private: + std::map joint_states_; + std::map joint_commands_; + rclcpp_lifecycle::State lifecycle_state_; private: diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2cff8b2a3a..25360f4fd5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,9 +15,13 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ +#include +#include +#include #include #include +#include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" @@ -117,11 +121,28 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) { + info_ = hardware_info; + import_state_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Sensor and store them. + */ + virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) + { + sensor_states_descriptions_ = + parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); + } + /// Exports all state interfaces for this hardware interface. /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * If overwritten: * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -129,7 +150,19 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_state_interfaces() + { + std::vector state_interfaces; + state_interfaces.reserve(sensor_states_descriptions_.size()); + + for (const auto & descr : sensor_states_descriptions_) + { + sensor_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()])); + } + + return state_interfaces; + } /// Read the current state values from the actuator. /** @@ -167,6 +200,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + double sensor_state_get_value(const InterfaceDescription & interface_descr) const + { + return sensor_states_.at(interface_descr.get_name()); + } + + void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value) + { + sensor_states_[interface_descr.get_name()] = value; + } + protected: /// Get the logger of the SensorInterface. /** @@ -181,6 +224,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; + + std::vector sensor_states_descriptions_; + +private: + std::map sensor_states_; + rclcpp_lifecycle::State lifecycle_state_; private: diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 625fe6da58..c2c09e7736 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -124,29 +124,44 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) { info_ = hardware_info; - add_state_interface_descriptions(); - add_command_interface_descriptions(); + import_state_interface_descriptions(info_); + import_command_interface_descriptions(info_); return CallbackReturn::SUCCESS; }; - virtual void add_state_interface_descriptions() + /** + * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint, GPIO, Sensor and store them. + */ + void import_state_interface_descriptions(const HardwareInfo & hardware_info) { - joint_states_descriptions_ = parse_joint_state_interface_descriptions_from_hardware_info(info_); - gpio_states_descriptions_ = parse_gpio_state_interface_descriptions_from_hardware_info(info_); + joint_states_descriptions_ = + parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); + gpio_states_descriptions_ = + parse_gpio_state_interface_descriptions_from_hardware_info(hardware_info); sensor_states_descriptions_ = - parse_sensor_state_interface_descriptions_from_hardware_info(info_); + parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); } - virtual void add_command_interface_descriptions() + /** + * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo. + * Separate them into the possible types: Joint and GPIO and store them. + */ + void import_command_interface_descriptions(const HardwareInfo & hardware_info) { joint_commands_descriptions_ = - parse_joint_command_interface_descriptions_from_hardware_info(info_); + parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); gpio_commands_descriptions_ = - parse_gpio_command_interface_descriptions_from_hardware_info(info_); + parse_gpio_command_interface_descriptions_from_hardware_info(hardware_info); } /// Exports all state interfaces for this hardware interface. /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * If overwritten: * The state interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -157,7 +172,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_state_interfaces() { std::vector state_interfaces; - state_interfaces.reserve(joint_states_descriptions_.size()); + state_interfaces.reserve( + joint_states_descriptions_.size() + sensor_states_descriptions_.size() + + gpio_states_descriptions_.size()); for (const auto & descr : joint_states_descriptions_) { @@ -165,11 +182,27 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); } + for (const auto & descr : sensor_states_descriptions_) + { + sensor_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()])); + } + + for (const auto & descr : gpio_states_descriptions_) + { + gpio_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + state_interfaces.emplace_back(StateInterface(descr, &gpio_states_[descr.get_name()])); + } + return state_interfaces; } /// Exports all command interfaces for this hardware interface. /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * * The command interfaces have to be created and transferred according * to the hardware info passed in for the configuration. * @@ -180,7 +213,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_command_interfaces() { std::vector command_interfaces; - command_interfaces.reserve(joint_commands_descriptions_.size()); + command_interfaces.reserve( + joint_commands_descriptions_.size() + gpio_commands_descriptions_.size()); for (const auto & descr : joint_commands_descriptions_) { @@ -188,6 +222,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); } + for (const auto & descr : gpio_commands_descriptions_) + { + gpio_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + command_interfaces.emplace_back(CommandInterface(descr, &gpio_commands_[descr.get_name()])); + } + return command_interfaces; } From 5b7b72ca848b1c54d6fb8f0c39f27fe4f65f0398 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 09:08:23 +0100 Subject: [PATCH 06/34] add first tests --- hardware_interface/src/component_parser.cpp | 5 - .../test/test_component_parser.cpp | 110 ++++++++++++++++++ hardware_interface/test/test_handle.cpp | 31 +++++ 3 files changed, 141 insertions(+), 5 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index f037a4a345..54d24474bc 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,11 +905,6 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } -bool parse_bool(const std::string & bool_string) -{ - return bool_string == "true" || bool_string == "True"; -} - std::vector parse_joint_state_interface_descriptions_from_hardware_info( const HardwareInfo & hw_info) { diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index be891787f3..aa6a56a1f5 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1404,3 +1404,113 @@ TEST_F(TestComponentParser, urdf_incomplete_throws_error) std::string(ros2_control_test_assets::urdf_tail); ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_state_descriptions = + parse_joint_state_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position"); + EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); + + EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); +} + +TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_command_descriptions = + parse_joint_command_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position"); + EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); + + EXPECT_EQ(joint_command_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); +} + +TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto sensor_state_descriptions = + parse_sensor_state_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll"); + EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); + EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); + EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); + + EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); +} + +TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_gpio_state_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); + EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); + + EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); +} + +TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_gpio_command_interface_descriptions_from_hardware_info(control_hardware[0]); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); +} diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..7d79c032f0 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -14,8 +14,11 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -64,3 +67,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} + +TEST(TestHandle, interface_description_command_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} From 616a951f3fac7269ca960d1bdfb8a3432d5278fc Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 13:36:06 +0100 Subject: [PATCH 07/34] adjust sensor_interface getting/setting and add tests --- hardware_interface/CMakeLists.txt | 1 + .../hardware_interface/sensor_interface.hpp | 33 +++- .../test/test_component_interfaces.cpp | 163 ++++++++++++++++++ .../components_urdfs.hpp | 14 ++ 4 files changed, 207 insertions(+), 4 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index d2f480e2f3..9cc766ebb0 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -78,6 +78,7 @@ if(BUILD_TESTING) ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) target_link_libraries(test_component_interfaces hardware_interface) + ament_target_dependencies(test_component_interfaces ros2_control_test_assets) ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface) diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 25360f4fd5..fcb7633c70 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "hardware_interface/component_parser.hpp" @@ -123,6 +124,13 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { info_ = hardware_info; import_state_interface_descriptions(info_); + state_interface_names_.reserve(state_interface_descriptions_.size()); + for (auto & descr : state_interface_descriptions_) + { + state_interface_names_.push_back(descr.get_name()); + state_interface_names_to_descriptions_.insert( + std::make_pair(state_interface_names_.back(), descr)); + } return CallbackReturn::SUCCESS; }; @@ -132,7 +140,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { - sensor_states_descriptions_ = + state_interface_descriptions_ = parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); } @@ -153,9 +161,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_state_interfaces() { std::vector state_interfaces; - state_interfaces.reserve(sensor_states_descriptions_.size()); + state_interfaces.reserve(state_interface_descriptions_.size()); - for (const auto & descr : sensor_states_descriptions_) + for (const auto & descr : state_interface_descriptions_) { sensor_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()])); @@ -210,6 +218,21 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI sensor_states_[interface_descr.get_name()] = value; } + double sensor_state_get_value(const std::string & interface_name) const + { + return sensor_states_.at(interface_name); + } + + void sensor_state_set_value(const std::string & interface_name, const double & value) + { + sensor_states_[interface_name] = value; + } + + const InterfaceDescription & get_interface_description(const std::string & interface_name) + { + return state_interface_names_to_descriptions_.at(interface_name); + } + protected: /// Get the logger of the SensorInterface. /** @@ -225,10 +248,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI HardwareInfo info_; - std::vector sensor_states_descriptions_; + std::vector state_interface_names_; + std::vector state_interface_descriptions_; private: std::map sensor_states_; + std::map state_interface_names_to_descriptions_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 986c32d37b..4d2424e799 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -32,6 +32,8 @@ #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" // Values to send over command interface to trigger error in write and read methods @@ -1013,3 +1015,164 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } + +namespace test_components +{ +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & descr : state_interface_descriptions_) + { + sensor_state_set_value(descr, 0.0); + } + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + sensor_state_set_value("joint1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_sensor_default_interface_export) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(1u, state_interfaces.size()); + EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()); + EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[0].get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[0].get_value()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index b0076b859b..ca1e34baf8 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -561,6 +561,20 @@ const auto valid_urdf_ros2_control_system_robot_with_all_interfaces = + +)"; + +// Voltage Sensor only +const auto valid_urdf_ros2_control_voltage_sensor_only = + R"( + + + ros2_control_demo_hardware/CameraWithIMUSensor + 2 + + + + )"; From 3f061b5681c9dfbe7965a6d7f74b511e0a5db795 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 16:45:30 +0100 Subject: [PATCH 08/34] add more tests --- .../hardware_interface/actuator_interface.hpp | 45 +++- .../hardware_interface/sensor_interface.hpp | 26 +-- .../hardware_interface/system_interface.hpp | 110 ++++++++-- .../test/test_component_interfaces.cpp | 192 +++++++++++++++++- .../components_urdfs.hpp | 26 +++ 5 files changed, 352 insertions(+), 47 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 07968f59f3..6d09378485 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "hardware_interface/component_parser.hpp" @@ -133,8 +134,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { - joint_states_descriptions_ = + auto joint_state_interface_descriptions = parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : joint_state_interface_descriptions) + { + joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } } /** @@ -143,8 +148,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) { - joint_commands_descriptions_ = + auto joint_command_interface_descriptions = parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : joint_command_interface_descriptions) + { + joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } } /// Exports all state interfaces for this hardware interface. @@ -164,9 +173,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual std::vector export_state_interfaces() { std::vector state_interfaces; - state_interfaces.reserve(joint_states_descriptions_.size()); + state_interfaces.reserve(joint_state_interfaces_.size()); - for (const auto & descr : joint_states_descriptions_) + for (const auto & [name, descr] : joint_state_interfaces_) { joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); @@ -191,9 +200,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual std::vector export_command_interfaces() { std::vector command_interfaces; - command_interfaces.reserve(joint_commands_descriptions_.size()); + command_interfaces.reserve(joint_command_interfaces_.size()); - for (const auto & descr : joint_commands_descriptions_) + for (const auto & [name, descr] : joint_command_interfaces_) { joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); @@ -308,6 +317,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod joint_commands_[interface_descr.get_name()] = value; } + double joint_state_get_value(const std::string & interface_descr) const + { + return joint_states_.at(interface_descr); + } + + void joint_state_set_value(const std::string & interface_descr, const double & value) + { + joint_states_[interface_descr] = value; + } + + double joint_command_get_value(const std::string & interface_descr) const + { + return joint_commands_.at(interface_descr); + } + + void joint_command_set_value(const std::string & interface_descr, const double & value) + { + joint_commands_[interface_descr] = value; + } + protected: /// Get the logger of the ActuatorInterface. /** @@ -322,8 +351,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; - std::vector joint_states_descriptions_; - std::vector joint_commands_descriptions_; + std::map joint_state_interfaces_; + std::map joint_command_interfaces_; private: std::map joint_states_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index fcb7633c70..cee9e79a10 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -124,13 +124,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { info_ = hardware_info; import_state_interface_descriptions(info_); - state_interface_names_.reserve(state_interface_descriptions_.size()); - for (auto & descr : state_interface_descriptions_) - { - state_interface_names_.push_back(descr.get_name()); - state_interface_names_to_descriptions_.insert( - std::make_pair(state_interface_names_.back(), descr)); - } return CallbackReturn::SUCCESS; }; @@ -140,8 +133,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { - state_interface_descriptions_ = + auto sensor_state_interface_descriptions = parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : sensor_state_interface_descriptions) + { + sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } } /// Exports all state interfaces for this hardware interface. @@ -161,9 +158,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_state_interfaces() { std::vector state_interfaces; - state_interfaces.reserve(state_interface_descriptions_.size()); + state_interfaces.reserve(sensor_state_interfaces_.size()); - for (const auto & descr : state_interface_descriptions_) + for (const auto & [name, descr] : sensor_state_interfaces_) { sensor_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()])); @@ -228,11 +225,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI sensor_states_[interface_name] = value; } - const InterfaceDescription & get_interface_description(const std::string & interface_name) - { - return state_interface_names_to_descriptions_.at(interface_name); - } - protected: /// Get the logger of the SensorInterface. /** @@ -248,12 +240,10 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI HardwareInfo info_; - std::vector state_interface_names_; - std::vector state_interface_descriptions_; + std::map sensor_state_interfaces_; private: std::map sensor_states_; - std::map state_interface_names_to_descriptions_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index c2c09e7736..499a85a181 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "hardware_interface/component_parser.hpp" @@ -135,12 +136,24 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void import_state_interface_descriptions(const HardwareInfo & hardware_info) { - joint_states_descriptions_ = + auto joint_state_interface_descriptions = parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); - gpio_states_descriptions_ = - parse_gpio_state_interface_descriptions_from_hardware_info(hardware_info); - sensor_states_descriptions_ = + for (const auto & description : joint_state_interface_descriptions) + { + joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto sensor_state_interface_descriptions = parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : sensor_state_interface_descriptions) + { + sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto gpio_state_interface_descriptions = + parse_gpio_state_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : gpio_state_interface_descriptions) + { + gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); + } } /** @@ -149,10 +162,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void import_command_interface_descriptions(const HardwareInfo & hardware_info) { - joint_commands_descriptions_ = + auto joint_command_interface_descriptions = parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); - gpio_commands_descriptions_ = + for (const auto & description : joint_command_interface_descriptions) + { + joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } + auto gpio_command_interface_descriptions = parse_gpio_command_interface_descriptions_from_hardware_info(hardware_info); + for (const auto & description : gpio_command_interface_descriptions) + { + gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); + } } /// Exports all state interfaces for this hardware interface. @@ -173,22 +194,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { std::vector state_interfaces; state_interfaces.reserve( - joint_states_descriptions_.size() + sensor_states_descriptions_.size() + - gpio_states_descriptions_.size()); + joint_state_interfaces_.size() + sensor_state_interfaces_.size() + + gpio_state_interfaces_.size()); - for (const auto & descr : joint_states_descriptions_) + for (const auto & [name, descr] : joint_state_interfaces_) { joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); } - for (const auto & descr : sensor_states_descriptions_) + for (const auto & [name, descr] : sensor_state_interfaces_) { sensor_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()])); } - for (const auto & descr : gpio_states_descriptions_) + for (const auto & [name, descr] : gpio_state_interfaces_) { gpio_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); state_interfaces.emplace_back(StateInterface(descr, &gpio_states_[descr.get_name()])); @@ -213,16 +234,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual std::vector export_command_interfaces() { std::vector command_interfaces; - command_interfaces.reserve( - joint_commands_descriptions_.size() + gpio_commands_descriptions_.size()); + command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); - for (const auto & descr : joint_commands_descriptions_) + for (const auto & [name, descr] : joint_command_interfaces_) { joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); } - for (const auto & descr : gpio_commands_descriptions_) + for (const auto & [name, descr] : gpio_command_interfaces_) { gpio_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); command_interfaces.emplace_back(CommandInterface(descr, &gpio_commands_[descr.get_name()])); @@ -367,6 +387,56 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI gpio_commands_[interface_descr.get_name()] = value; } + double joint_state_get_value(const std::string & interface_descr) const + { + return joint_states_.at(interface_descr); + } + + void joint_state_set_value(const std::string & interface_descr, const double & value) + { + joint_states_[interface_descr] = value; + } + + double joint_command_get_value(const std::string & interface_descr) const + { + return joint_commands_.at(interface_descr); + } + + void joint_command_set_value(const std::string & interface_descr, const double & value) + { + joint_commands_[interface_descr] = value; + } + + double sensor_state_get_value(const std::string & interface_descr) const + { + return sensor_states_.at(interface_descr); + } + + void sensor_state_set_value(const std::string & interface_descr, const double & value) + { + sensor_states_[interface_descr] = value; + } + + double gpio_state_get_value(const std::string & interface_descr) const + { + return gpio_states_.at(interface_descr); + } + + void gpio_state_set_value(const std::string & interface_descr, const double & value) + { + gpio_states_[interface_descr] = value; + } + + double gpio_command_get_value(const std::string & interface_descr) const + { + return gpio_commands_.at(interface_descr); + } + + void gpio_commands_set_value(const std::string & interface_descr, const double & value) + { + gpio_commands_[interface_descr] = value; + } + protected: /// Get the logger of the SystemInterface. /** @@ -381,13 +451,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; - std::vector joint_states_descriptions_; - std::vector joint_commands_descriptions_; + std::map joint_state_interfaces_; + std::map joint_command_interfaces_; - std::vector sensor_states_descriptions_; + std::map sensor_state_interfaces_; - std::vector gpio_states_descriptions_; - std::vector gpio_commands_descriptions_; + std::map gpio_state_interfaces_; + std::map gpio_command_interfaces_; private: std::map joint_states_; diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 4d2424e799..8d1c391a8d 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1035,7 +1035,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { - for (const auto & descr : state_interface_descriptions_) + for (const auto & [name, descr] : sensor_state_interfaces_) { sensor_state_set_value(descr, 0.0); } @@ -1176,3 +1176,193 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } + +namespace test_components +{ + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + joint_state_set_value("joint1/position", 0.0); + joint_state_set_value("joint1/velocity", 0.0); + + if (recoverable_error_happened_) + { + joint_command_set_value("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + auto position_state = joint_state_get_value("joint1/position"); + joint_state_set_value( + "joint1/position", position_state + joint_command_get_value("joint1/velocity")); + joint_state_set_value("joint1/velocity", joint_command_get_value("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + joint_state_set_value("joint1/velocity", 0.0); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator_default) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); + + double velocity_value = 1.0; + command_interfaces[0].set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value + EXPECT_EQ(0, state_interfaces[1].get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index ca1e34baf8..f1bd1e87c9 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -578,6 +578,32 @@ const auto valid_urdf_ros2_control_voltage_sensor_only = )"; +const auto valid_urdf_ros2_control_dummy_actuator_only = + R"( + + + ros2_control_demo_hardware/VelocityActuatorHardware + 1.13 + 3 + + + + -1 + 1 + + + + + + transmission_interface/RotationToLinerTansmission + + 325.949 + + 1337 + + +)"; + const auto valid_urdf_ros2_control_parameter_empty = R"( From 5784ade8cfbd15c0db07d5b761248e9b164396b3 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 15:49:03 +0100 Subject: [PATCH 09/34] first steps towards variants and storing of value in handle, missing: * variant * adjusting of resource manager --- .../hardware_interface/actuator_interface.hpp | 83 ++++----- .../include/hardware_interface/handle.hpp | 90 +++++----- .../hardware_interface/sensor_interface.hpp | 41 ++--- .../hardware_interface/system_interface.hpp | 160 +++++------------- .../test/test_component_interfaces.cpp | 19 +-- .../include/transmission_interface/handle.hpp | 8 +- 6 files changed, 163 insertions(+), 238 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 6d09378485..b3dfb85071 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -170,20 +170,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() { - std::vector state_interfaces; + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + std::vector state_interfaces; + return state_interfaces; + } + + std::vector> on_export_state_interfaces() + { + std::vector> state_interfaces; state_interfaces.reserve(joint_state_interfaces_.size()); for (const auto & [name, descr] : joint_state_interfaces_) { - joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); + actuator_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(actuator_states_.at(name)); } - return state_interfaces; } - /// Exports all command interfaces for this hardware interface. /** * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created @@ -197,20 +207,31 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of command interfaces */ - virtual std::vector export_command_interfaces() + [[deprecated( + "Replaced by vector on_export_command_interfaces() method. Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() { - std::vector command_interfaces; + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_command_interfaces() + std::vector command_interfaces; + return command_interfaces; + } + + std::vector> on_export_command_interfaces() + { + std::vector> command_interfaces; command_interfaces.reserve(joint_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) { - joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); + actuator_commands_.insert(std::make_pair(name, std::make_shared(descr))); + command_interfaces.push_back(actuator_commands_.at(name)); } return command_interfaces; } - /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. @@ -297,44 +318,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } - double joint_state_get_value(const InterfaceDescription & interface_descr) const - { - return joint_states_.at(interface_descr.get_name()); - } - - void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value) - { - joint_states_[interface_descr.get_name()] = value; - } - - double joint_command_get_value(const InterfaceDescription & interface_descr) const - { - return joint_commands_.at(interface_descr.get_name()); - } - - void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value) - { - joint_commands_[interface_descr.get_name()] = value; - } - - double joint_state_get_value(const std::string & interface_descr) const + void set_state(const std::string & interface_name, const double & value) { - return joint_states_.at(interface_descr); + actuator_states_.at(interface_name)->set_value(value); } - void joint_state_set_value(const std::string & interface_descr, const double & value) + double get_state(const std::string & interface_name) const { - joint_states_[interface_descr] = value; + return actuator_states_.at(interface_name)->get_value(); } - double joint_command_get_value(const std::string & interface_descr) const + void set_command(const std::string & interface_name, const double & value) { - return joint_commands_.at(interface_descr); + actuator_commands_.at(interface_name)->set_value(value); } - void joint_command_set_value(const std::string & interface_descr, const double & value) + double get_command(const std::string & interface_name) const { - joint_commands_[interface_descr] = value; + return actuator_commands_.at(interface_name)->get_value(); } protected: @@ -355,8 +356,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::map joint_command_interfaces_; private: - std::map joint_states_; - std::map joint_commands_; + std::map> actuator_states_; + std::map> actuator_commands_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index a5e6003516..6e62b498ca 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,43 +15,59 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" +#include "hardware_interface/types/handle_datatype_definitions.hpp" +#include "hardware_interface/visibility_control.h" namespace hardware_interface { /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } - explicit ReadOnlyHandle(const std::string & interface_name) + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_name), + interface_name_(interface_description.interface_info.name), + value_(std::numeric_limits::quiet_NaN()), + value_ptr_(&value_) + { + } + + [[deprecated( + "Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] explicit Handle(const std:: + string & + interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) + [[deprecated( + "Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] explicit Handle(const char * + interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + Handle(const Handle & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle & operator=(const Handle & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(Handle && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~Handle() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } @@ -75,50 +91,24 @@ class ReadOnlyHandle return *value_ptr_; } -protected: - std::string prefix_name_; - std::string interface_name_; - double * value_ptr_; -}; - -class ReadWriteHandle : public ReadOnlyHandle -{ -public: - ReadWriteHandle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) - { - } - - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - - ReadWriteHandle(const ReadWriteHandle & other) = default; - - ReadWriteHandle(ReadWriteHandle && other) = default; - - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; - - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - - virtual ~ReadWriteHandle() = default; - void set_value(double value) { THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; } + +protected: + std::string prefix_name_; + std::string interface_name_; + double value_; + double * value_ptr_; }; -class StateInterface : public ReadOnlyHandle +class StateInterface : public Handle { public: - explicit StateInterface( - const InterfaceDescription & interface_description, double * value_ptr = nullptr) - : ReadOnlyHandle( - interface_description.prefix_name, interface_description.interface_info.name, value_ptr) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } @@ -126,16 +116,14 @@ class StateInterface : public ReadOnlyHandle StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using Handle::Handle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public Handle { public: - explicit CommandInterface( - const InterfaceDescription & interface_description, double * value_ptr = nullptr) - : ReadWriteHandle( - interface_description.prefix_name, interface_description.interface_info.name, value_ptr) + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } /// CommandInterface copy constructor is actively deleted. @@ -148,7 +136,7 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index cee9e79a10..3754ab0595 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -155,15 +155,28 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() { - std::vector state_interfaces; + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + std::vector state_interfaces; + return state_interfaces; + } + + std::vector> on_export_state_interfaces() + { + std::vector> state_interfaces; state_interfaces.reserve(sensor_state_interfaces_.size()); for (const auto & [name, descr] : sensor_state_interfaces_) { - sensor_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()])); + // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is + // inserted + sensor_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(sensor_states_.at(name)); } return state_interfaces; @@ -205,24 +218,14 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } - double sensor_state_get_value(const InterfaceDescription & interface_descr) const - { - return sensor_states_.at(interface_descr.get_name()); - } - - void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value) - { - sensor_states_[interface_descr.get_name()] = value; - } - - double sensor_state_get_value(const std::string & interface_name) const + void set_state(const std::string & interface_name, const double & value) { - return sensor_states_.at(interface_name); + sensor_states_.at(interface_name)->set_value(value); } - void sensor_state_set_value(const std::string & interface_name, const double & value) + double get_state(const std::string & interface_name) const { - sensor_states_[interface_name] = value; + return sensor_states_.at(interface_name)->get_value(); } protected: @@ -243,7 +246,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::map sensor_state_interfaces_; private: - std::map sensor_states_; + std::map> sensor_states_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 499a85a181..f62d5d98fc 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -190,31 +190,39 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of state interfaces */ - virtual std::vector export_state_interfaces() + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. Exporting is handled " + "by the Framework.")]] virtual std::vector + export_state_interfaces() { - std::vector state_interfaces; + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_state_interfaces() + std::vector state_interfaces; + return state_interfaces; + } + + std::vector> on_export_state_interfaces() + { + std::vector> state_interfaces; state_interfaces.reserve( joint_state_interfaces_.size() + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); for (const auto & [name, descr] : joint_state_interfaces_) { - joint_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()])); + system_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(system_states_.at(name)); } - for (const auto & [name, descr] : sensor_state_interfaces_) { - sensor_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()])); + system_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(system_states_.at(name)); } - for (const auto & [name, descr] : gpio_state_interfaces_) { - gpio_states_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - state_interfaces.emplace_back(StateInterface(descr, &gpio_states_[descr.get_name()])); + system_states_.insert(std::make_pair(name, std::make_shared(descr))); + state_interfaces.push_back(system_states_.at(name)); } - return state_interfaces; } @@ -231,23 +239,34 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of command interfaces */ - virtual std::vector export_command_interfaces() + [[deprecated( + "Replaced by vector on_export_command_interfaces() method. Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_command_interfaces() + { + // return empty vector by default. For backward compatibility we check if all vectors is empty + // and if so call on_export_command_interfaces() + std::vector command_interfaces; + return command_interfaces; + } + + std::vector> on_export_command_interfaces() { - std::vector command_interfaces; + std::vector> command_interfaces; command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) { - joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); + system_commands_.insert(std::make_pair(name, std::make_shared(descr))); + command_interfaces.push_back(system_commands_.at(name)); } for (const auto & [name, descr] : gpio_command_interfaces_) { - gpio_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); - command_interfaces.emplace_back(CommandInterface(descr, &gpio_commands_[descr.get_name()])); + system_commands_.insert(std::make_pair(name, std::make_shared(descr))); + command_interfaces.push_back(system_commands_.at(name)); } - return command_interfaces; } @@ -337,104 +356,24 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } - double joint_state_get_value(const InterfaceDescription & interface_descr) const - { - return joint_states_.at(interface_descr.get_name()); - } - - void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value) - { - joint_states_[interface_descr.get_name()] = value; - } - - double joint_command_get_value(const InterfaceDescription & interface_descr) const - { - return joint_commands_.at(interface_descr.get_name()); - } - - void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value) - { - joint_commands_[interface_descr.get_name()] = value; - } - - double sensor_state_get_value(const InterfaceDescription & interface_descr) const - { - return sensor_states_.at(interface_descr.get_name()); - } - - void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value) - { - sensor_states_[interface_descr.get_name()] = value; - } - - double gpio_state_get_value(const InterfaceDescription & interface_descr) const - { - return gpio_states_.at(interface_descr.get_name()); - } - - void gpio_state_set_value(const InterfaceDescription & interface_descr, const double & value) - { - gpio_states_[interface_descr.get_name()] = value; - } - - double gpio_command_get_value(const InterfaceDescription & interface_descr) const - { - return gpio_commands_.at(interface_descr.get_name()); - } - - void gpio_commands_set_value(const InterfaceDescription & interface_descr, const double & value) + void set_state(const std::string & interface_name, const double & value) { - gpio_commands_[interface_descr.get_name()] = value; + system_states_.at(interface_name)->set_value(value); } - double joint_state_get_value(const std::string & interface_descr) const + double get_state(const std::string & interface_name) const { - return joint_states_.at(interface_descr); + return system_states_.at(interface_name)->get_value(); } - void joint_state_set_value(const std::string & interface_descr, const double & value) + void set_command(const std::string & interface_name, const double & value) { - joint_states_[interface_descr] = value; + system_commands_.at(interface_name)->set_value(value); } - double joint_command_get_value(const std::string & interface_descr) const + double get_command(const std::string & interface_name) const { - return joint_commands_.at(interface_descr); - } - - void joint_command_set_value(const std::string & interface_descr, const double & value) - { - joint_commands_[interface_descr] = value; - } - - double sensor_state_get_value(const std::string & interface_descr) const - { - return sensor_states_.at(interface_descr); - } - - void sensor_state_set_value(const std::string & interface_descr, const double & value) - { - sensor_states_[interface_descr] = value; - } - - double gpio_state_get_value(const std::string & interface_descr) const - { - return gpio_states_.at(interface_descr); - } - - void gpio_state_set_value(const std::string & interface_descr, const double & value) - { - gpio_states_[interface_descr] = value; - } - - double gpio_command_get_value(const std::string & interface_descr) const - { - return gpio_commands_.at(interface_descr); - } - - void gpio_commands_set_value(const std::string & interface_descr, const double & value) - { - gpio_commands_[interface_descr] = value; + return system_commands_.at(interface_name)->get_value(); } protected: @@ -460,13 +399,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::map gpio_command_interfaces_; private: - std::map joint_states_; - std::map joint_commands_; - - std::map sensor_states_; - - std::map gpio_states_; - std::map gpio_commands_; + std::map> system_states_; + std::map> system_commands_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 8d1c391a8d..44a1e3b911 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1037,7 +1037,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface { for (const auto & [name, descr] : sensor_state_interfaces_) { - sensor_state_set_value(descr, 0.0); + set_state(name, 0.0); } read_calls_ = 0; return CallbackReturn::SUCCESS; @@ -1055,7 +1055,7 @@ class DummySensorDefault : public hardware_interface::SensorInterface } // no-op, static value - sensor_state_set_value("joint1/voltage", voltage_level_hw_value_); + set_state("joint1/voltage", voltage_level_hw_value_); return hardware_interface::return_type::OK; } @@ -1196,12 +1196,12 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override { - joint_state_set_value("joint1/position", 0.0); - joint_state_set_value("joint1/velocity", 0.0); + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); if (recoverable_error_happened_) { - joint_command_set_value("joint1/velocity", 0.0); + set_command("joint1/velocity", 0.0); } read_calls_ = 0; @@ -1233,17 +1233,16 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { return hardware_interface::return_type::ERROR; } - auto position_state = joint_state_get_value("joint1/position"); - joint_state_set_value( - "joint1/position", position_state + joint_command_get_value("joint1/velocity")); - joint_state_set_value("joint1/velocity", joint_command_get_value("joint1/velocity")); + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); return hardware_interface::return_type::OK; } CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override { - joint_state_set_value("joint1/velocity", 0.0); + set_state("joint1/velocity", 0.0); return CallbackReturn::SUCCESS; } diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index 024a019bfd..4c40189648 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -20,17 +20,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; } // namespace transmission_interface From c42d23c3eb395cdafa1e1eb77088711a73d45cce Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 16:56:27 +0100 Subject: [PATCH 10/34] add variant support --- .../include/hardware_interface/handle.hpp | 34 ++++++++++++------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 6e62b498ca..09b0134b18 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,19 +17,25 @@ #include #include +#include +#include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" -#include "hardware_interface/types/handle_datatype_definitions.hpp" #include "hardware_interface/visibility_control.h" namespace hardware_interface { + +typedef std::variant HANDLE_DATATYPE; + /// A handle used to get and set a value on a given interface. class Handle { public: - [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] Handle( + [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) @@ -38,23 +44,24 @@ class Handle explicit Handle(const InterfaceDescription & interface_description) : prefix_name_(interface_description.prefix_name), - interface_name_(interface_description.interface_info.name), - value_(std::numeric_limits::quiet_NaN()), - value_ptr_(&value_) + interface_name_(interface_description.interface_info.name) { + // As soon as multiple datatypes are used in HANDLE_DATATYPE + // we need to initialize according the type passed in interface description + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); } - [[deprecated( - "Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] explicit Handle(const std:: - string & - interface_name) + [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - [[deprecated( - "Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] explicit Handle(const char * - interface_name) + [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } @@ -100,7 +107,8 @@ class Handle protected: std::string prefix_name_; std::string interface_name_; - double value_; + HANDLE_DATATYPE value_; + // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; }; From 1be5cc4609e6ec06ad54c331c67565dd614afe5e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 13:06:25 +0100 Subject: [PATCH 11/34] adjusted resource manager to work with shared_ptr adapt actuator,sensor and system --- .../include/hardware_interface/actuator.hpp | 16 +- .../hardware_interface/actuator_interface.hpp | 6 +- .../include/hardware_interface/sensor.hpp | 8 +- .../hardware_interface/sensor_interface.hpp | 3 +- .../include/hardware_interface/system.hpp | 16 +- .../hardware_interface/system_interface.hpp | 6 +- hardware_interface/src/actuator.cpp | 10 ++ hardware_interface/src/resource_manager.cpp | 144 ++++++++++++++++-- hardware_interface/src/sensor.cpp | 5 + hardware_interface/src/system.cpp | 10 ++ 10 files changed, 201 insertions(+), 23 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3534d9f587..e873fb37f7 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -69,11 +69,23 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> on_export_state_interfaces(); + + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> on_export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index b3dfb85071..ae4df9b2c7 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -171,7 +171,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. Exporting is " + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is " "handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -208,7 +209,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of command interfaces */ [[deprecated( - "Replaced by vector on_export_command_interfaces() method. Exporting is " + "Replaced by vector> on_export_command_interfaces() method. " + "Exporting is " "handled " "by the Framework.")]] virtual std::vector export_command_interfaces() diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 4126910d44..305816fc37 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -69,8 +69,14 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> on_export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 3754ab0595..9dc5fb6d5f 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -156,7 +156,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. Exporting is handled " + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() { diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 9308872ef8..6c07f7e1a0 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -69,11 +69,23 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_state_interfaces(); + HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> on_export_state_interfaces(); + + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC + std::vector + export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> on_export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f62d5d98fc..61a37b6c3b 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -191,7 +191,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. Exporting is handled " + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() { @@ -240,7 +241,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of command interfaces */ [[deprecated( - "Replaced by vector on_export_command_interfaces() method. Exporting is " + "Replaced by vector> on_export_command_interfaces() method. " + "Exporting is " "handled " "by the Framework.")]] virtual std::vector export_command_interfaces() diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 66419402f8..785563af95 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -193,12 +193,22 @@ std::vector Actuator::export_state_interfaces() return impl_->export_state_interfaces(); } +std::vector> Actuator::on_export_state_interfaces() +{ + return impl_->on_export_state_interfaces(); +} + std::vector Actuator::export_command_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here return impl_->export_command_interfaces(); } +std::vector> Actuator::on_export_command_interfaces() +{ + return impl_->on_export_command_interfaces(); +} + return_type Actuator::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5b13e5d331..1c7daa1a28 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -603,20 +603,70 @@ class ResourceStorage return result; } + void insert_state_interface(std::shared_ptr state_interface) + { + const auto [it, success] = + state_interface_map_.insert(std::make_pair(state_interface->get_name(), state_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_state_interfaces() + // method is removed + void insert_state_interface(const StateInterface & state_interface) + { + const auto [it, success] = state_interface_map_.emplace(std::make_pair( + state_interface.get_name(), std::make_shared(state_interface))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface.get_name() + "]"); + throw std::runtime_error(msg); + } + } + // TODO(Manuel) END: for backward compatibility + template void import_state_interfaces(HardwareT & hardware) { try { - auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; - interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + std::vector interfaces = hardware.export_state_interfaces(); + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.size() == 0) + { + std::vector> interface_ptrs = + hardware.on_export_state_interfaces(); + interface_names.reserve(interface_ptrs.size()); + for (auto const & interface : interface_ptrs) + { + insert_state_interface(interface); + interface_names.push_back(interface->get_name()); + } + } + // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_state_interfaces() + // method is removed + else { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + interface_names.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + insert_state_interface(interface); + interface_names.push_back(interface.get_name()); + } } + // TODO(Manuel) END: for backward compatibility + hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); @@ -637,14 +687,61 @@ class ResourceStorage } } + void insert_command_interface(std::shared_ptr command_interface) + { + const auto [it, success] = command_interface_map_.insert( + std::make_pair(command_interface->get_name(), command_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + command_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_command_interfaces() + // method is removed + void insert_command_interface(CommandInterface && command_interface) + { + std::string key = command_interface.get_name(); + const auto [it, success] = command_interface_map_.emplace( + std::make_pair(key, std::make_shared(std::move(command_interface)))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + key + "]"); + throw std::runtime_error(msg); + } + } + // TODO(Manuel) END: for backward compatibility + template void import_command_interfaces(HardwareT & hardware) { try { auto interfaces = hardware.export_command_interfaces(); - hardware_info_map_[hardware.get_name()].command_interfaces = - add_command_interfaces(interfaces); + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.size() == 0) + { + std::vector> interface_ptrs = + hardware.on_export_command_interfaces(); + hardware_info_map_[hardware.get_name()].command_interfaces = + add_command_interfaces(interface_ptrs); + } + // TODO(Manuel) BEGIN: for backward compatibility, can be removed if + // export_command_interfaces() method is removed + else + { + hardware_info_map_[hardware.get_name()].command_interfaces = + add_command_interfaces(interfaces); + } + // TODO(Manuel) END: for backward compatibility } catch (const std::exception & ex) { @@ -714,6 +811,8 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ + // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_command_interfaces() + // method is removed std::vector add_command_interfaces(std::vector & interfaces) { std::vector interface_names; @@ -721,7 +820,26 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); - command_interface_map_.emplace(std::make_pair(key, std::move(interface))); + insert_command_interface(std::move(interface)); + claimed_command_interface_map_.emplace(std::make_pair(key, false)); + interface_names.push_back(key); + } + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + + return interface_names; + } + // TODO(Manuel) END: for backward compatibility + + std::vector add_command_interfaces( + std::vector> & interfaces) + { + std::vector interface_names; + interface_names.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + auto key = interface->get_name(); + insert_command_interface(interface); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -1014,9 +1132,9 @@ class ResourceStorage std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces - std::map state_interface_map_; + std::map> state_interface_map_; /// Storage of all available command interfaces - std::map command_interface_map_; + std::map> command_interface_map_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -1153,7 +1271,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & } std::lock_guard guard(resource_interfaces_lock_); - return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); + return LoanedStateInterface(*(resource_storage_->state_interface_map_.at(key))); } // CM API: Called in "callback/slow"-thread @@ -1385,7 +1503,7 @@ LoanedCommandInterface ResourceManager::claim_command_interface(const std::strin resource_storage_->claimed_command_interface_map_[key] = true; std::lock_guard guard(resource_interfaces_lock_); return LoanedCommandInterface( - resource_storage_->command_interface_map_.at(key), + *(resource_storage_->command_interface_map_.at(key)), std::bind(&ResourceManager::release_command_interface, this, key)); } diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9b7f1f69d6..9d69e16661 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -191,6 +191,11 @@ std::vector Sensor::export_state_interfaces() return impl_->export_state_interfaces(); } +std::vector> Sensor::on_export_state_interfaces() +{ + return impl_->on_export_state_interfaces(); +} + std::string Sensor::get_name() const { return impl_->get_name(); } std::string Sensor::get_group_name() const { return impl_->get_group_name(); } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ba327d8ab2..920c0edf6a 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -191,11 +191,21 @@ std::vector System::export_state_interfaces() return impl_->export_state_interfaces(); } +std::vector> System::on_export_state_interfaces() +{ + return impl_->on_export_state_interfaces(); +} + std::vector System::export_command_interfaces() { return impl_->export_command_interfaces(); } +std::vector> System::on_export_command_interfaces() +{ + return impl_->on_export_command_interfaces(); +} + return_type System::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) From bfb59e4465fba278bb8be7b28ad3e97fbb755b62 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:16:47 +0100 Subject: [PATCH 12/34] cleanup --- .../include/hardware_interface/handle.hpp | 15 +++++++++---- hardware_interface/src/resource_manager.cpp | 22 +++++++++---------- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 09b0134b18..e8103df5d1 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -33,7 +33,7 @@ typedef std::variant HANDLE_DATATYPE; class Handle { public: - [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + [[deprecated("Use InterfaceDescription for initializing the Interface")]] Handle( const std::string & prefix_name, const std::string & interface_name, @@ -52,14 +52,14 @@ class Handle value_ptr_ = std::get_if(&value_); } - [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] + [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) @@ -93,23 +93,30 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } double get_value() const - { + { // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; + // END } void set_value(double value) { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value_ directly if old functionality is removed THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; + // END } protected: std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; + // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; + // END }; class StateInterface : public Handle diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 1c7daa1a28..2563c32f3a 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -616,8 +616,8 @@ class ResourceStorage } } - // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_state_interfaces() - // method is removed + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_state_interfaces() method is removed void insert_state_interface(const StateInterface & state_interface) { const auto [it, success] = state_interface_map_.emplace(std::make_pair( @@ -630,7 +630,7 @@ class ResourceStorage throw std::runtime_error(msg); } } - // TODO(Manuel) END: for backward compatibility + // END: for backward compatibility template void import_state_interfaces(HardwareT & hardware) @@ -643,7 +643,7 @@ class ResourceStorage // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well // b) default implementation for export_state_interfaces() is used -> new functionality -> // Framework exports and creates everything - if (interfaces.size() == 0) + if (interfaces.empty()) { std::vector> interface_ptrs = hardware.on_export_state_interfaces(); @@ -700,8 +700,8 @@ class ResourceStorage } } - // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_command_interfaces() - // method is removed + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed void insert_command_interface(CommandInterface && command_interface) { std::string key = command_interface.get_name(); @@ -715,7 +715,7 @@ class ResourceStorage throw std::runtime_error(msg); } } - // TODO(Manuel) END: for backward compatibility + // END: for backward compatibility template void import_command_interfaces(HardwareT & hardware) @@ -727,7 +727,7 @@ class ResourceStorage // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well // b) default implementation for export_command_interfaces() is used -> new functionality -> // Framework exports and creates everything - if (interfaces.size() == 0) + if (interfaces.empty()) { std::vector> interface_ptrs = hardware.on_export_command_interfaces(); @@ -811,8 +811,8 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ - // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_command_interfaces() - // method is removed + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed std::vector add_command_interfaces(std::vector & interfaces) { std::vector interface_names; @@ -829,7 +829,7 @@ class ResourceStorage return interface_names; } - // TODO(Manuel) END: for backward compatibility + // END: for backward compatibility std::vector add_command_interfaces( std::vector> & interfaces) From 7946a776bb7bb3121e09488fdf4890a88f19e14e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:17:03 +0100 Subject: [PATCH 13/34] fix failing tests --- .../mock_components/test_generic_system.cpp | 3 - .../test/test_component_interfaces.cpp | 62 +++++++++---------- 2 files changed, 31 insertions(+), 34 deletions(-) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 15d24fc7e7..854b35c8d3 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -515,7 +515,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -546,7 +545,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -555,7 +553,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 44a1e3b911..2511ad10a8 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1097,26 +1097,26 @@ TEST(TestComponentInterfaces, dummy_sensor_default_interface_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = sensor_hw.export_state_interfaces(); + auto state_interfaces = sensor_hw.on_export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()); - EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0].get_value()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0].get_value()); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); } TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) @@ -1132,7 +1132,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; auto state = sensor_hw.initialize(voltage_sensor_res); - auto state_interfaces = sensor_hw.export_state_interfaces(); + auto state_interfaces = sensor_hw.on_export_state_interfaces(); // Updated because is is INACTIVE state = sensor_hw.configure(); state = sensor_hw.activate(); @@ -1154,7 +1154,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1285,23 +1285,23 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = actuator_hw.export_state_interfaces(); + auto state_interfaces = actuator_hw.on_export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); - - auto command_interfaces = actuator_hw.export_command_interfaces(); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + + auto command_interfaces = actuator_hw.on_export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); double velocity_value = 1.0; - command_interfaces[0].set_value(velocity_value); // velocity + command_interfaces[0]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -1309,8 +1309,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); } @@ -1324,8 +1324,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -1339,8 +1339,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -1354,8 +1354,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); } From 17d43d3e7d4395a072e9ec89833299a50da6fc9d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 21 Dec 2023 09:04:32 +0100 Subject: [PATCH 14/34] change rest of component_interface test and mark what should be removed --- .../hardware_interface/actuator_interface.hpp | 6 +- .../include/hardware_interface/handle.hpp | 5 +- .../hardware_interface/sensor_interface.hpp | 3 +- .../hardware_interface/system_interface.hpp | 6 +- .../test/test_component_interfaces.cpp | 1204 ++++++++++++----- .../components_urdfs.hpp | 35 + 6 files changed, 903 insertions(+), 356 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index ae4df9b2c7..91674d4f17 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -179,8 +179,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { // return empty vector by default. For backward compatibility we check if all vectors is empty // and if so call on_export_state_interfaces() - std::vector state_interfaces; - return state_interfaces; + return {}; } std::vector> on_export_state_interfaces() @@ -217,8 +216,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { // return empty vector by default. For backward compatibility we check if all vectors is empty // and if so call on_export_command_interfaces() - std::vector command_interfaces; - return command_interfaces; + return {}; } std::vector> on_export_command_interfaces() diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index e8103df5d1..a91e51f973 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -93,8 +93,9 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } double get_value() const - { // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) return value_ if old functionality is removed + { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; // END diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 9dc5fb6d5f..3a1ff2c95e 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -163,8 +163,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // return empty vector by default. For backward compatibility we check if all vectors is empty // and if so call on_export_state_interfaces() - std::vector state_interfaces; - return state_interfaces; + return {}; } std::vector> on_export_state_interfaces() diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 61a37b6c3b..91cc52499b 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -198,8 +198,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // return empty vector by default. For backward compatibility we check if all vectors is empty // and if so call on_export_state_interfaces() - std::vector state_interfaces; - return state_interfaces; + return {}; } std::vector> on_export_state_interfaces() @@ -249,8 +248,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // return empty vector by default. For backward compatibility we check if all vectors is empty // and if so call on_export_command_interfaces() - std::vector command_interfaces; - return command_interfaces; + return {}; } std::vector> on_export_command_interfaces() diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 2511ad10a8..b5b308e624 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -50,6 +50,7 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +// BEGIN (Handle export change): for backward compatibility class DummyActuator : public hardware_interface::ActuatorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override @@ -156,7 +157,96 @@ class DummyActuator : public hardware_interface::ActuatorInterface unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; +// END +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); + + if (recoverable_error_happened_) + { + set_command("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/velocity", 0.0); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +// BEGIN (Handle export change): for backward compatibility class DummySensor : public hardware_interface::SensorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override @@ -220,7 +310,72 @@ class DummySensor : public hardware_interface::SensorInterface int read_calls_ = 0; bool recoverable_error_happened_ = false; }; +// END + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & [name, descr] : sensor_state_interfaces_) + { + set_state(name, 0.0); + } + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("joint1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; +// BEGIN (Handle export change): for backward compatibility class DummySystem : public hardware_interface::SystemInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override @@ -354,21 +509,121 @@ class DummySystem : public hardware_interface::SystemInterface unsigned int write_calls_ = 0; bool recoverable_error_happened_ = false; }; +// END -class DummySystemPreparePerform : public hardware_interface::SystemInterface +class DummySystemDefault : public hardware_interface::SystemInterface { - // Override the pure virtual functions with default behavior - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } // We hardcode the info return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override { return {}; } + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (auto i = 0ul; i < 3; ++i) + { + set_state(position_states_[i], 0.0); + set_state(velocity_states_[i], 0.0); + } + // reset command only if error is initiated + if (recoverable_error_happened_) + { + for (auto i = 0ul; i < 3; ++i) + { + set_command(velocity_commands_[i], 0.0); + } + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySystemDefault"; } - std::vector export_command_interfaces() override + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + return hardware_interface::return_type::ERROR; + } + + for (auto i = 0; i < 3; ++i) + { + auto current_pos = get_state(position_states_[i]); + set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); + set_state(velocity_states_[i], get_command(velocity_commands_[i])); + } + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & velocity_state : velocity_states_) + { + set_state(velocity_state, 0.0); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + std::vector position_states_ = { + "joint1/position", "joint2/position", "joint3/position"}; + std::vector velocity_states_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + std::vector velocity_commands_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySystemPreparePerform : public hardware_interface::SystemInterface +{ + // Override the pure virtual functions with default behavior + CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override { - return {}; + // We hardcode the info + return CallbackReturn::SUCCESS; } std::string get_name() const override { return "DummySystemPreparePerform"; } @@ -421,6 +676,7 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface } // namespace test_components +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_actuator) { hardware_interface::Actuator actuator_hw(std::make_unique()); @@ -511,12 +767,111 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ( hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); } +// END -TEST(TestComponentInterfaces, dummy_sensor) +TEST(TestComponentInterfaces, dummy_actuator_default) { - hardware_interface::Sensor sensor_hw(std::make_unique()); + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); - hardware_interface::HardwareInfo mock_hw_info{}; + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.on_export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + + auto command_interfaces = actuator_hw.on_export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + + double velocity_value = 1.0; + command_interfaces[0]->set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + + state = actuator_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); + EXPECT_EQ( + hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); +} + +// BEGIN (Handle export change): for backward compatibility +TEST(TestComponentInterfaces, dummy_sensor) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -543,7 +898,46 @@ TEST(TestComponentInterfaces, dummy_sensor) sensor_hw.read(TIME, PERIOD); EXPECT_EQ(0x666, state_interfaces[0].get_value()); } +// END + +TEST(TestComponentInterfaces, dummy_sensor_default) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.on_export_state_interfaces(); + ASSERT_EQ(1u, state_interfaces.size()); + EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + + // Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + + // Updated because is is INACTIVE + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + + // It can read now + sensor_hw.read(TIME, PERIOD); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); +} +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_system) { hardware_interface::System system_hw(std::make_unique()); @@ -668,6 +1062,137 @@ TEST(TestComponentInterfaces, dummy_system) EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); } +// END + +TEST(TestComponentInterfaces, dummy_system_default) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.on_export_state_interfaces(); + ASSERT_EQ(6u, state_interfaces.size()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); + EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); + + auto command_interfaces = system_hw.on_export_command_interfaces(); + ASSERT_EQ(3u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); + + double velocity_value = 1.0; + command_interfaces[0]->set_value(velocity_value); // velocity + command_interfaces[1]->set_value(velocity_value); // velocity + command_interfaces[2]->set_value(velocity_value); // velocity + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + + // Noting should change because it is UNCONFIGURED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); + + // Read and Write are working because it is INACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Read and Write are working because it is ACTIVE + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + + state = system_hw.shutdown(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // Noting should change because it is FINALIZED + for (auto step = 0u; step < 10; ++step) + { + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity + + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + } + + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); + EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); +} TEST(TestComponentInterfaces, dummy_command_mode_system) { @@ -701,6 +1226,7 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) system_hw.perform_command_mode_switch(two_keys, one_key)); } +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) { hardware_interface::Actuator actuator_hw(std::make_unique()); @@ -760,19 +1286,27 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// END -TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) +TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) { - hardware_interface::Actuator actuator_hw(std::make_unique()); + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = actuator_hw.export_state_interfaces(); - auto command_interfaces = actuator_hw.export_command_interfaces(); + auto state_interfaces = actuator_hw.on_export_state_interfaces(); + auto command_interfaces = actuator_hw.on_export_command_interfaces(); state = actuator_hw.configure(); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -784,9 +1318,9 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) // Initiate error on write (this is first time therefore recoverable) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -794,8 +1328,8 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); - EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -807,9 +1341,9 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) // Initiate error on write (this is the second time therefore unrecoverable) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -821,22 +1355,151 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) +// BEGIN (Handle export change): for backward compatibility +TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) { - hardware_interface::Sensor sensor_hw(std::make_unique()); + hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); + auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = sensor_hw.export_state_interfaces(); - // Updated because is is INACTIVE - state = sensor_hw.configure(); - state = sensor_hw.activate(); + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// END + +TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.on_export_state_interfaces(); + auto command_interfaces = actuator_hw.on_export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +// BEGIN (Handle export change): for backward compatibility +TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + hardware_interface::HardwareInfo mock_hw_info{}; + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); + auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); // Initiate recoverable error - call read 99 times OK and on 100-time will return error for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) @@ -885,7 +1548,67 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// END + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + + auto state_interfaces = sensor_hw.on_export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_system_read_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -950,7 +1673,79 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// END + +TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.on_export_state_interfaces(); + auto command_interfaces = system_hw.on_export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} +// BEGIN (Handle export change): for backward compatibility TEST(TestComponentInterfaces, dummy_system_write_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -1015,353 +1810,74 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } +// END -namespace test_components -{ -class DummySensorDefault : public hardware_interface::SensorInterface +TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) { - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override - { - if ( - hardware_interface::SensorInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - for (const auto & [name, descr] : sensor_state_interfaces_) - { - set_state(name, 0.0); - } - read_calls_ = 0; - return CallbackReturn::SUCCESS; - } - - std::string get_name() const override { return "DummySensorDefault"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, static value - set_state("joint1/voltage", voltage_level_hw_value_); - return hardware_interface::return_type::OK; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - double voltage_level_hw_value_ = 0x666; - - // Helper variables to initiate error on read - int read_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -} // namespace test_components - -TEST(TestComponentInterfaces, dummy_sensor_default_interface_export) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); + hardware_interface::System system_hw(std::make_unique()); const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + ros2_control_test_assets::urdf_tail; const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); - const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - auto state = sensor_hw.initialize(voltage_sensor_res); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = sensor_hw.on_export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); - EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); - - // Not updated because is is UNCONFIGURED - sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); - - // Updated because is is INACTIVE - state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0]->get_value()); - - // It can read now - sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); -} - -TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); - - const std::string urdf_to_test = - std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + - ros2_control_test_assets::urdf_tail; - const std::vector control_resources = - hardware_interface::parse_control_resources_from_urdf(urdf_to_test); - const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - auto state = sensor_hw.initialize(voltage_sensor_res); - - auto state_interfaces = sensor_hw.on_export_state_interfaces(); - // Updated because is is INACTIVE - state = sensor_hw.configure(); - state = sensor_hw.activate(); + auto state_interfaces = system_hw.on_export_state_interfaces(); + auto command_interfaces = system_hw.on_export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - // Initiate recoverable error - call read 99 times OK and on 100-time will return error + // Initiate error on write (this is first time therefore recoverable) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } - ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = sensor_hw.get_state(); + state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values - state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - - state = sensor_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error - for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - - state = sensor_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} - -namespace test_components -{ - -class DummyActuatorDefault : public hardware_interface::ActuatorInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override - { - // We hardcode the info - if ( - hardware_interface::ActuatorInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - set_state("joint1/position", 0.0); - set_state("joint1/velocity", 0.0); - - if (recoverable_error_happened_) - { - set_command("joint1/velocity", 0.0); - } - - read_calls_ = 0; - write_calls_ = 0; - - return CallbackReturn::SUCCESS; - } - - std::string get_name() const override { return "DummyActuatorDefault"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, state is getting propagated within write. - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++write_calls_; - if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - auto position_state = get_state("joint1/position"); - set_state("joint1/position", position_state + get_command("joint1/velocity")); - set_state("joint1/velocity", get_command("joint1/velocity")); - - return hardware_interface::return_type::OK; - } - - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override - { - set_state("joint1/velocity", 0.0); - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - // Helper variables to initiate error on read - unsigned int read_calls_ = 0; - unsigned int write_calls_ = 0; - bool recoverable_error_happened_ = false; -}; - -} // namespace test_components - -TEST(TestComponentInterfaces, dummy_actuator_default) -{ - hardware_interface::Actuator actuator_hw( - std::make_unique()); - const std::string urdf_to_test = - std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + - ros2_control_test_assets::urdf_tail; - const std::vector control_resources = - hardware_interface::parse_control_resources_from_urdf(urdf_to_test); - const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); - - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = actuator_hw.on_export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - - auto command_interfaces = actuator_hw.on_export_command_interfaces(); - ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - - double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } - - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - - // Read and Write are working because it is INACTIVE - for (auto step = 0u; step < 10; ++step) + for (auto index = 0ul; index < 3; ++index) { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } - - state = actuator_hw.activate(); + state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - // Read and Write are working because it is ACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = actuator_hw.shutdown(); + state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - } - - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index f1bd1e87c9..2a4625b277 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -604,6 +604,41 @@ const auto valid_urdf_ros2_control_dummy_actuator_only = )"; +const auto valid_urdf_ros2_control_dummy_system_robot = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + +)"; + const auto valid_urdf_ros2_control_parameter_empty = R"( From 5f0bf4315e37296d9f6c25c21a8d3c25858d1d97 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 23 Jan 2024 11:09:26 +0100 Subject: [PATCH 15/34] code review suggestions and: * components check for export of interfaces * change intefaces to allow custom export and test --- hardware_interface/CMakeLists.txt | 4 + .../include/hardware_interface/actuator.hpp | 16 +- .../hardware_interface/actuator_interface.hpp | 68 +++-- .../hardware_interface/component_parser.hpp | 3 - .../include/hardware_interface/handle.hpp | 7 +- .../hardware_interface/hardware_info.hpp | 8 +- .../include/hardware_interface/sensor.hpp | 8 +- .../hardware_interface/sensor_interface.hpp | 34 ++- .../include/hardware_interface/system.hpp | 16 +- .../hardware_interface/system_interface.hpp | 82 ++--- hardware_interface/src/actuator.cpp | 59 +++- hardware_interface/src/resource_manager.cpp | 96 ++---- hardware_interface/src/sensor.cpp | 31 +- hardware_interface/src/system.cpp | 61 +++- .../test/test_component_interfaces.cpp | 224 +++++++------- ...est_component_interfaces_custom_export.cpp | 279 ++++++++++++++++++ .../test/test_component_parser.cpp | 28 +- .../components_urdfs.hpp | 4 +- 18 files changed, 674 insertions(+), 354 deletions(-) create mode 100644 hardware_interface/test/test_component_interfaces_custom_export.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 9cc766ebb0..385ae96fb1 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -80,6 +80,10 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces hardware_interface) ament_target_dependencies(test_component_interfaces ros2_control_test_assets) + ament_add_gmock(test_component_interfaces_custom_export test/test_component_interfaces_custom_export.cpp) + target_link_libraries(test_component_interfaces_custom_export hardware_interface) + ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface) ament_target_dependencies(test_component_parser ros2_control_test_assets) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index e873fb37f7..e119faadbf 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -69,23 +69,11 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); - [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC - std::vector - export_state_interfaces(); - HARDWARE_INTERFACE_PUBLIC - std::vector> on_export_state_interfaces(); - - [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC - std::vector - export_command_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector> on_export_command_interfaces(); + std::vector export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 91674d4f17..aee7d8e449 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -158,20 +159,18 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /// Exports all state interfaces for this hardware interface. /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * If overwritten: - * The state interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -182,33 +181,41 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return {}; } - std::vector> on_export_state_interfaces() + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the actuator_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() { - std::vector> state_interfaces; + std::vector state_interfaces; state_interfaces.reserve(joint_state_interfaces_.size()); for (const auto & [name, descr] : joint_state_interfaces_) { - actuator_states_.insert(std::make_pair(name, std::make_shared(descr))); - state_interfaces.push_back(actuator_states_.at(name)); + auto state_interface = std::make_shared(descr); + actuator_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); } return state_interfaces; } + /// Exports all command interfaces for this hardware interface. /** - * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * The command interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. * * Note the ownership over the state interfaces is transferred to the caller. * - * \return vector of command interfaces + * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_command_interfaces() method. " + "Replaced by vector on_export_command_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -219,15 +226,23 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return {}; } - std::vector> on_export_command_interfaces() + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the actuator_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + virtual std::vector on_export_command_interfaces() { - std::vector> command_interfaces; + std::vector command_interfaces; command_interfaces.reserve(joint_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) { - actuator_commands_.insert(std::make_pair(name, std::make_shared(descr))); - command_interfaces.push_back(actuator_commands_.at(name)); + auto command_interface = std::make_shared(descr); + actuator_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); } return command_interfaces; @@ -355,9 +370,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::map joint_state_interfaces_; std::map joint_command_interfaces_; -private: - std::map> actuator_states_; - std::map> actuator_commands_; + std::unordered_map actuator_states_; + std::unordered_map actuator_commands_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 838363f011..df4549f700 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -77,8 +77,5 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_gpio_command_interface_descriptions_from_hardware_info( const HardwareInfo & hw_info); -HARDWARE_INTERFACE_PUBLIC -bool parse_bool(const std::string & bool_string); - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index a91e51f973..7d51505777 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include #include #include @@ -43,7 +44,7 @@ class Handle } explicit Handle(const InterfaceDescription & interface_description) - : prefix_name_(interface_description.prefix_name), + : prefix_name_(interface_description.prefix_name_), interface_name_(interface_description.interface_info.name) { // As soon as multiple datatypes are used in HANDLE_DATATYPE @@ -135,6 +136,8 @@ class StateInterface : public Handle using Handle::Handle; }; +using StateInterfaceSharedPtr = std::shared_ptr; + class CommandInterface : public Handle { public: @@ -155,6 +158,8 @@ class CommandInterface : public Handle using Handle::Handle; }; +using CommandInterfaceSharedPtr = std::shared_ptr; + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 8e71282e21..7155f43cfc 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -132,15 +132,15 @@ struct TransmissionInfo */ struct InterfaceDescription { - InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) - : prefix_name(prefix_name_in), interface_info(interface_info_in) + InterfaceDescription(const std::string & prefix_name, const InterfaceInfo & interface_info_in) + : prefix_name_(prefix_name), interface_info(interface_info_in) { } /** * Name of the interface defined by the user. */ - std::string prefix_name; + std::string prefix_name_; /** * What type of component is exported: joint, sensor or gpio @@ -152,7 +152,7 @@ struct InterfaceDescription */ InterfaceInfo interface_info; - std::string get_name() const { return prefix_name + "/" + interface_info.name; } + std::string get_name() const { return prefix_name_ + "/" + interface_info.name; } std::string get_interface_type() const { return interface_info.name; } }; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 305816fc37..baab843552 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -69,14 +69,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); - [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC - std::vector - export_state_interfaces(); - HARDWARE_INTERFACE_PUBLIC - std::vector> on_export_state_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 3a1ff2c95e..d09c2dadf5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -143,20 +144,18 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /// Exports all state interfaces for this hardware interface. /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * If overwritten: - * The state interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -166,17 +165,25 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return {}; } - std::vector> on_export_state_interfaces() + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the sensor_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() { - std::vector> state_interfaces; + std::vector state_interfaces; state_interfaces.reserve(sensor_state_interfaces_.size()); for (const auto & [name, descr] : sensor_state_interfaces_) { // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is // inserted - sensor_states_.insert(std::make_pair(name, std::make_shared(descr))); - state_interfaces.push_back(sensor_states_.at(name)); + auto state_interface = std::make_shared(descr); + sensor_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); } return state_interfaces; @@ -245,8 +252,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::map sensor_state_interfaces_; -private: - std::map> sensor_states_; + std::unordered_map sensor_states_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 6c07f7e1a0..1e7ad91706 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -69,23 +69,11 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & error(); - [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC - std::vector - export_state_interfaces(); - HARDWARE_INTERFACE_PUBLIC - std::vector> on_export_state_interfaces(); - - [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] HARDWARE_INTERFACE_PUBLIC - std::vector - export_command_interfaces(); + std::vector export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector> on_export_command_interfaces(); + std::vector export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 91cc52499b..3ae1405a3d 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -178,20 +179,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /// Exports all state interfaces for this hardware interface. /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * If overwritten: - * The state interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. * * Note the ownership over the state interfaces is transferred to the caller. * * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -201,46 +200,55 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return {}; } - std::vector> on_export_state_interfaces() + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() { - std::vector> state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( joint_state_interfaces_.size() + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); for (const auto & [name, descr] : joint_state_interfaces_) { - system_states_.insert(std::make_pair(name, std::make_shared(descr))); - state_interfaces.push_back(system_states_.at(name)); + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : sensor_state_interfaces_) { - system_states_.insert(std::make_pair(name, std::make_shared(descr))); - state_interfaces.push_back(system_states_.at(name)); + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : gpio_state_interfaces_) { - system_states_.insert(std::make_pair(name, std::make_shared(descr))); - state_interfaces.push_back(system_states_.at(name)); + auto state_interface = std::make_shared(descr); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); } return state_interfaces; } /// Exports all command interfaces for this hardware interface. /** - * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * The command interfaces have to be created and transferred according - * to the hardware info passed in for the configuration. + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. * * Note the ownership over the state interfaces is transferred to the caller. * - * \return vector of command interfaces + * \return vector of state interfaces */ [[deprecated( - "Replaced by vector> on_export_command_interfaces() method. " + "Replaced by vector on_export_command_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -251,21 +259,30 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return {}; } - std::vector> on_export_command_interfaces() + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + virtual std::vector on_export_command_interfaces() { - std::vector> command_interfaces; + std::vector command_interfaces; command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) { - system_commands_.insert(std::make_pair(name, std::make_shared(descr))); - command_interfaces.push_back(system_commands_.at(name)); + auto command_interface = std::make_shared(descr); + system_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); } for (const auto & [name, descr] : gpio_command_interfaces_) { - system_commands_.insert(std::make_pair(name, std::make_shared(descr))); - command_interfaces.push_back(system_commands_.at(name)); + auto command_interface = std::make_shared(descr); + system_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); } return command_interfaces; } @@ -398,9 +415,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::map gpio_state_interfaces_; std::map gpio_command_interfaces_; -private: - std::map> system_states_; - std::map> system_commands_; + std::unordered_map system_states_; + std::unordered_map system_commands_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 785563af95..d2bcb5fb21 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -187,26 +187,61 @@ const rclcpp_lifecycle::State & Actuator::error() return impl_->get_state(); } -std::vector Actuator::export_state_interfaces() +std::vector> Actuator::export_state_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here - return impl_->export_state_interfaces(); -} + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility -std::vector> Actuator::on_export_state_interfaces() -{ - return impl_->on_export_state_interfaces(); + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility } -std::vector Actuator::export_command_interfaces() +std::vector> Actuator::export_command_interfaces() { // TODO(karsten1987): Might be worth to do some brief sanity check here - return impl_->export_command_interfaces(); -} + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_command_interfaces(); + // END: for backward compatibility -std::vector> Actuator::on_export_command_interfaces() -{ - return impl_->on_export_command_interfaces(); + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_command_interfaces(); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(std::move(interface))); + } + return interface_ptrs; + // END: for backward compatibility } return_type Actuator::prepare_command_mode_switch( diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2563c32f3a..fbd8ca452a 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -603,73 +603,28 @@ class ResourceStorage return result; } - void insert_state_interface(std::shared_ptr state_interface) - { - const auto [it, success] = - state_interface_map_.insert(std::make_pair(state_interface->get_name(), state_interface)); - if (!success) - { - std::string msg( - "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + - state_interface->get_name() + "]"); - throw std::runtime_error(msg); - } - } - - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_state_interfaces() method is removed - void insert_state_interface(const StateInterface & state_interface) - { - const auto [it, success] = state_interface_map_.emplace(std::make_pair( - state_interface.get_name(), std::make_shared(state_interface))); - if (!success) - { - std::string msg( - "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + - state_interface.get_name() + "]"); - throw std::runtime_error(msg); - } - } - // END: for backward compatibility - template void import_state_interfaces(HardwareT & hardware) { try { std::vector interface_names; - std::vector interfaces = hardware.export_state_interfaces(); - // If no StateInterfaces has been exported, this could mean: - // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well - // b) default implementation for export_state_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - std::vector> interface_ptrs = - hardware.on_export_state_interfaces(); - interface_names.reserve(interface_ptrs.size()); - for (auto const & interface : interface_ptrs) - { - insert_state_interface(interface); - interface_names.push_back(interface->get_name()); - } - } - // TODO(Manuel) BEGIN: for backward compatibility, can be removed if export_state_interfaces() - // method is removed - else + std::vector> interfaces = hardware.export_state_interfaces(); + + interface_names.reserve(interfaces.size()); + for (auto const & interface : interfaces) { - interface_names.reserve(interfaces.size()); - for (auto const & interface : interfaces) + const auto [it, success] = + state_interface_map_.insert(std::make_pair(interface->get_name(), interface)); + if (!success) { - insert_state_interface(interface); - interface_names.push_back(interface.get_name()); + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + interface->get_name() + "]"); + throw std::runtime_error(msg); } + interface_names.push_back(interface->get_name()); } - // TODO(Manuel) END: for backward compatibility - - hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; - available_state_interfaces_.reserve( - available_state_interfaces_.capacity() + interface_names.size()); } catch (const std::exception & e) { @@ -722,25 +677,11 @@ class ResourceStorage { try { - auto interfaces = hardware.export_command_interfaces(); - // If no CommandInterface has been exported, this could mean: - // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well - // b) default implementation for export_command_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - std::vector> interface_ptrs = - hardware.on_export_command_interfaces(); - hardware_info_map_[hardware.get_name()].command_interfaces = - add_command_interfaces(interface_ptrs); - } - // TODO(Manuel) BEGIN: for backward compatibility, can be removed if - // export_command_interfaces() method is removed - else - { - hardware_info_map_[hardware.get_name()].command_interfaces = - add_command_interfaces(interfaces); - } + std::vector> interfaces = + hardware.export_command_interfaces(); + + hardware_info_map_[hardware.get_name()].command_interfaces = + add_command_interfaces(interfaces); // TODO(Manuel) END: for backward compatibility } catch (const std::exception & ex) @@ -811,8 +752,6 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed std::vector add_command_interfaces(std::vector & interfaces) { std::vector interface_names; @@ -829,7 +768,6 @@ class ResourceStorage return interface_names; } - // END: for backward compatibility std::vector add_command_interfaces( std::vector> & interfaces) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9d69e16661..cd539341d6 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -186,14 +186,33 @@ const rclcpp_lifecycle::State & Sensor::error() return impl_->get_state(); } -std::vector Sensor::export_state_interfaces() +std::vector> Sensor::export_state_interfaces() { - return impl_->export_state_interfaces(); -} + // TODO(karsten1987): Might be worth to do some brief sanity check here + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } -std::vector> Sensor::on_export_state_interfaces() -{ - return impl_->on_export_state_interfaces(); + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility } std::string Sensor::get_name() const { return impl_->get_name(); } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 920c0edf6a..70338f35a9 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -186,24 +186,61 @@ const rclcpp_lifecycle::State & System::error() return impl_->get_state(); } -std::vector System::export_state_interfaces() +std::vector> System::export_state_interfaces() { - return impl_->export_state_interfaces(); -} + // TODO(karsten1987): Might be worth to do some brief sanity check here + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_state_interfaces(); + // END: for backward compatibility -std::vector> System::on_export_state_interfaces() -{ - return impl_->on_export_state_interfaces(); -} + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } -std::vector System::export_command_interfaces() -{ - return impl_->export_command_interfaces(); + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility } -std::vector> System::on_export_command_interfaces() +std::vector> System::export_command_interfaces() { - return impl_->on_export_command_interfaces(); + // TODO(karsten1987): Might be worth to do some brief sanity check here + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interfaces = impl_->export_command_interfaces(); + // END: for backward compatibility + + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_command_interfaces(); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector> interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(std::move(interface))); + } + return interface_ptrs; + // END: for backward compatibility } return_type System::prepare_command_mode_switch( diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index b5b308e624..9dc80652a1 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -689,21 +689,21 @@ TEST(TestComponentInterfaces, dummy_actuator) auto state_interfaces = actuator_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); double velocity_value = 1.0; - command_interfaces[0].set_value(velocity_value); // velocity + command_interfaces[0]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -711,8 +711,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -726,8 +726,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -741,8 +741,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -756,8 +756,8 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(0, state_interfaces[1].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -785,7 +785,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = actuator_hw.on_export_state_interfaces(); + auto state_interfaces = actuator_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); @@ -794,7 +794,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - auto command_interfaces = actuator_hw.on_export_command_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); @@ -802,17 +802,17 @@ TEST(TestComponentInterfaces, dummy_actuator_default) double velocity_value = 1.0; command_interfaces[0]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } state = actuator_hw.configure(); @@ -852,12 +852,12 @@ TEST(TestComponentInterfaces, dummy_actuator_default) // Noting should change because it is FINALIZED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } EXPECT_EQ( @@ -879,24 +879,24 @@ TEST(TestComponentInterfaces, dummy_sensor) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()); - EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0].get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0].get_value()); + EXPECT_EQ(0.0, state_interfaces[0]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0].get_value()); + EXPECT_EQ(0x666, state_interfaces[0]->get_value()); } // END @@ -915,7 +915,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = sensor_hw.on_export_state_interfaces(); + auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); @@ -950,41 +950,41 @@ TEST(TestComponentInterfaces, dummy_system) auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(6u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()); - EXPECT_EQ("joint2/position", state_interfaces[2].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2].get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2].get_prefix_name()); - EXPECT_EQ("joint2/velocity", state_interfaces[3].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3].get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3].get_prefix_name()); - EXPECT_EQ("joint3/position", state_interfaces[4].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4].get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4].get_prefix_name()); - EXPECT_EQ("joint3/velocity", state_interfaces[5].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5].get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5].get_prefix_name()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); + EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); auto command_interfaces = system_hw.export_command_interfaces(); ASSERT_EQ(3u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()); - EXPECT_EQ("joint2/velocity", command_interfaces[1].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1].get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1].get_prefix_name()); - EXPECT_EQ("joint3/velocity", command_interfaces[2].get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2].get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2].get_prefix_name()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); double velocity_value = 1.0; - command_interfaces[0].set_value(velocity_value); // velocity - command_interfaces[1].set_value(velocity_value); // velocity - command_interfaces[2].set_value(velocity_value); // velocity + command_interfaces[0]->set_value(velocity_value); // velocity + command_interfaces[1]->set_value(velocity_value); // velocity + command_interfaces[2]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED @@ -992,12 +992,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1].get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3].get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4].get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5].get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1011,12 +1011,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1].get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3].get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4].get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5].get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1030,12 +1030,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1].get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3].get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4].get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5].get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1049,12 +1049,12 @@ TEST(TestComponentInterfaces, dummy_system) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1].get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3].get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4].get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5].get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1079,7 +1079,7 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = system_hw.on_export_state_interfaces(); + auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(6u, state_interfaces.size()); EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); @@ -1100,7 +1100,7 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); - auto command_interfaces = system_hw.on_export_command_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); ASSERT_EQ(3u, command_interfaces.size()); EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); @@ -1116,12 +1116,12 @@ TEST(TestComponentInterfaces, dummy_system_default) command_interfaces[0]->set_value(velocity_value); // velocity command_interfaces[1]->set_value(velocity_value); // velocity command_interfaces[2]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity @@ -1130,7 +1130,7 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } state = system_hw.configure(); @@ -1178,7 +1178,7 @@ TEST(TestComponentInterfaces, dummy_system_default) // Noting should change because it is FINALIZED for (auto step = 0u; step < 10; ++step) { - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity @@ -1187,7 +1187,7 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); @@ -1260,8 +1260,8 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); - EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1305,8 +1305,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = actuator_hw.on_export_state_interfaces(); - auto command_interfaces = actuator_hw.on_export_command_interfaces(); + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); state = actuator_hw.configure(); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1389,8 +1389,8 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); - EXPECT_EQ(command_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1433,8 +1433,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = actuator_hw.on_export_state_interfaces(); - auto command_interfaces = actuator_hw.on_export_command_interfaces(); + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); state = actuator_hw.configure(); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1520,7 +1520,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0].get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1563,7 +1563,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; auto state = sensor_hw.initialize(voltage_sensor_res); - auto state_interfaces = sensor_hw.on_export_state_interfaces(); + auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE state = sensor_hw.configure(); state = sensor_hw.activate(); @@ -1644,11 +1644,11 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index].get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index].get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1690,8 +1690,8 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = system_hw.on_export_state_interfaces(); - auto command_interfaces = system_hw.on_export_command_interfaces(); + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); state = system_hw.configure(); state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1781,11 +1781,11 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index].get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index].get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1827,8 +1827,8 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - auto state_interfaces = system_hw.on_export_state_interfaces(); - auto command_interfaces = system_hw.on_export_command_interfaces(); + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); state = system_hw.configure(); state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp new file mode 100644 index 0000000000..e438983686 --- /dev/null +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -0,0 +1,279 @@ +// Copyright 2020 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +// Values to send over command interface to trigger error in write and read methods + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +} // namespace + +using namespace ::testing; // NOLINT + +namespace test_components +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + std::string get_name() const override { return "DummyActuatorDefault"; } + + std::vector on_export_state_interfaces() override + { + auto interfaces = hardware_interface::ActuatorInterface::on_export_state_interfaces(); + auto unlisted_state_interface = std::make_shared( + info_.joints[0].name, "some_unlisted_interface", nullptr); + actuator_states_.insert( + std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + std::vector on_export_command_interfaces() override + { + auto interfaces = hardware_interface::ActuatorInterface::on_export_command_interfaces(); + auto unlisted_state_interface = std::make_shared( + info_.joints[0].name, "some_unlisted_interface", nullptr); + actuator_commands_.insert( + std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + std::string get_name() const override { return "DummySensorDefault"; } + + std::vector on_export_state_interfaces() override + { + auto interfaces = hardware_interface::SensorInterface::on_export_state_interfaces(); + auto unlisted_state_interface = std::make_shared( + info_.sensors[0].name, "some_unlisted_interface", nullptr); + sensor_states_.insert( + std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +class DummySystemDefault : public hardware_interface::SystemInterface +{ + std::string get_name() const override { return "DummySystemDefault"; } + + std::vector on_export_state_interfaces() override + { + auto interfaces = hardware_interface::SystemInterface::on_export_state_interfaces(); + auto unlisted_state_interface = std::make_shared( + info_.joints[0].name, "some_unlisted_interface", nullptr); + system_states_.insert( + std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + std::vector on_export_command_interfaces() override + { + auto interfaces = hardware_interface::SystemInterface::on_export_command_interfaces(); + auto unlisted_state_interface = std::make_shared( + info_.joints[0].name, "some_unlisted_interface", nullptr); + system_commands_.insert( + std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + interfaces.push_back(unlisted_state_interface); + + return interfaces; + } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return hardware_interface::return_type::OK; + } +}; + +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(3u, state_interfaces.size()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[2]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[2]->get_prefix_name()); + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(2u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[1]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[1]->get_prefix_name()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ(2u, state_interfaces.size()); + EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); + EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[1]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); +} + +TEST(TestComponentInterfaces, dummy_system_default_custom_export) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(7u, state_interfaces.size()); + EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); + EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[6]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[6]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[6]->get_prefix_name()); + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(4u, command_interfaces.size()); + EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); + EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); + EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[3]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[3]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[3]->get_prefix_name()); +} diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index aa6a56a1f5..f3c1d14c11 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1415,11 +1415,11 @@ TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardwa const auto joint_state_descriptions = parse_joint_state_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_state_descriptions[0].prefix_name_, "joint1"); EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position"); EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); - EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_state_descriptions[1].prefix_name_, "joint2"); EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "position"); EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); } @@ -1434,13 +1434,13 @@ TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hard const auto joint_command_descriptions = parse_joint_command_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1"); + EXPECT_EQ(joint_command_descriptions[0].prefix_name_, "joint1"); EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position"); EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); - EXPECT_EQ(joint_command_descriptions[1].prefix_name, "joint2"); + EXPECT_EQ(joint_command_descriptions[1].prefix_name_, "joint2"); EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "position"); EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); @@ -1456,17 +1456,17 @@ TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardw const auto sensor_state_descriptions = parse_sensor_state_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].prefix_name_, "sensor1"); EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll"); EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); - EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].prefix_name_, "sensor1"); EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch"); EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); - EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].prefix_name_, "sensor1"); EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw"); EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); - EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].prefix_name_, "sensor2"); EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "image"); EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); } @@ -1481,17 +1481,17 @@ TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardwar const auto gpio_state_descriptions = parse_gpio_state_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name_, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); - EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].prefix_name_, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); - EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].prefix_name_, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2"); EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); - EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].prefix_name_, "flange_vacuum"); EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); } @@ -1506,11 +1506,11 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw const auto gpio_state_descriptions = parse_gpio_command_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name_, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); - EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].prefix_name_, "flange_vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 2a4625b277..f78eb6e75c 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -567,9 +567,9 @@ const auto valid_urdf_ros2_control_system_robot_with_all_interfaces = // Voltage Sensor only const auto valid_urdf_ros2_control_voltage_sensor_only = R"( - + - ros2_control_demo_hardware/CameraWithIMUSensor + ros2_control_demo_hardware/SingleJointVoltageSensor 2 From ac376f92f08133029d53029ca91e92d5bb944d0c Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 09:56:07 +0100 Subject: [PATCH 16/34] undo prefix_ -> prefix (HWInfo) and Command-/StateIntefaceSharedPtr --- .../include/hardware_interface/actuator.hpp | 4 +-- .../hardware_interface/actuator_interface.hpp | 16 +++++------ .../include/hardware_interface/handle.hpp | 6 +--- .../hardware_interface/hardware_info.hpp | 8 +++--- .../include/hardware_interface/sensor.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 8 +++--- .../include/hardware_interface/system.hpp | 4 +-- .../hardware_interface/system_interface.hpp | 16 +++++------ ...est_component_interfaces_custom_export.cpp | 15 ++++++---- .../test/test_component_parser.cpp | 28 +++++++++---------- 10 files changed, 54 insertions(+), 53 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index e119faadbf..8e38cfb044 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -70,10 +70,10 @@ class Actuator final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index aee7d8e449..7f99409cd0 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -170,7 +170,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector> on_export_state_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -188,9 +188,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector on_export_state_interfaces() + virtual std::vector> on_export_state_interfaces() { - std::vector state_interfaces; + std::vector> state_interfaces; state_interfaces.reserve(joint_state_interfaces_.size()); for (const auto & [name, descr] : joint_state_interfaces_) @@ -215,7 +215,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_command_interfaces() method. " + "Replaced by vector> on_export_command_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -233,9 +233,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of shared pointers to the created and stored CommandInterfaces */ - virtual std::vector on_export_command_interfaces() + virtual std::vector> on_export_command_interfaces() { - std::vector command_interfaces; + std::vector> command_interfaces; command_interfaces.reserve(joint_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) @@ -370,8 +370,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::map joint_state_interfaces_; std::map joint_command_interfaces_; - std::unordered_map actuator_states_; - std::unordered_map actuator_commands_; + std::unordered_map> actuator_states_; + std::unordered_map> actuator_commands_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 7d51505777..72b1d6cf92 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -44,7 +44,7 @@ class Handle } explicit Handle(const InterfaceDescription & interface_description) - : prefix_name_(interface_description.prefix_name_), + : prefix_name_(interface_description.prefix_name), interface_name_(interface_description.interface_info.name) { // As soon as multiple datatypes are used in HANDLE_DATATYPE @@ -136,8 +136,6 @@ class StateInterface : public Handle using Handle::Handle; }; -using StateInterfaceSharedPtr = std::shared_ptr; - class CommandInterface : public Handle { public: @@ -158,8 +156,6 @@ class CommandInterface : public Handle using Handle::Handle; }; -using CommandInterfaceSharedPtr = std::shared_ptr; - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 7155f43cfc..8e71282e21 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -132,15 +132,15 @@ struct TransmissionInfo */ struct InterfaceDescription { - InterfaceDescription(const std::string & prefix_name, const InterfaceInfo & interface_info_in) - : prefix_name_(prefix_name), interface_info(interface_info_in) + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), interface_info(interface_info_in) { } /** * Name of the interface defined by the user. */ - std::string prefix_name_; + std::string prefix_name; /** * What type of component is exported: joint, sensor or gpio @@ -152,7 +152,7 @@ struct InterfaceDescription */ InterfaceInfo interface_info; - std::string get_name() const { return prefix_name_ + "/" + interface_info.name; } + std::string get_name() const { return prefix_name + "/" + interface_info.name; } std::string get_interface_type() const { return interface_info.name; } }; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index baab843552..3240709cc3 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -70,7 +70,7 @@ class Sensor final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC std::string get_name() const; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index d09c2dadf5..04b6849a52 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -155,7 +155,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector> on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -172,9 +172,9 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector on_export_state_interfaces() + virtual std::vector> on_export_state_interfaces() { - std::vector state_interfaces; + std::vector> state_interfaces; state_interfaces.reserve(sensor_state_interfaces_.size()); for (const auto & [name, descr] : sensor_state_interfaces_) @@ -252,7 +252,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::map sensor_state_interfaces_; - std::unordered_map sensor_states_; + std::unordered_map> sensor_states_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 1e7ad91706..835cfa18f2 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -70,10 +70,10 @@ class System final const rclcpp_lifecycle::State & error(); HARDWARE_INTERFACE_PUBLIC - std::vector export_state_interfaces(); + std::vector> export_state_interfaces(); HARDWARE_INTERFACE_PUBLIC - std::vector export_command_interfaces(); + std::vector> export_command_interfaces(); HARDWARE_INTERFACE_PUBLIC return_type prepare_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 3ae1405a3d..08c8c75ef3 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -190,7 +190,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector> on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -207,9 +207,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector on_export_state_interfaces() + virtual std::vector> on_export_state_interfaces() { - std::vector state_interfaces; + std::vector> state_interfaces; state_interfaces.reserve( joint_state_interfaces_.size() + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); @@ -248,7 +248,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_command_interfaces() method. " + "Replaced by vector> on_export_command_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -266,9 +266,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored CommandInterfaces */ - virtual std::vector on_export_command_interfaces() + virtual std::vector> on_export_command_interfaces() { - std::vector command_interfaces; + std::vector> command_interfaces; command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) @@ -415,8 +415,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::map gpio_state_interfaces_; std::map gpio_command_interfaces_; - std::unordered_map system_states_; - std::unordered_map system_commands_; + std::unordered_map> system_states_; + std::unordered_map> system_commands_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index e438983686..07fc3204e3 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -56,7 +56,8 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { std::string get_name() const override { return "DummyActuatorDefault"; } - std::vector on_export_state_interfaces() override + std::vector> on_export_state_interfaces() + override { auto interfaces = hardware_interface::ActuatorInterface::on_export_state_interfaces(); auto unlisted_state_interface = std::make_shared( @@ -68,7 +69,8 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface return interfaces; } - std::vector on_export_command_interfaces() override + std::vector> on_export_command_interfaces() + override { auto interfaces = hardware_interface::ActuatorInterface::on_export_command_interfaces(); auto unlisted_state_interface = std::make_shared( @@ -97,7 +99,8 @@ class DummySensorDefault : public hardware_interface::SensorInterface { std::string get_name() const override { return "DummySensorDefault"; } - std::vector on_export_state_interfaces() override + std::vector> on_export_state_interfaces() + override { auto interfaces = hardware_interface::SensorInterface::on_export_state_interfaces(); auto unlisted_state_interface = std::make_shared( @@ -120,7 +123,8 @@ class DummySystemDefault : public hardware_interface::SystemInterface { std::string get_name() const override { return "DummySystemDefault"; } - std::vector on_export_state_interfaces() override + std::vector> on_export_state_interfaces() + override { auto interfaces = hardware_interface::SystemInterface::on_export_state_interfaces(); auto unlisted_state_interface = std::make_shared( @@ -132,7 +136,8 @@ class DummySystemDefault : public hardware_interface::SystemInterface return interfaces; } - std::vector on_export_command_interfaces() override + std::vector> on_export_command_interfaces() + override { auto interfaces = hardware_interface::SystemInterface::on_export_command_interfaces(); auto unlisted_state_interface = std::make_shared( diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index f3c1d14c11..aa6a56a1f5 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1415,11 +1415,11 @@ TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardwa const auto joint_state_descriptions = parse_joint_state_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(joint_state_descriptions[0].prefix_name_, "joint1"); + EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1"); EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position"); EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); - EXPECT_EQ(joint_state_descriptions[1].prefix_name_, "joint2"); + EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2"); EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "position"); EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); } @@ -1434,13 +1434,13 @@ TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hard const auto joint_command_descriptions = parse_joint_command_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(joint_command_descriptions[0].prefix_name_, "joint1"); + EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1"); EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position"); EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); - EXPECT_EQ(joint_command_descriptions[1].prefix_name_, "joint2"); + EXPECT_EQ(joint_command_descriptions[1].prefix_name, "joint2"); EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "position"); EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); @@ -1456,17 +1456,17 @@ TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardw const auto sensor_state_descriptions = parse_sensor_state_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(sensor_state_descriptions[0].prefix_name_, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1"); EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll"); EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); - EXPECT_EQ(sensor_state_descriptions[1].prefix_name_, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1"); EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch"); EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); - EXPECT_EQ(sensor_state_descriptions[2].prefix_name_, "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1"); EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw"); EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); - EXPECT_EQ(sensor_state_descriptions[3].prefix_name_, "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2"); EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "image"); EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); } @@ -1481,17 +1481,17 @@ TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardwar const auto gpio_state_descriptions = parse_gpio_state_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(gpio_state_descriptions[0].prefix_name_, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); - EXPECT_EQ(gpio_state_descriptions[1].prefix_name_, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); - EXPECT_EQ(gpio_state_descriptions[2].prefix_name_, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2"); EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); - EXPECT_EQ(gpio_state_descriptions[3].prefix_name_, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum"); EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); } @@ -1506,11 +1506,11 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw const auto gpio_state_descriptions = parse_gpio_command_interface_descriptions_from_hardware_info(control_hardware[0]); - EXPECT_EQ(gpio_state_descriptions[0].prefix_name_, "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); - EXPECT_EQ(gpio_state_descriptions[1].prefix_name_, "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); } From 44af40dbec567bb19508d5c22dfdf5174bfc777f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 10:17:11 +0100 Subject: [PATCH 17/34] merge parser functions --- .../hardware_interface/actuator_interface.hpp | 4 +- .../hardware_interface/component_parser.hpp | 43 +++-------- .../hardware_interface/sensor_interface.hpp | 2 +- .../hardware_interface/system_interface.hpp | 10 +-- hardware_interface/src/component_parser.cpp | 72 +++++-------------- .../test/test_component_parser.cpp | 10 +-- 6 files changed, 39 insertions(+), 102 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 7f99409cd0..75a9eb3d2e 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -136,7 +136,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_state_interface_descriptions = - parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); + parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); for (const auto & description : joint_state_interface_descriptions) { joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); @@ -150,7 +150,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_command_interface_descriptions = - parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); + parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); for (const auto & description : joint_command_interface_descriptions) { joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index df4549f700..b9ca460800 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,49 +33,22 @@ HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); /** - * \param[in] hw_info the hardware description - * \return vector filled with information about robot's SommandInterfaces for the joints + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's StateInterfaces for the component * which are exported */ HARDWARE_INTERFACE_PUBLIC -std::vector parse_joint_state_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info); +std::vector parse_state_interface_descriptions_from_hardware_info( + const std::vector & component_info); /** - * \param[in] hw_info the hardware description - * \return vector filled with information about robot's SommandInterfaces for the sensors + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's CommandInterfaces for the component * which are exported */ HARDWARE_INTERFACE_PUBLIC -std::vector parse_sensor_state_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info); - -/** - * \param[in] hw_info the hardware description - * \return vector filled with information about robot's SommandInterfaces for the gpios - * which are exported - */ -HARDWARE_INTERFACE_PUBLIC -std::vector parse_gpio_state_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info); - -/** - * \param[in] hw_info the hardware description - * \return vector filled with information about robot's CommandInterfaces for the joints - * which are exported - */ -HARDWARE_INTERFACE_PUBLIC -std::vector parse_joint_command_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info); - -/** - * \param[in] hw_info the hardware description - * \return vector filled with information about robot's CommandInterfaces for the gpios - * which are exported - */ -HARDWARE_INTERFACE_PUBLIC -std::vector parse_gpio_command_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info); +std::vector parse_command_interface_descriptions_from_hardware_info( + const std::vector & component_info); } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 04b6849a52..96a4ce53b6 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -135,7 +135,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto sensor_state_interface_descriptions = - parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); + parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); for (const auto & description : sensor_state_interface_descriptions) { sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 08c8c75ef3..4be31b9f43 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -138,19 +138,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_state_interface_descriptions = - parse_joint_state_interface_descriptions_from_hardware_info(hardware_info); + parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); for (const auto & description : joint_state_interface_descriptions) { joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto sensor_state_interface_descriptions = - parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info); + parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); for (const auto & description : sensor_state_interface_descriptions) { sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto gpio_state_interface_descriptions = - parse_gpio_state_interface_descriptions_from_hardware_info(hardware_info); + parse_state_interface_descriptions_from_hardware_info(hardware_info.gpios); for (const auto & description : gpio_state_interface_descriptions) { gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); @@ -164,13 +164,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void import_command_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_command_interface_descriptions = - parse_joint_command_interface_descriptions_from_hardware_info(hardware_info); + parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); for (const auto & description : joint_command_interface_descriptions) { joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto gpio_command_interface_descriptions = - parse_gpio_command_interface_descriptions_from_hardware_info(hardware_info); + parse_command_interface_descriptions_from_hardware_info(hardware_info.gpios); for (const auto & description : gpio_command_interface_descriptions) { gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 54d24474bc..53b182bf99 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,74 +905,38 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } -std::vector parse_joint_state_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info) +std::vector parse_state_interface_descriptions_from_hardware_info( + const std::vector & component_info) { - std::vector joint_state_interface_descriptions; - joint_state_interface_descriptions.reserve(hw_info.joints.size()); + std::vector component_state_interface_descriptions; + component_state_interface_descriptions.reserve(component_info.size()); - for (const auto & joint : hw_info.joints) + for (const auto & component : component_info) { - for (const auto & state_interface : joint.state_interfaces) + for (const auto & state_interface : component.state_interfaces) { - joint_state_interface_descriptions.emplace_back( - InterfaceDescription(joint.name, state_interface)); + component_state_interface_descriptions.emplace_back( + InterfaceDescription(component.name, state_interface)); } } - return joint_state_interface_descriptions; + return component_state_interface_descriptions; } -std::vector parse_sensor_state_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info) -{ - std::vector sensor_state_interface_descriptions; - sensor_state_interface_descriptions.reserve(hw_info.sensors.size()); - - for (const auto & sensor : hw_info.sensors) - { - for (const auto & state_interface : sensor.state_interfaces) - { - sensor_state_interface_descriptions.emplace_back( - InterfaceDescription(sensor.name, state_interface)); - } - } - return sensor_state_interface_descriptions; -} - -std::vector parse_gpio_state_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info) -{ - std::vector gpio_state_interface_descriptions; - gpio_state_interface_descriptions.reserve(hw_info.gpios.size()); - - for (const auto & gpio : hw_info.gpios) - { - for (const auto & state_interface : gpio.state_interfaces) - { - gpio_state_interface_descriptions.emplace_back( - InterfaceDescription(gpio.name, state_interface)); - } - } - return gpio_state_interface_descriptions; -} - -std::vector parse_joint_command_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info) +std::vector parse_command_interface_descriptions_from_hardware_info( + const std::vector & component_info) { - std::vector - gpio_state_intejoint_command_interface_descriptionsrface_descriptions; - gpio_state_intejoint_command_interface_descriptionsrface_descriptions.reserve( - hw_info.joints.size()); + std::vector component_command_interface_descriptions; + component_command_interface_descriptions.reserve(component_info.size()); - for (const auto & joint : hw_info.joints) + for (const auto & component : component_info) { - for (const auto & command_interface : joint.command_interfaces) + for (const auto & command_interface : component.command_interfaces) { - gpio_state_intejoint_command_interface_descriptionsrface_descriptions.emplace_back( - InterfaceDescription(joint.name, command_interface)); + component_command_interface_descriptions.emplace_back( + InterfaceDescription(component.name, command_interface)); } } - return gpio_state_intejoint_command_interface_descriptionsrface_descriptions; + return component_command_interface_descriptions; } std::vector parse_gpio_command_interface_descriptions_from_hardware_info( diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index aa6a56a1f5..cdbff8ca21 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1414,7 +1414,7 @@ TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardwa const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto joint_state_descriptions = - parse_joint_state_interface_descriptions_from_hardware_info(control_hardware[0]); + parse_state_interface_descriptions_from_hardware_info(control_hardware[0].joints); EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1"); EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position"); EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); @@ -1433,7 +1433,7 @@ TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hard const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto joint_command_descriptions = - parse_joint_command_interface_descriptions_from_hardware_info(control_hardware[0]); + parse_command_interface_descriptions_from_hardware_info(control_hardware[0].joints); EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1"); EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position"); EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); @@ -1455,7 +1455,7 @@ TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardw const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto sensor_state_descriptions = - parse_sensor_state_interface_descriptions_from_hardware_info(control_hardware[0]); + parse_state_interface_descriptions_from_hardware_info(control_hardware[0].sensors); EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1"); EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll"); EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); @@ -1480,7 +1480,7 @@ TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardwar const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto gpio_state_descriptions = - parse_gpio_state_interface_descriptions_from_hardware_info(control_hardware[0]); + parse_state_interface_descriptions_from_hardware_info(control_hardware[0].gpios); EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); @@ -1505,7 +1505,7 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto gpio_state_descriptions = - parse_gpio_command_interface_descriptions_from_hardware_info(control_hardware[0]); + parse_command_interface_descriptions_from_hardware_info(control_hardware[0].gpios); EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); From bd064f0df67b15267217d7c7be66afa1e2a71df7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 09:34:49 +0100 Subject: [PATCH 18/34] export_interfaces_2() virtual for custom interface export --- .../hardware_interface/actuator_interface.hpp | 33 ++- .../hardware_interface/hardware_info.hpp | 9 +- .../hardware_interface/sensor_interface.hpp | 16 +- .../hardware_interface/system_interface.hpp | 37 ++- ...est_component_interfaces_custom_export.cpp | 225 +++++++++++++----- hardware_interface/test/test_components.hpp | 40 ++++ 6 files changed, 281 insertions(+), 79 deletions(-) create mode 100644 hardware_interface/test/test_components.hpp diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 75a9eb3d2e..8415dd818f 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -181,6 +181,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return {}; } + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector> export_state_interfaces_2() + { + // return empty vector by default. + return {}; + } + /** * Default implementation for exporting the StateInterfaces. The StateInterfaces are created * according to the InterfaceDescription. The memory accessed by the controllers and hardware is @@ -190,7 +204,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector> on_export_state_interfaces() { - std::vector> state_interfaces; + std::vector> state_interfaces = export_state_interfaces_2(); state_interfaces.reserve(joint_state_interfaces_.size()); for (const auto & [name, descr] : joint_state_interfaces_) @@ -226,6 +240,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return {}; } + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * + * Note method name is going to be changed to export_command_interfaces() as soon as the + * deprecated version is removed. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector> export_command_interfaces_2() + { + // return empty vector by default. + return {}; + } + /** * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created * according to the InterfaceDescription. The memory accessed by the controllers and hardware is @@ -235,7 +263,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector> on_export_command_interfaces() { - std::vector> command_interfaces; + std::vector> command_interfaces = + export_command_interfaces_2(); command_interfaces.reserve(joint_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 8e71282e21..bf14f24041 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -40,9 +40,9 @@ struct InterfaceInfo std::string max; /// (Optional) Initial value of the interface. std::string initial_value; - /// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs. + /// (Optional) The datatype of the interface, e.g. "bool", "int". std::string data_type; - /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. + /// (Optional) If the handle is an array, the size of the array. int size; /// (Optional) enable or disable the limits for the command interfaces bool enable_limits; @@ -142,11 +142,6 @@ struct InterfaceDescription */ std::string prefix_name; - /** - * What type of component is exported: joint, sensor or gpio - */ - std::string component_type; - /** * Information about the Interface type (position, velocity,...) as well as limits and so on. */ diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 96a4ce53b6..f13be0be4a 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -165,6 +165,20 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return {}; } + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector> export_state_interfaces_2() + { + // return empty vector by default. + return {}; + } + /** * Default implementation for exporting the StateInterfaces. The StateInterfaces are created * according to the InterfaceDescription. The memory accessed by the controllers and hardware is @@ -174,7 +188,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::vector> on_export_state_interfaces() { - std::vector> state_interfaces; + std::vector> state_interfaces = export_state_interfaces_2(); state_interfaces.reserve(sensor_state_interfaces_.size()); for (const auto & [name, descr] : sensor_state_interfaces_) diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 4be31b9f43..4ab0048600 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -200,6 +200,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return {}; } + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector> export_state_interfaces_2() + { + // return empty vector by default. + return {}; + } + /** * Default implementation for exporting the StateInterfaces. The StateInterfaces are created * according to the InterfaceDescription. The memory accessed by the controllers and hardware is @@ -207,9 +221,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector> on_export_state_interfaces() + std::vector> on_export_state_interfaces() { - std::vector> state_interfaces; + std::vector> state_interfaces = export_state_interfaces_2(); state_interfaces.reserve( joint_state_interfaces_.size() + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); @@ -259,6 +273,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return {}; } + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * + * Note method name is going to be changed to export_command_interfaces() as soon as the + * deprecated version is removed. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector> export_command_interfaces_2() + { + // return empty vector by default. + return {}; + } + /** * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created * according to the InterfaceDescription. The memory accessed by the controllers and hardware is @@ -266,9 +294,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored CommandInterfaces */ - virtual std::vector> on_export_command_interfaces() + std::vector> on_export_command_interfaces() { - std::vector> command_interfaces; + std::vector> command_interfaces = + export_command_interfaces_2(); command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); for (const auto & [name, descr] : joint_command_interfaces_) diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 07fc3204e3..fe76dc21ac 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -36,6 +36,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" // Values to send over command interface to trigger error in write and read methods @@ -56,10 +57,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { std::string get_name() const override { return "DummyActuatorDefault"; } - std::vector> on_export_state_interfaces() + std::vector> export_state_interfaces_2() override { - auto interfaces = hardware_interface::ActuatorInterface::on_export_state_interfaces(); + std::vector> interfaces; auto unlisted_state_interface = std::make_shared( info_.joints[0].name, "some_unlisted_interface", nullptr); actuator_states_.insert( @@ -69,10 +70,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface return interfaces; } - std::vector> on_export_command_interfaces() + std::vector> export_command_interfaces_2() override { - auto interfaces = hardware_interface::ActuatorInterface::on_export_command_interfaces(); + std::vector> interfaces; auto unlisted_state_interface = std::make_shared( info_.joints[0].name, "some_unlisted_interface", nullptr); actuator_commands_.insert( @@ -99,10 +100,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface { std::string get_name() const override { return "DummySensorDefault"; } - std::vector> on_export_state_interfaces() + std::vector> export_state_interfaces_2() override { - auto interfaces = hardware_interface::SensorInterface::on_export_state_interfaces(); + std::vector> interfaces; auto unlisted_state_interface = std::make_shared( info_.sensors[0].name, "some_unlisted_interface", nullptr); sensor_states_.insert( @@ -123,10 +124,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface { std::string get_name() const override { return "DummySystemDefault"; } - std::vector> on_export_state_interfaces() + std::vector> export_state_interfaces_2() override { - auto interfaces = hardware_interface::SystemInterface::on_export_state_interfaces(); + std::vector> interfaces; auto unlisted_state_interface = std::make_shared( info_.joints[0].name, "some_unlisted_interface", nullptr); system_states_.insert( @@ -136,10 +137,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface return interfaces; } - std::vector> on_export_command_interfaces() + std::vector> export_command_interfaces_2() override { - auto interfaces = hardware_interface::SystemInterface::on_export_command_interfaces(); + std::vector> interfaces; auto unlisted_state_interface = std::make_shared( info_.joints[0].name, "some_unlisted_interface", nullptr); system_commands_.insert( @@ -182,24 +183,50 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) auto state_interfaces = actuator_hw.export_state_interfaces(); ASSERT_EQ(3u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[2]->get_name()); - EXPECT_EQ("some_unlisted_interface", state_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[2]->get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(2u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[1]->get_name()); - EXPECT_EQ("some_unlisted_interface", command_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[1]->get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } } TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) @@ -219,13 +246,23 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); - EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); - EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[1]->get_name()); - EXPECT_EQ("some_unlisted_interface", state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } } TEST(TestComponentInterfaces, dummy_system_default_custom_export) @@ -245,40 +282,98 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(7u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); - EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); - EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[6]->get_name()); - EXPECT_EQ("some_unlisted_interface", state_interfaces[6]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[6]->get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", state_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = system_hw.export_command_interfaces(); ASSERT_EQ(4u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); - EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[3]->get_name()); - EXPECT_EQ("some_unlisted_interface", command_interfaces[3]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[3]->get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/some_unlisted_interface"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/some_unlisted_interface", command_interfaces[position]->get_name()); + EXPECT_EQ("some_unlisted_interface", command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } } diff --git a/hardware_interface/test/test_components.hpp b/hardware_interface/test/test_components.hpp new file mode 100644 index 0000000000..cd61d7b529 --- /dev/null +++ b/hardware_interface/test/test_components.hpp @@ -0,0 +1,40 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_COMPONENTS_HPP_ +#define TEST_COMPONENTS_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" + +namespace test_components +{ + +template +std::pair vector_contains(const std::vector & vec, const T & element) +{ + auto it = std::find_if( + vec.begin(), vec.end(), + [element](const auto & state_interface) + { return state_interface->get_name() == std::string(element); }); + + return std::make_pair(it != vec.end(), std::distance(vec.begin(), it)); +} + +} // namespace test_components +#endif // TEST_COMPONENTS_HPP_ From ca89202beb410e057e5e09875c2b46b31a6f4293 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 10:51:39 +0100 Subject: [PATCH 19/34] use unordered map and adjust tests --- .../hardware_interface/actuator_interface.hpp | 5 +- .../hardware_interface/sensor_interface.hpp | 3 +- .../hardware_interface/system_interface.hpp | 11 +- .../test/test_component_interfaces.cpp | 279 ++++++++++++------ 4 files changed, 201 insertions(+), 97 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 8415dd818f..bc3f5e36f1 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -16,7 +16,6 @@ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #include -#include #include #include #include @@ -396,8 +395,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; - std::map joint_state_interfaces_; - std::map joint_command_interfaces_; + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; std::unordered_map> actuator_states_; std::unordered_map> actuator_commands_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index f13be0be4a..8a3ea92479 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -16,7 +16,6 @@ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #include -#include #include #include #include @@ -264,7 +263,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI HardwareInfo info_; - std::map sensor_state_interfaces_; + std::unordered_map sensor_state_interfaces_; std::unordered_map> sensor_states_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 4ab0048600..72e4217551 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -16,7 +16,6 @@ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #include -#include #include #include #include @@ -436,13 +435,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; - std::map joint_state_interfaces_; - std::map joint_command_interfaces_; + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; - std::map sensor_state_interfaces_; + std::unordered_map sensor_state_interfaces_; - std::map gpio_state_interfaces_; - std::map gpio_command_interfaces_; + std::unordered_map gpio_state_interfaces_; + std::unordered_map gpio_command_interfaces_; std::unordered_map> system_states_; std::unordered_map> system_commands_; diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 9dc80652a1..4d9bed186e 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include "hardware_interface/actuator.hpp" @@ -34,6 +36,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" // Values to send over command interface to trigger error in write and read methods @@ -787,30 +790,49 @@ TEST(TestComponentInterfaces, dummy_actuator_default) auto state_interfaces = actuator_hw.export_state_interfaces(); ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto si_joint1_vel = test_components::vector_contains(state_interfaces, "joint1/velocity").second; for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -824,8 +846,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -839,8 +862,10 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -854,8 +879,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -917,24 +942,30 @@ TEST(TestComponentInterfaces, dummy_sensor_default) auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); - EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } // Not updated because is is UNCONFIGURED + auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0]->get_value()); + EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); + EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); } // BEGIN (Handle export change): for backward compatibility @@ -1081,54 +1112,114 @@ TEST(TestComponentInterfaces, dummy_system_default) auto state_interfaces = system_hw.export_state_interfaces(); ASSERT_EQ(6u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); - EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = system_hw.export_command_interfaces(); ASSERT_EQ(3u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); - + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } + + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + auto ci_joint2_vel = + test_components::vector_contains(command_interfaces, "joint2/velocity").second; + auto ci_joint3_vel = + test_components::vector_contains(command_interfaces, "joint3/velocity").second; double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - command_interfaces[1]->set_value(velocity_value); // velocity - command_interfaces[2]->set_value(velocity_value); // velocity + command_interfaces[ci_joint1_vel]->set_value(velocity_value); // velocity + command_interfaces[ci_joint2_vel]->set_value(velocity_value); // velocity + command_interfaces[ci_joint3_vel]->set_value(velocity_value); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); // Noting should change because it is UNCONFIGURED + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto si_joint1_vel = test_components::vector_contains(state_interfaces, "joint1/velocity").second; + auto si_joint2_pos = test_components::vector_contains(state_interfaces, "joint2/position").second; + auto si_joint2_vel = test_components::vector_contains(state_interfaces, "joint2/velocity").second; + auto si_joint3_pos = test_components::vector_contains(state_interfaces, "joint3/position").second; + auto si_joint3_vel = test_components::vector_contains(state_interfaces, "joint3/velocity").second; for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1142,12 +1233,15 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1161,12 +1255,18 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + (10 + step) * velocity_value, + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1180,12 +1280,12 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1327,9 +1427,12 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1455,9 +1558,12 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1584,8 +1690,9 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); // activate again and expect reset values + auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); From 6795a969dd0f322ea4a7c23a8cdedf9fc7a1e46e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 1 Feb 2024 10:46:13 +0100 Subject: [PATCH 20/34] make states and commands of component interfaces private --- .../hardware_interface/actuator_interface.hpp | 55 +++++++++++++---- .../hardware_interface/sensor_interface.hpp | 29 +++++++-- .../hardware_interface/system_interface.hpp | 57 ++++++++++++++---- hardware_interface/src/component_parser.cpp | 17 ------ ...est_component_interfaces_custom_export.cpp | 60 ++++++++----------- 5 files changed, 139 insertions(+), 79 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index bc3f5e36f1..cc32adce3e 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -182,13 +182,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. * * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated * version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector> export_state_interfaces_2() + virtual std::vector export_state_interfaces_2() { // return empty vector by default. return {}; @@ -203,8 +204,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector> on_export_state_interfaces() { - std::vector> state_interfaces = export_state_interfaces_2(); - state_interfaces.reserve(joint_state_interfaces_.size()); + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interfaces_2(); + + std::vector> state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + actuator_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } for (const auto & [name, descr] : joint_state_interfaces_) { @@ -241,13 +258,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. * * Note method name is going to be changed to export_command_interfaces() as soon as the * deprecated version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted CommandInterfaces */ - virtual std::vector> export_command_interfaces_2() + virtual std::vector export_command_interfaces_2() { // return empty vector by default. return {}; @@ -262,9 +280,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector> on_export_command_interfaces() { - std::vector> command_interfaces = + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = export_command_interfaces_2(); - command_interfaces.reserve(joint_command_interfaces_.size()); + + std::vector> command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + actuator_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } for (const auto & [name, descr] : joint_command_interfaces_) { @@ -398,14 +431,16 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; - std::unordered_map> actuator_states_; - std::unordered_map> actuator_commands_; + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger actuator_logger_; + std::unordered_map> actuator_states_; + std::unordered_map> actuator_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 8a3ea92479..fbe6e1ac94 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -166,13 +166,14 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. * * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated * version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector> export_state_interfaces_2() + virtual std::vector export_state_interfaces_2() { // return empty vector by default. return {}; @@ -187,8 +188,24 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::vector> on_export_state_interfaces() { - std::vector> state_interfaces = export_state_interfaces_2(); - state_interfaces.reserve(sensor_state_interfaces_.size()); + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interfaces_2(); + + std::vector> state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + sensor_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } for (const auto & [name, descr] : sensor_state_interfaces_) { @@ -264,14 +281,14 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI HardwareInfo info_; std::unordered_map sensor_state_interfaces_; - - std::unordered_map> sensor_states_; + std::unordered_map unlisted_state_interfaces_; rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger sensor_logger_; + std::unordered_map> sensor_states_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 72e4217551..8f5fa99ce3 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -201,13 +201,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. * * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated * version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector> export_state_interfaces_2() + virtual std::vector export_state_interfaces_2() { // return empty vector by default. return {}; @@ -222,10 +223,25 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ std::vector> on_export_state_interfaces() { - std::vector> state_interfaces = export_state_interfaces_2(); + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interfaces_2(); + + std::vector> state_interfaces; state_interfaces.reserve( - joint_state_interfaces_.size() + sensor_state_interfaces_.size() + - gpio_state_interfaces_.size()); + unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } for (const auto & [name, descr] : joint_state_interfaces_) { @@ -274,13 +290,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. * * Note method name is going to be changed to export_command_interfaces() as soon as the * deprecated version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted CommandInterfaces */ - virtual std::vector> export_command_interfaces_2() + virtual std::vector export_command_interfaces_2() { // return empty vector by default. return {}; @@ -295,9 +312,25 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ std::vector> on_export_command_interfaces() { - std::vector> command_interfaces = + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = export_command_interfaces_2(); - command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); + + std::vector> command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + + gpio_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + system_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } for (const auto & [name, descr] : joint_command_interfaces_) { @@ -443,14 +476,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map gpio_state_interfaces_; std::unordered_map gpio_command_interfaces_; - std::unordered_map> system_states_; - std::unordered_map> system_commands_; + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger system_logger_; + std::unordered_map> system_states_; + std::unordered_map> system_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 53b182bf99..9dac3d9f66 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -939,21 +939,4 @@ std::vector parse_command_interface_descriptions_from_hard return component_command_interface_descriptions; } -std::vector parse_gpio_command_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info) -{ - std::vector gpio_command_interface_descriptions; - gpio_command_interface_descriptions.reserve(hw_info.gpios.size()); - - for (const auto & gpio : hw_info.gpios) - { - for (const auto & command_interface : gpio.command_interfaces) - { - gpio_command_interface_descriptions.emplace_back( - InterfaceDescription(gpio.name, command_interface)); - } - } - return gpio_command_interface_descriptions; -} - } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index fe76dc21ac..7a5011892b 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -57,27 +57,23 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { std::string get_name() const override { return "DummyActuatorDefault"; } - std::vector> export_state_interfaces_2() - override + std::vector export_state_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - actuator_states_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; } - std::vector> export_command_interfaces_2() - override + std::vector export_command_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - actuator_commands_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; @@ -100,14 +96,12 @@ class DummySensorDefault : public hardware_interface::SensorInterface { std::string get_name() const override { return "DummySensorDefault"; } - std::vector> export_state_interfaces_2() - override + std::vector export_state_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.sensors[0].name, "some_unlisted_interface", nullptr); - sensor_states_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.sensors[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; @@ -124,27 +118,23 @@ class DummySystemDefault : public hardware_interface::SystemInterface { std::string get_name() const override { return "DummySystemDefault"; } - std::vector> export_state_interfaces_2() - override + std::vector export_state_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - system_states_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; } - std::vector> export_command_interfaces_2() - override + std::vector export_command_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - system_commands_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; From 5ad4044e771baf6d7e258c0e190e678ce875e421 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 10 Apr 2024 17:34:13 +0200 Subject: [PATCH 21/34] add migration guide for --- doc/Iron.rst | 137 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 137 insertions(+) create mode 100644 doc/Iron.rst diff --git a/doc/Iron.rst b/doc/Iron.rst new file mode 100644 index 0000000000..b5524ebee2 --- /dev/null +++ b/doc/Iron.rst @@ -0,0 +1,137 @@ +Iron to Jazzy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +component parser +***************** +Changes from `(PR #1256) `__ + +* All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise a ``std::runtime_error`` is thrown. This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` tags instead. +* The syntax for mimic joints is changed to the `official URDF specification `__. The parameters within the ``ros2_control`` tag are not supported any more. Instead of + + .. code-block:: xml + + + + + + 0.15 + + + + + + right_finger_joint + 1 + + + + + + + + define your mimic joints directly in the joint definitions: + + .. code-block:: xml + + + + + + + + + + + + + + + + + +Adaption of Command-/StateInterfaces +*************************************** +Changes from `(PR #1240) `__ + +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` method based on the ``InterfaceDescription`` (check the hardware_info.hpp). +* The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. +* To access the automatically created ``Command-/StateInterfaces`` we provide to ``std::unordered_map``. Where the string is the fully qualified name of the interface and the ``InterfaceDescription`` the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the .ros2_control.xacro but created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function and creating some custom ``Command-/StateInterfaces``. + +Migration of Command-/StateInterfaces +------------------------------------- +To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: +1. Delete the ``std::vector export_command_interfaces() override`` and ``std::vector export_state_interfaces() override``. +2. Delete the allocated Memory for any ``Command-/StateInterfaces``: +3. If you have a ``std::vector hw_commands_;`` for joint ``CommandInterfaces`` delete it and any usage/appearance. Wherever you iterated over a state/command or accessed commands stated like this: + +.. code-block:: c++ + + // states + for (uint i = 0; i < hw_states_.size(); i++) + { + hw_states_[i] = 0; + auto some_state = hw_states_[i]; + } + + // commands + for (uint i = 0; i < hw_commands_.size(); i++) + { + hw_commands_[i] = 0; + auto some_command = hw_commands_[i]; + } + + // specific state/command + hw_commands_[x] = hw_states_[y]; + +replace it with + +.. code-block:: c++ + + // states replace with this + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + auto some_state = get_state(name); + } + + //commands replace with this + for (const auto & [name, descr] : joint_commands_interfaces_) + { + set_command(name, 0.0); + auto some_command = get_command(name); + } + + // replace specific state/command, for this you need to store the names which are std::strings + // somewhere or know them. However be careful since the names are fully qualified names which + // means that the prefix is included for the name: E.g.: prefix/joint_1/velocity + set_command(name_of_command_interface_x, get_state(name_of_state_interface_y)); + +Migration of unlisted Command-/StateInterfaces not defined in .ros2_control.xacro +--------------------------------------------------------------------------------- +If you have some unlisted ``Command-/StateInterfaces`` not included in the .ros2_control.xacro you can follow those steps: +1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` +2. Create the InterfaceDescription for each of the Interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: + + .. code-block:: c++ + + std::vector my_unlisted_interfaces; + + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "5.0"; + unlisted_interface.data_type = "1.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + + return my_unlisted_interfaces; + + 3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. + 4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. + +Custom exportation of Command-/StateInterfaces +---------------------------------------------- +In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: +* If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. +* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. +* Names must be unique! From 4addfe3665bbff28df11e57d5e172c7dd41cf776 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Apr 2024 11:43:36 +0200 Subject: [PATCH 22/34] update writing a hardware component --- .../doc/writing_new_hardware_component.rst | 52 ++++++++++++++----- 1 file changed, 39 insertions(+), 13 deletions(-) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index 698f6cf6e2..fabee16489 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -52,26 +52,52 @@ The following is a step-by-step guide to create source files, basic tests, and c 4. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. 5. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. - - 6. Implement ``export_state_interfaces`` and ``export_command_interfaces`` methods where interfaces that hardware offers are defined. + 6. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` method based on the interfaces defined in the .ros2_control.xacro which gets parsed and creates ``InterfaceDescriptions`` accordingly (check the hardware_info.hpp). + To access the automatically created ``Command-/StateInterfaces`` we provide to ``std::unordered_map``. Where the string is the fully qualified name of the interface and the ``InterfaceDescription`` the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the .ros2_control.xacro but created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function and creating some custom ``Command-/StateInterfaces``. For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. As a reminder, the full interface names have structure ``/``. + 7. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the .ros2_control.xacro you can follow those steps: + + 1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` + 2. 5. Create the InterfaceDescription for each of the Interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: + + .. code-block:: c++ + + std::vector my_unlisted_interfaces; + + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "5.0"; + unlisted_interface.data_type = "1.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + + return my_unlisted_interfaces; + + 3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. + 4. 1. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. + 8. (optional)In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: + + * If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. + * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. + * Names must be unique! - 7. (optional) For *Actuator* and *System* types of hardware interface implement ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` if your hardware accepts multiple control modes. + 1. (optional) For *Actuator* and *System* types of hardware interface implement ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` if your hardware accepts multiple control modes. - 8. Implement the ``on_activate`` method where hardware "power" is enabled. + 2. Implement the ``on_activate`` method where hardware "power" is enabled. - 9. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. + 3. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. - 10. Implement ``on_shutdown`` method where hardware is shutdown gracefully. + 4. Implement ``on_shutdown`` method where hardware is shutdown gracefully. - 11. Implement ``on_error`` method where different errors from all states are handled. + 5. Implement ``on_error`` method where different errors from all states are handled. - 12. Implement the ``read`` method getting the states from the hardware and storing them to internal variables defined in ``export_state_interfaces``. + 6. Implement the ``read`` method getting the states from the hardware and storing them to internal variables defined in ``export_state_interfaces``. - 13. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. + 7. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. - 14. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. + 8. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header. As first parameters you should provide exact hardware interface class, e.g., ``::``, and as second the base class, i.e., ``hardware_interface::(Actuator|Sensor|System)Interface``. @@ -125,12 +151,12 @@ The following is a step-by-step guide to create source files, basic tests, and c 2. Add at least the following package into ```` tag: ``ament_add_gmock`` and ``hardware_interface``. -9. **Compiling and testing the hardware component** +9. **Compiling and testing the hardware component** - 1. Now everything is ready to compile the hardware component using the ``colcon build `` command. + 3. Now everything is ready to compile the hardware component using the ``colcon build `` command. Remember to go into the root of your workspace before executing this command. - 2. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. + 4. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. That's it! Enjoy writing great controllers! From 9d78bb7da22a32dfa5e99393bc6fcf3344de49c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 18 Apr 2024 12:07:30 +0200 Subject: [PATCH 23/34] Fix RST syntax and some typos (#18) * Fix rst syntax and some typos * Fix rst syntax and small typos * Fix clang issue --- doc/Iron.rst | 29 ++-- .../doc/writing_new_hardware_component.rst | 128 +++++++++--------- hardware_interface/test/test_components.hpp | 3 +- 3 files changed, 82 insertions(+), 78 deletions(-) diff --git a/doc/Iron.rst b/doc/Iron.rst index b5524ebee2..3a463c736a 100644 --- a/doc/Iron.rst +++ b/doc/Iron.rst @@ -53,16 +53,19 @@ Adaption of Command-/StateInterfaces *************************************** Changes from `(PR #1240) `__ -* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` method based on the ``InterfaceDescription`` (check the hardware_info.hpp). +* ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). * The memory for storing the value of a ``Command-/StateInterfaces`` is no longer allocated in the hardware but instead in the ``Command-/StateInterfaces`` itself. -* To access the automatically created ``Command-/StateInterfaces`` we provide to ``std::unordered_map``. Where the string is the fully qualified name of the interface and the ``InterfaceDescription`` the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the .ros2_control.xacro but created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function and creating some custom ``Command-/StateInterfaces``. +* To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function by creating some custom ``Command-/StateInterfaces``. Migration of Command-/StateInterfaces ------------------------------------- To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: + 1. Delete the ``std::vector export_command_interfaces() override`` and ``std::vector export_state_interfaces() override``. -2. Delete the allocated Memory for any ``Command-/StateInterfaces``: -3. If you have a ``std::vector hw_commands_;`` for joint ``CommandInterfaces`` delete it and any usage/appearance. Wherever you iterated over a state/command or accessed commands stated like this: +2. Delete allocated memory for any ``Command-/StateInterfaces``, e.g.: + + * If you have a ``std::vector hw_commands_;`` for joints' ``CommandInterfaces`` delete the definition and any usage/appearance. + * Wherever you iterated over a state/command or accessed commands like this: .. code-block:: c++ @@ -106,11 +109,12 @@ replace it with // means that the prefix is included for the name: E.g.: prefix/joint_1/velocity set_command(name_of_command_interface_x, get_state(name_of_state_interface_y)); -Migration of unlisted Command-/StateInterfaces not defined in .ros2_control.xacro ---------------------------------------------------------------------------------- -If you have some unlisted ``Command-/StateInterfaces`` not included in the .ros2_control.xacro you can follow those steps: +Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` XML-tag +-------------------------------------------------------------------------------------- +If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: + 1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` -2. Create the InterfaceDescription for each of the Interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: +2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: .. code-block:: c++ @@ -119,19 +123,18 @@ If you have some unlisted ``Command-/StateInterfaces`` not included in the .ros2 InterfaceInfo unlisted_interface; unlisted_interface.name = "some_unlisted_interface"; unlisted_interface.min = "-5.0"; - unlisted_interface.data_type = "5.0"; - unlisted_interface.data_type = "1.0"; unlisted_interface.data_type = "double"; my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); return my_unlisted_interfaces; - 3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. - 4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. +3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. +4. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. Custom exportation of Command-/StateInterfaces ---------------------------------------------- In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: + * If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. -* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. +* Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return ``shared_ptrs`` and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. * Names must be unique! diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index fabee16489..21c55cb311 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -8,7 +8,7 @@ Writing a Hardware Component In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new hardware interface. -1. **Preparing package** +#. **Preparing package** If the package for the hardware interface does not exist, then create it first. The package should have ``ament_cmake`` as a build type. @@ -17,7 +17,7 @@ The following is a step-by-step guide to create source files, basic tests, and c Use the ``--help`` flag for more information on how to use it. There is also an option to create library source files and compile rules to help you in the following steps. -2. **Preparing source files** +#. **Preparing source files** After creating the package, you should have at least ``CMakeLists.txt`` and ``package.xml`` files in it. Create also ``include//`` and ``src`` folders if they do not already exist. @@ -25,7 +25,7 @@ The following is a step-by-step guide to create source files, basic tests, and c Optionally add ``visibility_control.h`` with the definition of export rules for Windows. You can copy this file from an existing controller package and change the name prefix to the ````. -3. **Adding declarations into header file (.hpp)** +#. **Adding declarations into header file (.hpp)** 1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ). @@ -39,124 +39,126 @@ The following is a step-by-step guide to create source files, basic tests, and c class HardwareInterfaceName : public hardware_interface::$InterfaceType$Interface 5. Add a constructor without parameters and the following public methods implementing ``LifecycleNodeInterface``: ``on_configure``, ``on_cleanup``, ``on_shutdown``, ``on_activate``, ``on_deactivate``, ``on_error``; and overriding ``$InterfaceType$Interface`` definition: ``on_init``, ``export_state_interfaces``, ``export_command_interfaces``, ``prepare_command_mode_switch`` (optional), ``perform_command_mode_switch`` (optional), ``read``, ``write``. - For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. -4. **Adding definitions into source file (.cpp)** + For further explanation of hardware-lifecycle check the `pull request `_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation `_ for *Actuator*, *Sensor* or *System*. - 1. Include the header file of your hardware interface and add a namespace definition to simplify further development. +#. **Adding definitions into source file (.cpp)** - 2. Implement ``on_init`` method. Here, you should initialize all member variables and process the parameters from the ``info`` argument. + #. Include the header file of your hardware interface and add a namespace definition to simplify further development. + + #. Implement ``on_init`` method. Here, you should initialize all member variables and process the parameters from the ``info`` argument. In the first line usually the parents ``on_init`` is called to process standard values, like name. This is done using: ``hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)``. If all required parameters are set and valid and everything works fine return ``CallbackReturn::SUCCESS`` or ``return CallbackReturn::ERROR`` otherwise. - 4. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. + #. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. + + #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. + #. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). + + * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function by creating some custom ``Command-/StateInterfaces``. + * For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. + * As a reminder, the full interface names have structure ``/``. + + #. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: - 5. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. - 6. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` method based on the interfaces defined in the .ros2_control.xacro which gets parsed and creates ``InterfaceDescriptions`` accordingly (check the hardware_info.hpp). - To access the automatically created ``Command-/StateInterfaces`` we provide to ``std::unordered_map``. Where the string is the fully qualified name of the interface and the ``InterfaceDescription`` the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the .ros2_control.xacro but created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function and creating some custom ``Command-/StateInterfaces``. - For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. - As a reminder, the full interface names have structure ``/``. - 7. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the .ros2_control.xacro you can follow those steps: + #. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` + #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: - 1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` - 2. 5. Create the InterfaceDescription for each of the Interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: + .. code-block:: c++ - .. code-block:: c++ + std::vector my_unlisted_interfaces; - std::vector my_unlisted_interfaces; + InterfaceInfo unlisted_interface; + unlisted_interface.name = "some_unlisted_interface"; + unlisted_interface.min = "-5.0"; + unlisted_interface.data_type = "double"; + my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); - InterfaceInfo unlisted_interface; - unlisted_interface.name = "some_unlisted_interface"; - unlisted_interface.min = "-5.0"; - unlisted_interface.data_type = "5.0"; - unlisted_interface.data_type = "1.0"; - unlisted_interface.data_type = "double"; - my_unlisted_interfaces.push_back(InterfaceDescription(info_.name, unlisted_interface)); + return my_unlisted_interfaces; - return my_unlisted_interfaces; + #. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. + #. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. - 3. The unlisted interface will then be stored in either the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_`` map depending in which function they are created. - 4. 1. You can access it like any other interface with the ``get_state(name)``, ``set_state(name, value)``, ``get_command(name)`` or ``set_command(name, value)``. E.g. ``get_state("some_unlisted_interface")``. - 8. (optional)In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: + #. (optional) In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: - * If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. - * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. - * Names must be unique! + * If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. + * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. + * Names must be unique! - 1. (optional) For *Actuator* and *System* types of hardware interface implement ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` if your hardware accepts multiple control modes. + #. (optional) For *Actuator* and *System* types of hardware interface implement ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` if your hardware accepts multiple control modes. - 2. Implement the ``on_activate`` method where hardware "power" is enabled. + #. Implement the ``on_activate`` method where hardware "power" is enabled. - 3. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. + #. Implement the ``on_deactivate`` method, which does the opposite of ``on_activate``. - 4. Implement ``on_shutdown`` method where hardware is shutdown gracefully. + #. Implement ``on_shutdown`` method where hardware is shutdown gracefully. - 5. Implement ``on_error`` method where different errors from all states are handled. + #. Implement ``on_error`` method where different errors from all states are handled. - 6. Implement the ``read`` method getting the states from the hardware and storing them to internal variables defined in ``export_state_interfaces``. + #. Implement the ``read`` method getting the states from the hardware and storing them to internal variables defined in ``export_state_interfaces``. - 7. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. + #. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. - 8. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. + #. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header. As first parameters you should provide exact hardware interface class, e.g., ``::``, and as second the base class, i.e., ``hardware_interface::(Actuator|Sensor|System)Interface``. -5. **Writing export definition for pluginlib** +#. **Writing export definition for pluginlib** - 1. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. + #. Create the ``.xml`` file in the package and add a definition of the library and hardware interface's class which has to be visible for the pluginlib. The easiest way to do that is to check definition for mock components in the :ref:`hardware_interface mock_components ` section. - 2. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., + #. Usually, the plugin name is defined by the package (namespace) and the class name, e.g., ``/``. This name defines the hardware interface's type when the resource manager searches for it. The other two parameters have to correspond to the definition done in the macro at the bottom of the ``.cpp`` file. -6. **Writing a simple test to check if the controller can be found and loaded** +#. **Writing a simple test to check if the controller can be found and loaded** - 1. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. + #. Create the folder ``test`` in your package, if it does not exist already, and add a file named ``test_load_.cpp``. - 2. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. + #. You can copy the ``load_generic_system_2dof`` content defined in the `test_generic_system.cpp `_ package. - 3. Change the name of the copied test and in the last line, where hardware interface type is specified put the name defined in ``.xml`` file, e.g., ``/``. + #. Change the name of the copied test and in the last line, where hardware interface type is specified put the name defined in ``.xml`` file, e.g., ``/``. -7. **Add compile directives into ``CMakeLists.txt`` file** +#. **Add compile directives into ``CMakeLists.txt`` file** - 1. Under the line ``find_package(ament_cmake REQUIRED)`` add further dependencies. + #. Under the line ``find_package(ament_cmake REQUIRED)`` add further dependencies. Those are at least: ``hardware_interface``, ``pluginlib``, ``rclcpp`` and ``rclcpp_lifecycle``. - 2. Add a compile directive for a shared library providing the ``.cpp`` file as the source. + #. Add a compile directive for a shared library providing the ``.cpp`` file as the source. - 3. Add targeted include directories for the library. This is usually only ``include``. + #. Add targeted include directories for the library. This is usually only ``include``. - 4. Add ament dependencies needed by the library. You should add at least those listed under 1. + #. Add ament dependencies needed by the library. You should add at least those listed under 1. - 5. Export for pluginlib description file using the following command: + #. Export for pluginlib description file using the following command: .. code:: cmake pluginlib_export_plugin_description_file(hardware_interface .xml) - 6. Add install directives for targets and include directory. + #. Add install directives for targets and include directory. - 7. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. + #. In the test section add the following dependencies: ``ament_cmake_gmock``, ``hardware_interface``. - 8. Add compile definitions for the tests using the ``ament_add_gmock`` directive. + #. Add compile definitions for the tests using the ``ament_add_gmock`` directive. For details, see how it is done for mock hardware in the `ros2_control `_ package. - 9. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. + #. (optional) Add your hardware interface`s library into ``ament_export_libraries`` before ``ament_package()``. -8. **Add dependencies into ``package.xml`` file** +#. **Add dependencies into ``package.xml`` file** - 1. Add at least the following packages into ```` tag: ``hardware_interface``, ``pluginlib``, ``rclcpp``, and ``rclcpp_lifecycle``. + #. Add at least the following packages into ```` tag: ``hardware_interface``, ``pluginlib``, ``rclcpp``, and ``rclcpp_lifecycle``. - 2. Add at least the following package into ```` tag: ``ament_add_gmock`` and ``hardware_interface``. + #. Add at least the following package into ```` tag: ``ament_add_gmock`` and ``hardware_interface``. -9. **Compiling and testing the hardware component** +#. **Compiling and testing the hardware component** - 3. Now everything is ready to compile the hardware component using the ``colcon build `` command. + #. Now everything is ready to compile the hardware component using the ``colcon build `` command. Remember to go into the root of your workspace before executing this command. - 4. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. + #. If compilation was successful, source the ``setup.bash`` file from the install folder and execute ``colcon test `` to check if the new controller can be found through ``pluginlib`` library and be loaded by the controller manager. That's it! Enjoy writing great controllers! diff --git a/hardware_interface/test/test_components.hpp b/hardware_interface/test/test_components.hpp index cd61d7b529..def72b8398 100644 --- a/hardware_interface/test/test_components.hpp +++ b/hardware_interface/test/test_components.hpp @@ -29,8 +29,7 @@ template std::pair vector_contains(const std::vector & vec, const T & element) { auto it = std::find_if( - vec.begin(), vec.end(), - [element](const auto & state_interface) + vec.begin(), vec.end(), [element](const auto & state_interface) { return state_interface->get_name() == std::string(element); }); return std::make_pair(it != vec.end(), std::distance(vec.begin(), it)); From 66d204e91c7d98a747214bace8caf0547c41d5ef Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 22 Apr 2024 11:16:08 +0200 Subject: [PATCH 24/34] add changes for chainable controllers (epxport of rerference interfaces) --- .../chainable_controller_interface.hpp | 12 ++- .../controller_interface.hpp | 3 +- .../controller_interface_base.hpp | 3 +- .../src/chainable_controller_interface.cpp | 78 ++++++++++++++++--- .../src/controller_interface.cpp | 3 +- .../test_chainable_controller_interface.cpp | 13 ++-- controller_manager/src/controller_manager.cpp | 26 +++++-- .../hardware_interface/resource_manager.hpp | 3 +- hardware_interface/src/resource_manager.cpp | 9 ++- .../test/test_resource_manager.cpp | 4 +- 10 files changed, 119 insertions(+), 35 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2e39f038b1..fa82a94c3b 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,7 +15,9 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include #include +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -60,7 +62,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase std::vector export_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector export_state_interfaces() final; + + CONTROLLER_INTERFACE_PUBLIC + std::vector> export_reference_interfaces() + final; CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; @@ -135,7 +141,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// Storage of values for reference interfaces std::vector exported_reference_interface_names_; + // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; + // END + std::unordered_map> + reference_interfaces_ptrs_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 43fd269803..5d0fc5051a 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -55,7 +55,8 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector> export_reference_interfaces() + final; /** * Controller is not chainable, therefore no chained mode can be set. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9eaee9f15b..2a71017018 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -221,7 +221,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of command interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_reference_interfaces() = 0; + virtual std::vector> + export_reference_interfaces() = 0; /** * Export interfaces for a chainable controller that can be used as state interface by other diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 9f4a171ec3..05effe1483 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -68,29 +68,87 @@ ChainableControllerInterface::export_state_interfaces() return state_interfaces; } -std::vector -ChainableControllerInterface::export_reference_interfaces() +std::vector> +ChainableControllerInterface::export_state_interfaces() { - auto reference_interfaces = on_export_reference_interfaces(); + auto state_interfaces = on_export_state_interfaces(); - // check if the names of the reference interfaces begin with the controller's name - for (const auto & interface : reference_interfaces) + // check if the names of the controller state interfaces begin with the controller's name + for (const auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { RCLCPP_FATAL( get_node()->get_logger(), "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory " - " for reference interfaces. No reference interface will be exported. Please correct and " - "recompile the controller with name '%s' and try again.", + "mandatory for state interfaces. No state interface will be exported. Please " + "correct and recompile the controller with name '%s' and try again.", interface.get_name().c_str(), get_node()->get_name()); - reference_interfaces.clear(); + state_interfaces.clear(); break; } } - return reference_interfaces; + return state_interfaces; +} + +std::vector> +ChainableControllerInterface::export_reference_interfaces() +{ + auto reference_interfaces = on_export_reference_interfaces(); + std::vector> reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + + // BEGIN (Handle export change): for backward compatibility + // check if the "reference_interfaces_" variable is resized to number of interfaces + if (reference_interfaces_.size() != reference_interfaces.size()) + { + // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the + // framework + std::string error_msg = + "The internal storage for reference values 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + // END + + // check if the names of the reference interfaces begin with the controller's name + const auto ref_interface_size = reference_interfaces.size(); + for (auto & interface : reference_interfaces) + { + if (interface.get_prefix_name() != get_node()->get_name()) + { + std::string error_msg = "The name of the interface " + interface.get_name() + + " does not begin with the controller's name. This is mandatory for " + "reference interfaces. Please " + "correct and recompile the controller with name " + + get_node()->get_name() + " and try again."; + throw std::runtime_error(error_msg); + } + + std::shared_ptr interface_ptr = + std::make_shared(std::move(interface)); + reference_interfaces_ptrs_vec.push_back(interface_ptr); + reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr)); + } + + if (reference_interfaces_ptrs_.size() != ref_interface_size) + { + std::string error_msg = + "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + + std::to_string(reference_interfaces_ptrs_.size()) + + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + + return reference_interfaces_ptrs_vec; } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index a039501aa1..fae7a0cf75 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -27,7 +27,8 @@ std::vector ControllerInterface::export_stat return {}; } -std::vector ControllerInterface::export_reference_interfaces() +std::vector> +ControllerInterface::export_reference_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 2f04273f3e..cb50ebf352 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -47,11 +47,11 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_THAT(exported_state_interfaces, SizeIs(1)); - EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state"); + ASSERT_EQ(reference_interfaces.size(), 1u); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -114,8 +114,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -124,7 +123,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Success setting chained mode - reference_interfaces[0].set_value(0.0); + reference_interfaces[0]->set_value(0.0); EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 48dbbc720a..efa8b28a53 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -752,14 +752,26 @@ controller_interface::return_type ControllerManager::configure_controller( "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); auto state_interfaces = controller->export_state_interfaces(); - auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && state_interfaces.empty()) + std::vector> interfaces; + try { - // TODO(destogl): Add test for this! - RCLCPP_ERROR( - get_logger(), - "Controller '%s' is chainable, but does not export any state or reference interfaces.", - controller_name.c_str()); + interfaces = controller->export_reference_interfaces(); + if (interfaces.empty() && state_interfaces.empty()) + { + // TODO(destogl): Add test for this! + RCLCPP_ERROR( + get_logger(), + "Controller '%s' is chainable, but does not export any reference interfaces. Did you " + "override the on_export_method() correctly?", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::runtime_error & e) + { + RCLCPP_FATAL( + get_logger(), "Creation of the reference interfaces failed with following error: %s", + e.what()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, ref_interfaces); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 065ecabed9..3bd76f8db0 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -194,7 +194,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces. */ void import_controller_reference_interfaces( - const std::string & controller_name, std::vector & interfaces); + const std::string & controller_name, + const std::vector> & interfaces); /// Get list of reference interface of a controller. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index fbd8ca452a..4e9ef8c719 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -642,7 +642,7 @@ class ResourceStorage } } - void insert_command_interface(std::shared_ptr command_interface) + void insert_command_interface(const std::shared_ptr command_interface) { const auto [it, success] = command_interface_map_.insert( std::make_pair(command_interface->get_name(), command_interface)); @@ -770,11 +770,11 @@ class ResourceStorage } std::vector add_command_interfaces( - std::vector> & interfaces) + const std::vector> & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + for (const auto & interface : interfaces) { auto key = interface->get_name(); insert_command_interface(interface); @@ -1305,7 +1305,8 @@ void ResourceManager::remove_controller_exported_state_interfaces( // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, + const std::vector> & interfaces) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); auto interface_names = resource_storage_->add_command_interfaces(interfaces); diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 51d81a90ab..f932c610fe 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1230,12 +1230,12 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; - std::vector reference_interfaces; + std::vector> reference_interfaces; std::vector reference_interface_values = {1.0, 2.0, 3.0}; for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( + reference_interfaces.push_back(std::make_shared( CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); } From 444a36c633862528a997a544203cebae210beaf7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 22 May 2024 18:05:45 +0200 Subject: [PATCH 25/34] rename export_state/command_interface_2 to export_state/commad_interface_descriptions --- .../doc/writing_new_hardware_component.rst | 8 ++++---- .../hardware_interface/actuator_interface.hpp | 10 ++++++---- .../hardware_interface/sensor_interface.hpp | 5 +++-- .../hardware_interface/system_interface.hpp | 10 ++++++---- .../test_component_interfaces_custom_export.cpp | 15 ++++++++++----- 5 files changed, 29 insertions(+), 19 deletions(-) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index 21c55cb311..ad3728c21a 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -55,14 +55,14 @@ The following is a step-by-step guide to create source files, basic tests, and c #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. #. ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which gets parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp `__). - * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function by creating some custom ``Command-/StateInterfaces``. + * To access the automatically created ``Command-/StateInterfaces`` we provide the ``std::unordered_map``, where the string is the fully qualified name of the interface and the ``InterfaceDescription`` is the configuration of the interface. The ``std::unordered_map<>`` are divided into ``type_state_interfaces_`` and ``type_command_interfaces_`` where type can be: ``joint``, ``sensor``, ``gpio`` and ``unlisted``. E.g. the ``CommandInterfaces`` for all joints can be found in the ``joint_command_interfaces_`` map. The ``unlisted`` includes all interfaces not listed in the ``ros2_control`` XML-tag but were created by overriding the ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` function by creating some custom ``Command-/StateInterfaces``. * For the ``Sensor``-type hardware interface there is no ``export_command_interfaces`` method. * As a reminder, the full interface names have structure ``/``. #. (optional) If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: - #. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` - #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: + #. Override the ``virtual std::vector export_command_interface_descriptions()`` or ``virtual std::vector export_state_interface_descriptions()`` + #. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` function, add it to a vector and return the vector: .. code-block:: c++ @@ -81,7 +81,7 @@ The following is a step-by-step guide to create source files, basic tests, and c #. (optional) In case the default implementation (``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` ) for exporting the ``Command-/StateInterfaces`` is not enough you can override them. You should however consider the following things: - * If you want to have unlisted interfaces available you need to call the ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. + * If you want to have unlisted interfaces available you need to call the ``export_command_interface_descriptions()`` or ``export_state_interface_descriptions()`` and add them to the ``unlisted_command_interfaces_`` or ``unlisted_state_interfaces_``. * Don't forget to store the created ``Command-/StateInterfaces`` internally as you only return shared_ptrs and the resource manager will not provide access to the created ``Command-/StateInterfaces`` for the hardware. So you must take care of storing them yourself. * Names must be unique! diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index cc32adce3e..3080c33c62 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -189,7 +189,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector export_state_interfaces_2() + virtual std::vector + export_state_interface_descriptions() { // return empty vector by default. return {}; @@ -206,7 +207,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_state_interfaces_2(); + export_state_interface_descriptions(); std::vector> state_interfaces; state_interfaces.reserve( @@ -265,7 +266,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of descriptions to the unlisted CommandInterfaces */ - virtual std::vector export_command_interfaces_2() + virtual std::vector + export_command_interface_descriptions() { // return empty vector by default. return {}; @@ -282,7 +284,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_command_interfaces_2(); + export_command_interface_descriptions(); std::vector> command_interfaces; command_interfaces.reserve( diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index fbe6e1ac94..0726fd90ce 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -173,7 +173,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector export_state_interfaces_2() + virtual std::vector + export_state_interface_descriptions() { // return empty vector by default. return {}; @@ -190,7 +191,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_state_interfaces_2(); + export_state_interface_descriptions(); std::vector> state_interfaces; state_interfaces.reserve( diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 8f5fa99ce3..d41af76e9f 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -208,7 +208,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector export_state_interfaces_2() + virtual std::vector + export_state_interface_descriptions() { // return empty vector by default. return {}; @@ -225,7 +226,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_state_interfaces_2(); + export_state_interface_descriptions(); std::vector> state_interfaces; state_interfaces.reserve( @@ -297,7 +298,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of descriptions to the unlisted CommandInterfaces */ - virtual std::vector export_command_interfaces_2() + virtual std::vector + export_command_interface_descriptions() { // return empty vector by default. return {}; @@ -314,7 +316,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { // import the unlisted interfaces std::vector unlisted_interface_descriptions = - export_command_interfaces_2(); + export_command_interface_descriptions(); std::vector> command_interfaces; command_interfaces.reserve( diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 7a5011892b..38794a8b91 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -57,7 +57,8 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { std::string get_name() const override { return "DummyActuatorDefault"; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; @@ -68,7 +69,8 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface return interfaces; } - std::vector export_command_interfaces_2() override + std::vector export_command_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; @@ -96,7 +98,8 @@ class DummySensorDefault : public hardware_interface::SensorInterface { std::string get_name() const override { return "DummySensorDefault"; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; @@ -118,7 +121,8 @@ class DummySystemDefault : public hardware_interface::SystemInterface { std::string get_name() const override { return "DummySystemDefault"; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; @@ -129,7 +133,8 @@ class DummySystemDefault : public hardware_interface::SystemInterface return interfaces; } - std::vector export_command_interfaces_2() override + std::vector export_command_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; From e90413c80cfc9926f615749f800b93e25f631382 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 25 Jul 2024 16:05:01 +0200 Subject: [PATCH 26/34] adapt to new changes and remove some rebase artefacts --- .../chainable_controller_interface.hpp | 5 +- .../controller_interface.hpp | 2 +- .../controller_interface_base.hpp | 3 +- .../src/chainable_controller_interface.cpp | 50 ++++-------- .../src/controller_interface.cpp | 3 +- .../test_chainable_controller_interface.cpp | 16 ++-- controller_manager/src/controller_manager.cpp | 9 ++- .../hardware_interface/actuator_interface.hpp | 2 +- .../hardware_interface/resource_manager.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 2 +- .../hardware_interface/system_interface.hpp | 2 +- hardware_interface/src/resource_manager.cpp | 80 +++++++++++-------- .../test/test_component_interfaces.cpp | 24 ++++-- ...est_component_interfaces_custom_export.cpp | 9 ++- 14 files changed, 105 insertions(+), 104 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index fa82a94c3b..68d032edbc 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -59,10 +59,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; - - CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector> export_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC std::vector> export_reference_interfaces() diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 5d0fc5051a..b1e7afbfc0 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -47,7 +47,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector> export_state_interfaces() final; /** * Controller has no reference interfaces. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2a71017018..07f000dff7 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -231,7 +231,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_state_interfaces() = 0; + virtual std::vector> + export_state_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 05effe1483..9a5916b8b3 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,52 +44,32 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector +std::vector> ChainableControllerInterface::export_state_interfaces() { auto state_interfaces = on_export_state_interfaces(); + std::vector> state_interfaces_ptrs_vec; + state_interfaces_ptrs_vec.reserve(state_interfaces.size()); // check if the names of the controller state interfaces begin with the controller's name - for (const auto & interface : state_interfaces) + for (auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " + std::string error_msg = + "The name of the interface '" + interface.get_name() + + "' does not begin with the controller's name. This is " "mandatory for state interfaces. No state interface will be exported. Please " - "correct and recompile the controller with name '%s' and try again.", - interface.get_name().c_str(), get_node()->get_name()); - state_interfaces.clear(); - break; + "correct and recompile the controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); } - } - - return state_interfaces; -} - -std::vector> -ChainableControllerInterface::export_state_interfaces() -{ - auto state_interfaces = on_export_state_interfaces(); - // check if the names of the controller state interfaces begin with the controller's name - for (const auto & interface : state_interfaces) - { - if (interface.get_prefix_name() != get_node()->get_name()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory for state interfaces. No state interface will be exported. Please " - "correct and recompile the controller with name '%s' and try again.", - interface.get_name().c_str(), get_node()->get_name()); - state_interfaces.clear(); - break; - } + state_interfaces_ptrs_vec.push_back( + std::make_shared(interface)); } - return state_interfaces; + return state_interfaces_ptrs_vec; } std::vector> @@ -187,8 +167,8 @@ ChainableControllerInterface::on_export_state_interfaces() std::vector state_interfaces; for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); + state_interfaces.emplace_back( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]); } return state_interfaces; } diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index fae7a0cf75..58c1c53496 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -22,7 +22,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_state_interfaces() +std::vector> +ControllerInterface::export_state_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index cb50ebf352..d7075a0157 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -47,11 +47,11 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_EQ(reference_interfaces.size(), 1u); - EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); + ASSERT_THAT(exported_state_interfaces, SizeIs(1)); + EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); - EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -68,10 +68,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_THAT(reference_interfaces, SizeIs(1)); - EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) @@ -127,7 +127,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index efa8b28a53..721a954f03 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -751,12 +751,13 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto state_interfaces = controller->export_state_interfaces(); - std::vector> interfaces; + std::vector> state_interfaces; + std::vector> ref_interfaces; try { - interfaces = controller->export_reference_interfaces(); - if (interfaces.empty() && state_interfaces.empty()) + state_interfaces = controller->export_state_interfaces(); + ref_interfaces = controller->export_reference_interfaces(); + if (ref_interfaces.empty() && state_interfaces.empty()) { // TODO(destogl): Add test for this! RCLCPP_ERROR( diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 3080c33c62..1265d43028 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -120,7 +120,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; import_state_interface_descriptions(info_); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 3bd76f8db0..3f886f7ef4 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -141,7 +141,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ void import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces); + const std::string & controller_name, std::vector> & interfaces); /// Get list of exported tate interface of a controller. /** diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 0726fd90ce..573b2d3643 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -120,7 +120,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; import_state_interface_descriptions(info_); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index d41af76e9f..9f1ef60b32 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -122,7 +122,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; import_state_interface_descriptions(info_); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4e9ef8c719..466c61bc45 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -606,40 +606,29 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { - try - { - std::vector interface_names; - std::vector> interfaces = hardware.export_state_interfaces(); + std::vector interface_names; + std::vector> interfaces = hardware.export_state_interfaces(); - interface_names.reserve(interfaces.size()); - for (auto const & interface : interfaces) + interface_names.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + try { - const auto [it, success] = - state_interface_map_.insert(std::make_pair(interface->get_name(), interface)); - if (!success) - { - std::string msg( - "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + - interface->get_name() + "]"); - throw std::runtime_error(msg); - } - interface_names.push_back(interface->get_name()); + interface_names.push_back(add_state_interface(interface)); + } + // We don't want to crash during runtime because a StateInterface could not be added + catch (const std::exception & e) + { + RCLCPP_WARN( + get_logger(), + "Exception occurred while importing state interfaces for the hardware '%s' : %s", + hardware.get_name().c_str(), e.what()); } } - catch (const std::exception & e) - { - RCLCPP_ERROR( - get_logger(), - "Exception occurred while importing state interfaces for the hardware '%s' : %s", - hardware.get_name().c_str(), e.what()); - } - catch (...) - { - RCLCPP_ERROR( - get_logger(), - "Unknown exception occurred while importing state interfaces for the hardware '%s'", - hardware.get_name().c_str()); - } + + hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); } void insert_command_interface(const std::shared_ptr command_interface) @@ -700,6 +689,19 @@ class ResourceStorage } } + std::string add_state_interface(std::shared_ptr interface) + { + auto interface_name = interface->get_name(); + const auto [it, success] = state_interface_map_.emplace(interface_name, interface); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + interface->get_name() + "]"); + throw std::runtime_error(msg); + } + return interface_name; + } /// Adds exported state interfaces into internal storage. /** * Adds state interfaces to the internal storage. State interfaces exported from hardware or @@ -711,15 +713,23 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ - std::vector add_state_interfaces(std::vector & interfaces) + std::vector add_state_interfaces( + std::vector> & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); for (auto & interface : interfaces) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + try + { + interface_names.push_back(add_state_interface(interface)); + } + // We don't want to crash during runtime because a StateInterface could not be added + catch (const std::exception & e) + { + RCLCPP_WARN( + get_logger(), "Exception occurred while importing state interfaces: %s", e.what()); + } } available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); @@ -1243,7 +1253,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, std::vector> & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 4d9bed186e..e8fcc5d41d 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -783,7 +783,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -936,7 +937,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - auto state = sensor_hw.initialize(voltage_sensor_res); + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1106,7 +1108,8 @@ TEST(TestComponentInterfaces, dummy_system_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - auto state = system_hw.initialize(dummy_system); + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1400,7 +1403,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1532,7 +1536,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1667,7 +1672,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - auto state = sensor_hw.initialize(voltage_sensor_res); + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1793,7 +1799,8 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - auto state = system_hw.initialize(dummy_system); + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1930,7 +1937,8 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - auto state = system_hw.initialize(dummy_system); + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 38794a8b91..04408e1d1e 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -171,7 +171,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -235,7 +236,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - auto state = sensor_hw.initialize(voltage_sensor_res); + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -271,7 +273,8 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - auto state = system_hw.initialize(dummy_system); + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); From 52767479dc6a51988e445a68148c673adc6e8004 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jul 2024 09:57:15 +0200 Subject: [PATCH 27/34] fix test failures --- .../src/chainable_controller_interface.cpp | 6 +++--- .../test/test_chainable_controller_interface.cpp | 14 ++++++++++---- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 9a5916b8b3..d7cf82f32e 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -57,9 +57,9 @@ ChainableControllerInterface::export_state_interfaces() if (interface.get_prefix_name() != get_node()->get_name()) { std::string error_msg = - "The name of the interface '" + interface.get_name() + - "' does not begin with the controller's name. This is " - "mandatory for state interfaces. No state interface will be exported. Please " + "The prefix of the interface '" + interface.get_prefix_name() + + "' does not equal the controller's name '" + get_node()->get_name() + + "'. This is mandatory for state interfaces. No state interface will be exported. Please " "correct and recompile the controller with name '" + get_node()->get_name() + "' and try again."; throw std::runtime_error(error_msg); diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index d7075a0157..8918b7178e 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -15,6 +15,7 @@ #include "test_chainable_controller_interface.hpp" #include +#include using ::testing::IsEmpty; using ::testing::SizeIs; @@ -51,7 +52,7 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); - EXPECT_EQ(exported_state_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -88,10 +89,15 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); // expect empty return because interface prefix is not equal to the node name - auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_THAT(reference_interfaces, IsEmpty()); + std::vector> exported_reference_interfaces; + EXPECT_THROW( + { exported_reference_interfaces = controller.export_reference_interfaces(); }, + std::runtime_error); + ASSERT_THAT(exported_reference_interfaces, IsEmpty()); // expect empty return because interface prefix is not equal to the node name - auto exported_state_interfaces = controller.export_state_interfaces(); + std::vector> exported_state_interfaces; + EXPECT_THROW( + { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error); ASSERT_THAT(exported_state_interfaces, IsEmpty()); } From 267df75df8029875f2e163286886bb0903544034 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Jan 2024 11:52:13 +0100 Subject: [PATCH 28/34] add interface for warning, error and report * ddd basic test for error codes setting * docs for error and warning signals --- doc/index.rst | 1 + hardware_interface/CMakeLists.txt | 4 + .../error_and_warning_interfaces_userdoc.rst | 28 + .../hardware_interface/actuator_interface.hpp | 97 +- .../include/hardware_interface/handle.hpp | 4 +- .../hardware_interface/sensor_interface.hpp | 79 +- .../hardware_interface/system_interface.hpp | 97 +- ...rdware_interface_emergency_stop_signal.hpp | 29 + .../hardware_interface_error_signals.hpp | 105 ++ .../hardware_interface_warning_signals.hpp | 104 ++ .../test/test_component_interfaces.cpp | 11 +- ...est_component_interfaces_custom_export.cpp | 27 +- .../test/test_error_warning_codes.cpp | 1254 +++++++++++++++++ 13 files changed, 1823 insertions(+), 17 deletions(-) create mode 100644 hardware_interface/doc/error_and_warning_interfaces_userdoc.rst create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp create mode 100644 hardware_interface/test/test_error_warning_codes.cpp diff --git a/doc/index.rst b/doc/index.rst index 09a2ddf745..1521d49da8 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -27,6 +27,7 @@ Concepts Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst> Joint Kinematics <../hardware_interface/doc/joints_userdoc.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> + Hardware Components <../hardware_interface/doc/error_and_warning_interfaces_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> ===================================== diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 385ae96fb1..fcc8917fd0 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -84,6 +84,10 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces_custom_export hardware_interface) ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp) + target_link_libraries(test_error_warning_codes hardware_interface) + ament_target_dependencies(test_error_warning_codes ros2_control_test_assets) + ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface) ament_target_dependencies(test_component_parser ros2_control_test_assets) diff --git a/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst b/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst new file mode 100644 index 0000000000..3cbf10e0a7 --- /dev/null +++ b/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst @@ -0,0 +1,28 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst + +.. _error_and_warning_interfaces_userdoc: + +Error and Warning Interfaces +============================ + +By default we now create the following error and warning interfaces: + ++-----------------+--------------+----------------------------------------------------------------------------------------------------------------------+ +| Type | Datatype | Description | ++=================+====================+================================================================================================================+ +| Emergency Stop | Bool | Used for signaling that hardwares emergency stop is active. Only for Actuator and System. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Error Code | array | Used for sending 32 error codes (uint8_t) at the same time. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Error Message | array | Used for sending 32 error messages where the message at position x corresponds to error code at position x. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Warning Code | array | Used for sending 32 Warning codes (int8_t) at the same time. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Warning Message | array | Used for sending 32 warning messages where the message at position x corresponds to warning code at position x.| ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ + +The error and warning interfaces are created as ``StateInterfaces`` and are stored inside the Actuator-, Sensor- or SystemInterface. They can be accessed via getter and setter methods. E.g. if you want to get/set the emergency stop signal you can do so with the ``get_emergency_stop()`` or ``set_emergency_stop(const bool & emergency_stop)`` methods. For the error and warning signals similar getters and setters exist. + +Note: The SensorInterface does not have a Emergency Stop interface. diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1265d43028..6d96b99cc5 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -25,7 +25,10 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -125,6 +128,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod info_ = hardware_info; import_state_interface_descriptions(info_); import_command_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -156,6 +160,52 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod } } + /** + * Creates all interfaces used for reporting emergency stop, warning and error messages. + * The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called joint_1 -> interface for WARNING_SIGNAL is called: joint_1/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // EMERGENCY STOP + InterfaceInfo emergency_interface_info; + emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL; + emergency_interface_info.data_type = "bool"; + InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info); + emergency_stop_ = std::make_shared(emergency_interface_descr); + + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "array[32]"; + InterfaceDescription error_interface_descr(info_.name, error_interface_info); + error_signal_ = std::make_shared(error_interface_descr); + // create error signal report message interface + InterfaceInfo error_msg_interface_info; + error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; + error_msg_interface_info.data_type = "array[32]"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "array[32]"; + InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); + warning_signal_ = std::make_shared(warning_interface_descr); + // create warning signal report message interface + InterfaceInfo warning_msg_interface_info; + warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; + warning_msg_interface_info.data_type = "array[32]"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -230,6 +280,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod actuator_states_.insert(std::make_pair(name, state_interface)); state_interfaces.push_back(state_interface); } + + // export warning signal interfaces + state_interfaces.push_back(emergency_stop_); + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -416,6 +474,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return actuator_commands_.at(interface_name)->get_value(); } + void set_emergency_stop(const double & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + double get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: /// Get the logger of the ActuatorInterface. /** @@ -436,13 +523,19 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; - rclcpp_lifecycle::State lifecycle_state_; - private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger actuator_logger_; std::unordered_map> actuator_states_; std::unordered_map> actuator_commands_; + + std::shared_ptr emergency_stop_; + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + + rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 72b1d6cf92..16d06c578d 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include #include @@ -23,8 +24,9 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/visibility_control.h" - namespace hardware_interface { diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 573b2d3643..e4b45916f7 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -25,7 +25,9 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -124,6 +126,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { info_ = hardware_info; import_state_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -141,6 +144,45 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } } + /** + * Creates all interfaces used for reporting warning and error messages. + * The available report interfaces are: ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called sensor_1 -> interface for WARNING_SIGNAL is called: sensor_1/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "array[32]"; + InterfaceDescription error_interface_descr(info_.name, error_interface_info); + error_signal_ = std::make_shared(error_interface_descr); + // create error signal report message interface + InterfaceInfo error_msg_interface_info; + error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; + error_msg_interface_info.data_type = "array[32]"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "array[32]"; + InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); + warning_signal_ = std::make_shared(warning_interface_descr); + // create warning signal report message interface + InterfaceInfo warning_msg_interface_info; + warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; + warning_msg_interface_info.data_type = "array[32]"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -217,6 +259,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI state_interfaces.push_back(state_interface); } + // export warning signal interfaces + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -266,6 +314,28 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return sensor_states_.at(interface_name)->get_value(); } + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: /// Get the logger of the SensorInterface. /** @@ -284,12 +354,17 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map sensor_state_interfaces_; std::unordered_map unlisted_state_interfaces_; - rclcpp_lifecycle::State lifecycle_state_; - private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger sensor_logger_; std::unordered_map> sensor_states_; + + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + + rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 9f1ef60b32..07c9ef9984 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,8 +25,11 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -127,6 +130,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI info_ = hardware_info; import_state_interface_descriptions(info_); import_command_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -176,6 +180,52 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } } + /** + * Creates all interfaces used for reporting emergency stop, warning and error messages. + * The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called rrbot -> interface for WARNING_SIGNAL is called: rrbot/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // EMERGENCY STOP + InterfaceInfo emergency_interface_info; + emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL; + emergency_interface_info.data_type = "bool"; + InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info); + emergency_stop_ = std::make_shared(emergency_interface_descr); + + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "array[32]"; + InterfaceDescription error_interface_descr(info_.name, error_interface_info); + error_signal_ = std::make_shared(error_interface_descr); + // create error signal report message interface + InterfaceInfo error_msg_interface_info; + error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; + error_msg_interface_info.data_type = "array[32]"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "array[32]"; + InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); + warning_signal_ = std::make_shared(warning_interface_descr); + // create warning signal report message interface + InterfaceInfo warning_msg_interface_info; + warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; + warning_msg_interface_info.data_type = "array[32]"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -262,6 +312,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI system_states_.insert(std::make_pair(name, state_interface)); state_interfaces.push_back(state_interface); } + + // export warning signal interfaces + state_interfaces.push_back(emergency_stop_); + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -456,6 +514,35 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return system_commands_.at(interface_name)->get_value(); } + void set_emergency_stop(const double & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + double get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: /// Get the logger of the SystemInterface. /** @@ -481,13 +568,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; - rclcpp_lifecycle::State lifecycle_state_; - private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::Logger system_logger_; std::unordered_map> system_states_; std::unordered_map> system_commands_; + + std::shared_ptr emergency_stop_; + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + + rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp new file mode 100644 index 0000000000..c74941f9df --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp @@ -0,0 +1,29 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ + +#include +#include + +namespace hardware_interface +{ +// Count of how many different emergency stop signals there are that can be reported. +const size_t emergency_stop_signal_count = 1; + +constexpr char EMERGENCY_STOP_SIGNAL[] = "EMERGENCY_STOP_SIGNAL"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp new file mode 100644 index 0000000000..d5159f29c8 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -0,0 +1,105 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ + +#include +#include + +namespace hardware_interface +{ + +// Count of how many different error signals there are that can be reported. +const size_t error_signal_count = 32; + +constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; +// Available error signal names +enum class error_signal : std::uint8_t +{ + ERROR_SIGNAL_0 = 0, + ERROR_SIGNAL_1 = 1, + ERROR_SIGNAL_2 = 2, + ERROR_SIGNAL_3 = 3, + ERROR_SIGNAL_4 = 4, + ERROR_SIGNAL_5 = 5, + ERROR_SIGNAL_6 = 6, + ERROR_SIGNAL_7 = 7, + ERROR_SIGNAL_8 = 8, + ERROR_SIGNAL_9 = 9, + ERROR_SIGNAL_10 = 10, + ERROR_SIGNAL_11 = 11, + ERROR_SIGNAL_12 = 12, + ERROR_SIGNAL_13 = 13, + ERROR_SIGNAL_14 = 14, + ERROR_SIGNAL_15 = 15, + ERROR_SIGNAL_16 = 16, + ERROR_SIGNAL_17 = 17, + ERROR_SIGNAL_18 = 18, + ERROR_SIGNAL_19 = 19, + ERROR_SIGNAL_20 = 20, + ERROR_SIGNAL_21 = 21, + ERROR_SIGNAL_22 = 22, + ERROR_SIGNAL_23 = 23, + ERROR_SIGNAL_24 = 24, + ERROR_SIGNAL_25 = 25, + ERROR_SIGNAL_26 = 26, + ERROR_SIGNAL_27 = 27, + ERROR_SIGNAL_28 = 28, + ERROR_SIGNAL_29 = 29, + ERROR_SIGNAL_30 = 30, + ERROR_SIGNAL_31 = 31 +}; + +constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; +// Available WARNING signal message names +enum class error_signal_message : std::uint8_t +{ + ERROR_SIGNAL_MESSAGE_0 = 0, + ERROR_SIGNAL_MESSAGE_1 = 1, + ERROR_SIGNAL_MESSAGE_2 = 2, + ERROR_SIGNAL_MESSAGE_3 = 3, + ERROR_SIGNAL_MESSAGE_4 = 4, + ERROR_SIGNAL_MESSAGE_5 = 5, + ERROR_SIGNAL_MESSAGE_6 = 6, + ERROR_SIGNAL_MESSAGE_7 = 7, + ERROR_SIGNAL_MESSAGE_8 = 8, + ERROR_SIGNAL_MESSAGE_9 = 9, + ERROR_SIGNAL_MESSAGE_10 = 10, + ERROR_SIGNAL_MESSAGE_11 = 11, + ERROR_SIGNAL_MESSAGE_12 = 12, + ERROR_SIGNAL_MESSAGE_13 = 13, + ERROR_SIGNAL_MESSAGE_14 = 14, + ERROR_SIGNAL_MESSAGE_15 = 15, + ERROR_SIGNAL_MESSAGE_16 = 16, + ERROR_SIGNAL_MESSAGE_17 = 17, + ERROR_SIGNAL_MESSAGE_18 = 18, + ERROR_SIGNAL_MESSAGE_19 = 19, + ERROR_SIGNAL_MESSAGE_20 = 20, + ERROR_SIGNAL_MESSAGE_21 = 21, + ERROR_SIGNAL_MESSAGE_22 = 22, + ERROR_SIGNAL_MESSAGE_23 = 23, + ERROR_SIGNAL_MESSAGE_24 = 24, + ERROR_SIGNAL_MESSAGE_25 = 25, + ERROR_SIGNAL_MESSAGE_26 = 26, + ERROR_SIGNAL_MESSAGE_27 = 27, + ERROR_SIGNAL_MESSAGE_28 = 28, + ERROR_SIGNAL_MESSAGE_29 = 29, + ERROR_SIGNAL_MESSAGE_30 = 30, + ERROR_SIGNAL_MESSAGE_31 = 31 +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp new file mode 100644 index 0000000000..58af04b80a --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -0,0 +1,104 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ + +#include +#include + +namespace hardware_interface +{ +// Count of how many different warn signals there are that can be reported. +const size_t warning_signal_count = 32; + +constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; +// Available warning signals names mapping to position in the interface +enum class warning_signal : std::uint8_t +{ + WARNING_SIGNAL_0 = 0, + WARNING_SIGNAL_1 = 1, + WARNING_SIGNAL_2 = 2, + WARNING_SIGNAL_3 = 3, + WARNING_SIGNAL_4 = 4, + WARNING_SIGNAL_5 = 5, + WARNING_SIGNAL_6 = 6, + WARNING_SIGNAL_7 = 7, + WARNING_SIGNAL_8 = 8, + WARNING_SIGNAL_9 = 9, + WARNING_SIGNAL_10 = 10, + WARNING_SIGNAL_11 = 11, + WARNING_SIGNAL_12 = 12, + WARNING_SIGNAL_13 = 13, + WARNING_SIGNAL_14 = 14, + WARNING_SIGNAL_15 = 15, + WARNING_SIGNAL_16 = 16, + WARNING_SIGNAL_17 = 17, + WARNING_SIGNAL_18 = 18, + WARNING_SIGNAL_19 = 19, + WARNING_SIGNAL_20 = 20, + WARNING_SIGNAL_21 = 21, + WARNING_SIGNAL_22 = 22, + WARNING_SIGNAL_23 = 23, + WARNING_SIGNAL_24 = 24, + WARNING_SIGNAL_25 = 25, + WARNING_SIGNAL_26 = 26, + WARNING_SIGNAL_27 = 27, + WARNING_SIGNAL_28 = 28, + WARNING_SIGNAL_29 = 29, + WARNING_SIGNAL_30 = 30, + WARNING_SIGNAL_31 = 31 +}; + +constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; +// Available WARNING signal message names +enum class warning_signal_message : std::uint8_t +{ + WARNING_SIGNAL_MESSAGE_0 = 0, + WARNING_SIGNAL_MESSAGE_1 = 1, + WARNING_SIGNAL_MESSAGE_2 = 2, + WARNING_SIGNAL_MESSAGE_3 = 3, + WARNING_SIGNAL_MESSAGE_4 = 4, + WARNING_SIGNAL_MESSAGE_5 = 5, + WARNING_SIGNAL_MESSAGE_6 = 6, + WARNING_SIGNAL_MESSAGE_7 = 7, + WARNING_SIGNAL_MESSAGE_8 = 8, + WARNING_SIGNAL_MESSAGE_9 = 9, + WARNING_SIGNAL_MESSAGE_10 = 10, + WARNING_SIGNAL_MESSAGE_11 = 11, + WARNING_SIGNAL_MESSAGE_12 = 12, + WARNING_SIGNAL_MESSAGE_13 = 13, + WARNING_SIGNAL_MESSAGE_14 = 14, + WARNING_SIGNAL_MESSAGE_15 = 15, + WARNING_SIGNAL_MESSAGE_16 = 16, + WARNING_SIGNAL_MESSAGE_17 = 17, + WARNING_SIGNAL_MESSAGE_18 = 18, + WARNING_SIGNAL_MESSAGE_19 = 19, + WARNING_SIGNAL_MESSAGE_20 = 20, + WARNING_SIGNAL_MESSAGE_21 = 21, + WARNING_SIGNAL_MESSAGE_22 = 22, + WARNING_SIGNAL_MESSAGE_23 = 23, + WARNING_SIGNAL_MESSAGE_24 = 24, + WARNING_SIGNAL_MESSAGE_25 = 25, + WARNING_SIGNAL_MESSAGE_26 = 26, + WARNING_SIGNAL_MESSAGE_27 = 27, + WARNING_SIGNAL_MESSAGE_28 = 28, + WARNING_SIGNAL_MESSAGE_29 = 29, + WARNING_SIGNAL_MESSAGE_30 = 30, + WARNING_SIGNAL_MESSAGE_31 = 31 +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index e8fcc5d41d..5dc009b894 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -45,6 +45,11 @@ namespace const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +const auto warnig_signals_size = 2; +const auto error_signals_size = 2; +const auto report_signals_size = + emergency_stop_signal_size + warnig_signals_size + error_signals_size; } // namespace using namespace ::testing; // NOLINT @@ -790,7 +795,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); + ASSERT_EQ(2u + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -943,7 +948,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); + ASSERT_EQ(1u + warnig_signals_size + error_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/voltage"); @@ -1114,7 +1119,7 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(6u, state_interfaces.size()); + ASSERT_EQ(6u + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 04408e1d1e..ad9cbc34ab 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control development team +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -40,11 +40,15 @@ // Values to send over command interface to trigger error in write and read methods +// Values to send over command interface to trigger error in write and read methods + namespace { -const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); -constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +const auto warnig_signals_size = 2; +const auto error_signals_size = 2; +const auto report_signals_size = + emergency_stop_signal_size + warnig_signals_size + error_signals_size; } // namespace using namespace ::testing; // NOLINT @@ -177,8 +181,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 2u; + const auto interfaces_size = listed_interface_size + report_signals_size; auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(3u, state_interfaces.size()); + // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" + ASSERT_EQ(interfaces_size + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -241,8 +248,11 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 1u; + const auto interfaces_sizeze = listed_interface_size + error_signals_size + warnig_signals_size; auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); + // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" + ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/voltage"); @@ -278,8 +288,11 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 6u; + const auto interfaces_sizeze = listed_interface_size + report_signals_size; auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(7u, state_interfaces.size()); + // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" + ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp new file mode 100644 index 0000000000..06e13a9fa3 --- /dev/null +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -0,0 +1,1254 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" +// Values to send over command interface to trigger error in write and read methods + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +const auto warnig_signals_size = 2; +const auto error_signals_size = 2; +const auto report_signals_size = + emergency_stop_signal_size + warnig_signals_size + error_signals_size; +} // namespace + +using namespace ::testing; // NOLINT + +namespace test_components +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); + set_emergency_stop(0.0); + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + + if (recoverable_error_happened_) + { + set_command("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/velocity", 0.0); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & [name, descr] : sensor_state_interfaces_) + { + set_state(name, 0.0); + } + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("joint1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySystemDefault : public hardware_interface::SystemInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (auto i = 0ul; i < 3; ++i) + { + set_state(position_states_[i], 0.0); + set_state(velocity_states_[i], 0.0); + } + set_emergency_stop(0.0); + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + // reset command only if error is initiated + if (recoverable_error_happened_) + { + for (auto i = 0ul; i < 3; ++i) + { + set_command(velocity_commands_[i], 0.0); + } + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySystemDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + for (auto i = 0; i < 3; ++i) + { + auto current_pos = get_state(position_states_[i]); + set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); + set_state(velocity_states_[i], get_command(velocity_commands_[i])); + } + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & velocity_state : velocity_states_) + { + set_state(velocity_state, 0.0); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + std::vector position_states_ = { + "joint1/position", "joint2/position", "joint3/position"}; + std::vector velocity_states_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + std::vector velocity_commands_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator_default) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + // EMERGENCY STOP + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::EMERGENCY_STOP_SIGNAL, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + // ERROR_SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + // WARNING SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_sensor_default) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 1; + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ( + state_interface_offset + warnig_signals_size + error_signals_size, state_interfaces.size()); + // check that the normal interfaces get exported as expected + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + // ERROR_SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } + // WARNING SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "SingleJointVoltage/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_system_default) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + // EMERGENCY STOP + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::EMERGENCY_STOP_SIGNAL, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + // ERROR_SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + // WARNING SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(3u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + + const auto state_interface_offset = 1; + auto state_interfaces = sensor_hw.export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + auto error_signal = + test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "SingleJointVoltage/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} From 6303e836a24fdc92304fe2e0f663ffbe8c50ddb7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 24 Jan 2024 11:53:23 +0100 Subject: [PATCH 29/34] full variant support : * add parsing of handle type for initialization of variant and adjust most tests * rename unclear variables * add changes for chainable controllers (epxport of rerference interfaces) --- .../chainable_controller_interface.hpp | 2 + .../src/chainable_controller_interface.cpp | 13 + .../test_chainable_controller_interface.cpp | 4 +- .../test_chainable_controller_interface.hpp | 36 +- .../test/test_force_torque_sensor.cpp | 122 +- controller_interface/test/test_imu_sensor.cpp | 77 +- .../test_semantic_component_interface.cpp | 20 +- .../test_chainable_controller.cpp | 10 +- .../hardware_interface/actuator_interface.hpp | 53 +- .../include/hardware_interface/handle.hpp | 167 ++- .../hardware_interface/hardware_info.hpp | 37 + .../loaned_command_interface.hpp | 2 +- .../loaned_state_interface.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 47 +- .../hardware_interface/system_interface.hpp | 54 +- .../types/handle_datatype.hpp | 48 + .../hardware_interface_error_signals.hpp | 75 +- .../hardware_interface_warning_signals.hpp | 75 +- .../mock_components/generic_system.hpp | 67 +- .../src/mock_components/generic_system.cpp | 539 ++++----- .../test/test_component_interfaces.cpp | 1056 ++--------------- ...est_component_interfaces_custom_export.cpp | 10 +- .../test/test_error_warning_codes.cpp | 316 +++-- hardware_interface/test/test_handle.cpp | 764 +++++++++++- .../test_force_torque_sensor.cpp | 53 +- .../test_single_joint_actuator.cpp | 39 +- .../test_system_with_command_modes.cpp | 55 +- .../test_two_joint_system.cpp | 28 +- .../test/test_components/test_actuator.cpp | 60 +- .../test/test_components/test_sensor.cpp | 12 - .../test/test_components/test_system.cpp | 16 +- .../test/test_resource_manager.cpp | 32 +- .../differential_transmission.hpp | 31 +- .../four_bar_linkage_transmission.hpp | 28 +- .../simple_transmission.hpp | 29 +- .../test/differential_transmission_test.cpp | 270 +++-- .../four_bar_linkage_transmission_test.cpp | 265 +++-- .../test/simple_transmission_test.cpp | 92 +- transmission_interface/test/utils_test.cpp | 7 +- 39 files changed, 2386 insertions(+), 2227 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/handle_datatype.hpp diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 68d032edbc..4820afd817 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,7 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include #include #include #include @@ -140,6 +141,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase std::vector exported_reference_interface_names_; // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; + std::unordered_map> ref_interface_to_value_; // END std::unordered_map> reference_interfaces_ptrs_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index d7cf82f32e..057065a31d 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -78,7 +78,10 @@ ChainableControllerInterface::export_reference_interfaces() auto reference_interfaces = on_export_reference_interfaces(); std::vector> reference_interfaces_ptrs_vec; reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + std::vector> reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + // BEGIN (Handle export change): for backward compatibility // BEGIN (Handle export change): for backward compatibility // check if the "reference_interfaces_" variable is resized to number of interfaces if (reference_interfaces_.size() != reference_interfaces.size()) @@ -93,6 +96,14 @@ ChainableControllerInterface::export_reference_interfaces() "controller with name '" + get_node()->get_name() + "' and try again."; throw std::runtime_error(error_msg); + std::string error_msg = + "The internal storage for reference values 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); } // END @@ -100,6 +111,7 @@ ChainableControllerInterface::export_reference_interfaces() const auto ref_interface_size = reference_interfaces.size(); for (auto & interface : reference_interfaces) { + auto & interface = reference_interfaces[i]; if (interface.get_prefix_name() != get_node()->get_name()) { std::string error_msg = "The name of the interface " + interface.get_name() + @@ -129,6 +141,7 @@ ChainableControllerInterface::export_reference_interfaces() } return reference_interfaces_ptrs_vec; + return reference_interfaces_ptrs_vec; } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 8918b7178e..56890a4ce2 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -52,7 +52,7 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); - EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -120,7 +120,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index a9a581d3bb..ebd268504c 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -22,6 +22,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller"; constexpr double INTERFACE_VALUE = 1989.0; @@ -31,6 +33,9 @@ constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0; constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; + class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface { @@ -82,15 +87,16 @@ class TestableChainableControllerInterface { std::vector command_interfaces; - command_interfaces.push_back(hardware_interface::CommandInterface( - name_prefix_of_interfaces_, "test_itf", &reference_interfaces_[0])); + command_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription( + name_prefix_of_reference_interfaces_, + InterfaceInfo(ref_itf_name_, std::to_string(INTERFACE_VALUE), "double")))); return command_interfaces; } bool on_set_chained_mode(bool /*chained_mode*/) override { - if (reference_interfaces_[0] == 0.0) + if (reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == 0.0) { state_interfaces_values_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; return true; @@ -104,40 +110,52 @@ class TestableChainableControllerInterface controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (reference_interface_value_ == INTERFACE_VALUE_SUBSCRIBER_ERROR) + if ( + reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == + INTERFACE_VALUE_SUBSCRIBER_ERROR) { return controller_interface::return_type::ERROR; } - reference_interfaces_[0] = reference_interface_value_; + reference_interfaces_ptrs_[ref_itf_full_name_]->set_value(reference_interface_value_); return controller_interface::return_type::OK; } controller_interface::return_type update_and_write_commands( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (reference_interfaces_[0] == INTERFACE_VALUE_UPDATE_ERROR) + if ( + reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == + INTERFACE_VALUE_UPDATE_ERROR) { return controller_interface::return_type::ERROR; } - reference_interfaces_[0] -= 1; - state_interfaces_values_[0] += 1; + reference_interfaces_ptrs_[ref_itf_full_name_]->operator-=(1); return controller_interface::return_type::OK; } void set_name_prefix_of_reference_interfaces(const std::string & prefix) { - name_prefix_of_interfaces_ = prefix; + name_prefix_of_reference_interfaces_ = prefix; + update_ref_itf_full_name(); } void set_new_reference_interface_value(const double ref_itf_value) { reference_interface_value_ = ref_itf_value; + update_ref_itf_full_name(); + } + + void update_ref_itf_full_name() + { + ref_itf_full_name_ = name_prefix_of_reference_interfaces_ + "/" + ref_itf_name_; } std::string name_prefix_of_interfaces_; + std::string ref_itf_name_ = "test_itf"; + std::string ref_itf_full_name_; double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF; }; diff --git a/controller_interface/test/test_force_torque_sensor.cpp b/controller_interface/test/test_force_torque_sensor.cpp index dd1e55e126..e60ec5c987 100644 --- a/controller_interface/test/test_force_torque_sensor.cpp +++ b/controller_interface/test/test_force_torque_sensor.cpp @@ -47,20 +47,42 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_y; + fts_info_y.name = fts_interface_names_[1]; + fts_info_y.initial_value = std::to_string(force_values_[1]); + hardware_interface::InterfaceDescription fts_descr_y(sensor_name_, fts_info_y); + hardware_interface::StateInterface force_y{fts_descr_y}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_x; + trq_info_x.name = fts_interface_names_[3]; + trq_info_x.initial_value = std::to_string(torque_values_[0]); + hardware_interface::InterfaceDescription torque_descr_x(sensor_name_, trq_info_x); + hardware_interface::StateInterface torque_x{torque_descr_x}; + + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; @@ -131,16 +153,30 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force.x and force.z - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque.y and torque.z - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; @@ -213,20 +249,42 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names) ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z"); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_y; + fts_info_y.name = fts_interface_names_[1]; + fts_info_y.initial_value = std::to_string(force_values_[1]); + hardware_interface::InterfaceDescription fts_descr_y(sensor_name_, fts_info_y); + hardware_interface::StateInterface force_y{fts_descr_y}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_x; + trq_info_x.name = fts_interface_names_[3]; + trq_info_x.initial_value = std::to_string(torque_values_[0]); + hardware_interface::InterfaceDescription torque_descr_x(sensor_name_, trq_info_x); + hardware_interface::StateInterface torque_x{torque_descr_x}; + + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_imu_sensor.cpp b/controller_interface/test/test_imu_sensor.cpp index 1fbf205238..ca0b10b251 100644 --- a/controller_interface/test/test_imu_sensor.cpp +++ b/controller_interface/test/test_imu_sensor.cpp @@ -46,30 +46,67 @@ TEST_F(IMUSensorTest, validate_all) std::vector interface_names = imu_sensor_->get_state_interface_names(); // assign values to orientation - hardware_interface::StateInterface orientation_x{ - sensor_name_, imu_interface_names_[0], &orientation_values_[0]}; - hardware_interface::StateInterface orientation_y{ - sensor_name_, imu_interface_names_[1], &orientation_values_[1]}; - hardware_interface::StateInterface orientation_z{ - sensor_name_, imu_interface_names_[2], &orientation_values_[2]}; - hardware_interface::StateInterface orientation_w{ - sensor_name_, imu_interface_names_[3], &orientation_values_[4]}; + hardware_interface::InterfaceInfo orientation_x_info; + orientation_x_info.name = imu_interface_names_[0]; + orientation_x_info.initial_value = std::to_string(orientation_values_[0]); + hardware_interface::InterfaceDescription orientation_x_descr(sensor_name_, orientation_x_info); + hardware_interface::StateInterface orientation_x{orientation_x_descr}; + + hardware_interface::InterfaceInfo orientation_y_info; + orientation_y_info.name = imu_interface_names_[1]; + orientation_y_info.initial_value = std::to_string(orientation_values_[1]); + hardware_interface::InterfaceDescription orientation_y_descr(sensor_name_, orientation_y_info); + hardware_interface::StateInterface orientation_y{orientation_y_descr}; + + hardware_interface::InterfaceInfo orientation_z_info; + orientation_z_info.name = imu_interface_names_[2]; + orientation_z_info.initial_value = std::to_string(orientation_values_[2]); + hardware_interface::InterfaceDescription orientation_z_descr(sensor_name_, orientation_z_info); + hardware_interface::StateInterface orientation_z{orientation_z_descr}; + + hardware_interface::InterfaceInfo orientation_w_info; + orientation_w_info.name = imu_interface_names_[3]; + orientation_w_info.initial_value = std::to_string(orientation_values_[3]); + hardware_interface::InterfaceDescription orientation_w_descr(sensor_name_, orientation_w_info); + hardware_interface::StateInterface orientation_w{orientation_w_descr}; // assign values to angular velocity - hardware_interface::StateInterface angular_velocity_x{ - sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]}; - hardware_interface::StateInterface angular_velocity_y{ - sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]}; - hardware_interface::StateInterface angular_velocity_z{ - sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]}; + hardware_interface::InterfaceInfo angular_x_info; + angular_x_info.name = imu_interface_names_[4]; + angular_x_info.initial_value = std::to_string(angular_velocity_values_[0]); + hardware_interface::InterfaceDescription angular_x_descr(sensor_name_, angular_x_info); + hardware_interface::StateInterface angular_velocity_x{angular_x_descr}; + + hardware_interface::InterfaceInfo angular_y_info; + angular_y_info.name = imu_interface_names_[5]; + angular_y_info.initial_value = std::to_string(angular_velocity_values_[1]); + hardware_interface::InterfaceDescription angular_y_descr(sensor_name_, angular_y_info); + hardware_interface::StateInterface angular_velocity_y{angular_y_descr}; + + hardware_interface::InterfaceInfo angular_z_info; + angular_z_info.name = imu_interface_names_[6]; + angular_z_info.initial_value = std::to_string(angular_velocity_values_[2]); + hardware_interface::InterfaceDescription angular_z_descr(sensor_name_, angular_z_info); + hardware_interface::StateInterface angular_velocity_z{angular_z_descr}; // assign values to linear acceleration - hardware_interface::StateInterface linear_acceleration_x{ - sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]}; - hardware_interface::StateInterface linear_acceleration_y{ - sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]}; - hardware_interface::StateInterface linear_acceleration_z{ - sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]}; + hardware_interface::InterfaceInfo linear_x_info; + linear_x_info.name = imu_interface_names_[7]; + linear_x_info.initial_value = std::to_string(linear_acceleration_values_[0]); + hardware_interface::InterfaceDescription linear_x_descr(sensor_name_, linear_x_info); + hardware_interface::StateInterface linear_acceleration_x{linear_x_descr}; + + hardware_interface::InterfaceInfo linear_y_info; + linear_y_info.name = imu_interface_names_[8]; + linear_y_info.initial_value = std::to_string(linear_acceleration_values_[1]); + hardware_interface::InterfaceDescription linear_y_descr(sensor_name_, linear_y_info); + hardware_interface::StateInterface linear_acceleration_y{linear_y_descr}; + + hardware_interface::InterfaceInfo linear_z_info; + linear_z_info.name = imu_interface_names_[9]; + linear_z_info.initial_value = std::to_string(linear_acceleration_values_[2]); + hardware_interface::InterfaceDescription linear_z_descr(sensor_name_, linear_z_info); + hardware_interface::StateInterface linear_acceleration_z{linear_z_descr}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_semantic_component_interface.cpp b/controller_interface/test/test_semantic_component_interface.cpp index f81b4f64fe..a1b0e90cdf 100644 --- a/controller_interface/test/test_semantic_component_interface.cpp +++ b/controller_interface/test/test_semantic_component_interface.cpp @@ -94,9 +94,23 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces) std::vector interface_values = {1.1, 3.3, 5.5}; // assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5 - hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]}; - hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]}; - hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]}; + hardware_interface::InterfaceInfo info_1; + info_1.name = "1"; + info_1.initial_value = std::to_string(interface_values[0]); + hardware_interface::InterfaceDescription interface_1_decr(component_name_, info_1); + hardware_interface::StateInterface interface_1{interface_1_decr}; + + hardware_interface::InterfaceInfo info_3; + info_3.name = "3"; + info_3.initial_value = std::to_string(interface_values[1]); + hardware_interface::InterfaceDescription interface_3_decr(component_name_, info_3); + hardware_interface::StateInterface interface_3{interface_3_decr}; + + hardware_interface::InterfaceInfo info_5; + info_5.name = "5"; + info_5.initial_value = std::to_string(interface_values[2]); + hardware_interface::InterfaceDescription interface_5_decr(component_name_, info_5); + hardware_interface::StateInterface interface_5{interface_5_decr}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index e43f2a13a1..df270aeb35 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -18,10 +18,16 @@ #include #include +#include "hardware_interface/hardware_info.hpp" + #include "lifecycle_msgs/msg/state.hpp" namespace test_chainable_controller { + +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; + TestChainableController::TestChainableController() : controller_interface::ChainableControllerInterface(), cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE}, @@ -192,8 +198,8 @@ TestChainableController::on_export_reference_interfaces() for (size_t i = 0; i < reference_interface_names_.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); + reference_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription( + get_node()->get_name(), InterfaceInfo(reference_interface_names_[i], "double")))); } return reference_interfaces; diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 6d96b99cc5..6f318f8e09 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -181,13 +181,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -195,13 +197,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -461,7 +465,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod double get_state(const std::string & interface_name) const { - return actuator_states_.at(interface_name)->get_value(); + return actuator_states_.at(interface_name)->get_value(); } void set_command(const std::string & interface_name, const double & value) @@ -471,37 +475,52 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod double get_command(const std::string & interface_name) const { - return actuator_commands_.at(interface_name)->get_value(); + return actuator_commands_.at(interface_name)->get_value(); } - void set_emergency_stop(const double & emergency_stop) + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); } - double get_emergency_stop() const { return emergency_stop_->get_value(); } + bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - double get_error_code() const { return error_signal_->get_value(); } + std::vector get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message(std::vector error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::vector get_error_message() const + { + return error_signal_message_->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::vector warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::vector get_warning_code() const + { + return warning_signal_->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::vector get_warning_message() const + { + return warning_signal_message_->get_value>(); + } protected: /// Get the logger of the ActuatorInterface. diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 16d06c578d..97f4c3f167 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,63 +15,69 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ -#include #include #include #include +#include #include #include +#include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" -#include "hardware_interface/types/hardware_interface_error_signals.hpp" -#include "hardware_interface/types/hardware_interface_warning_signals.hpp" -#include "hardware_interface/visibility_control.h" +#include "hardware_interface/types/handle_datatype.hpp" + namespace hardware_interface { -typedef std::variant HANDLE_DATATYPE; - /// A handle used to get and set a value on a given interface. class Handle { public: - [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - Handle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) - { - } - explicit Handle(const InterfaceDescription & interface_description) : prefix_name_(interface_description.prefix_name), interface_name_(interface_description.interface_info.name) { - // As soon as multiple datatypes are used in HANDLE_DATATYPE - // we need to initialize according the type passed in interface description - value_ = std::numeric_limits::quiet_NaN(); - value_ptr_ = std::get_if(&value_); + init_handle_value(interface_description.interface_info); } - [[deprecated("Use InterfaceDescription for initializing the Interface")]] + Handle() = delete; - explicit Handle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + Handle(const Handle & other) = default; + + Handle(Handle && other) = default; + + /// Returns true if handle is valid. We define valid if the handle has a non empty prefix name and + /// a non empty interface name. This allows us to construct empty non valid default Handles which + /// can later be assigned e.g. in Transmissions + inline operator bool() const { + return !( + prefix_name_.empty() || interface_name_.empty() || + std::holds_alternative(value_)); } - [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - explicit Handle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + template < + typename T, typename std::enable_if< + std::is_integral::value || std::is_floating_point::value, int>::type = 0> + void operator+=(const T & value) { + value_ = std::get(value_) + value; } - Handle(const Handle & other) = default; + template < + typename T, typename std::enable_if< + std::is_integral::value || std::is_floating_point::value, int>::type = 0> + void operator-=(const T & value) + { + value_ = std::get(value_) - value; + } - Handle(Handle && other) = default; + template ::value, int>::type = 0> + void operator=(const T & value) + { + value_ = value; + } Handle & operator=(const Handle & other) = default; @@ -79,9 +85,6 @@ class Handle virtual ~Handle() = default; - /// Returns true if handle references a value. - inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } const std::string & get_interface_name() const { return interface_name_; } @@ -95,32 +98,102 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } - double get_value() const + template ::value, int>::type = 0> + T get_value() const { - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) return value_ if old functionality is removed - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; - // END + return std::get(value_); } - void set_value(double value) + template ::value, int>::type = 0> + void set_value(T value) { - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) set value_ directly if old functionality is removed - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; - // END + value_ = value; } protected: + // used for the + bool correct_vector_size(const size_t & expected, const size_t & actual) + { + return expected == actual; + } + + void init_handle_value(const InterfaceInfo & interface_info) + { + if (interface_info.data_type == "bool") + { + value_ = interface_info.initial_value.empty() ? false + : (interface_info.initial_value == "true" || + interface_info.initial_value == "True"); + } + else if (interface_info.data_type == "int") + { + value_ = interface_info.initial_value.empty() ? 0 : std::stoi(interface_info.initial_value); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + value_ = std::vector(hardware_interface::warning_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if (interface_info.size != 0 && hardware_interface::error_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::error_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::error_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::warning_signal_count, ""); + } + else if (interface_info.data_type == "double") + { + value_ = interface_info.initial_value.empty() ? std::numeric_limits::quiet_NaN() + : std::stod(interface_info.initial_value); + } + // Default for empty is std::monostate + else if (interface_info.data_type.empty()) + { + value_ = {std::monostate()}; + } + // If not empty and it belongs to none of the above types, we still want to throw as there might + // be a typo in the data_type like "bol" or user wants some unsupported type + else + { + throw std::runtime_error( + "The data_type:{" + interface_info.data_type + "} for the InterfaceInfo with name:{" + + interface_info.name + + "} is not supported for Handles. Supported data_types are: bool, double, vector, " + "vector and vector."); + } + } + std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed - double * value_ptr_; - // END }; class StateInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index bf14f24041..04dccd74ac 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -29,6 +29,37 @@ namespace hardware_interface */ struct InterfaceInfo { + // Add default constructor, so that e.g. size is initialized to sensible value + InterfaceInfo() + { + // cpp_lint complains about min and max include otherwise + name = ""; + min = ""; + max = ""; + initial_value = ""; + data_type = ""; + size = 0; + } + + explicit InterfaceInfo(const std::string & name_in) : InterfaceInfo() { name = name_in; } + + explicit InterfaceInfo(const std::string & name_in, const std::string & data_type_in) + : InterfaceInfo() + { + name = name_in; + data_type = data_type_in; + } + + explicit InterfaceInfo( + const std::string & name_in, const std::string & initial_value_in, + const std::string & data_type_in) + : InterfaceInfo() + { + name = name_in; + initial_value = initial_value_in; + data_type = data_type_in; + } + /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -137,6 +168,12 @@ struct InterfaceDescription { } + explicit InterfaceDescription( + const std::string & prefix_name_in, const std::string & interface_info_name) + : prefix_name(prefix_name_in), interface_info(interface_info_name) + { + } + /** * Name of the interface defined by the user. */ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..6fc445ac29 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -65,7 +65,7 @@ class LoanedCommandInterface void set_value(double val) { command_interface_.set_value(val); } - double get_value() const { return command_interface_.get_value(); } + double get_value() const { return command_interface_.get_value(); } protected: CommandInterface & command_interface_; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..11abae41d0 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -63,7 +63,7 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const { return state_interface_.get_value(); } protected: StateInterface & state_interface_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index e4b45916f7..ab27a5c718 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -158,13 +158,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -172,13 +174,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -311,30 +315,45 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_state(const std::string & interface_name) const { - return sensor_states_.at(interface_name)->get_value(); + return sensor_states_.at(interface_name)->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - double get_error_code() const { return error_signal_->get_value(); } + std::vector get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message(std::vector error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::vector get_error_message() const + { + return error_signal_message_->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::vector warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::vector get_warning_code() const + { + return warning_signal_->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::vector get_warning_message() const + { + return warning_signal_message_->get_value>(); + } protected: /// Get the logger of the SensorInterface. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 07c9ef9984..4871123664 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -201,13 +201,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -215,13 +217,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -493,7 +497,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return state. */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } - void set_state(const std::string & interface_name, const double & value) { system_states_.at(interface_name)->set_value(value); @@ -501,7 +504,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_state(const std::string & interface_name) const { - return system_states_.at(interface_name)->get_value(); + return system_states_.at(interface_name)->get_value(); } void set_command(const std::string & interface_name, const double & value) @@ -511,37 +514,52 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_command(const std::string & interface_name) const { - return system_commands_.at(interface_name)->get_value(); + return system_commands_.at(interface_name)->get_value(); } - void set_emergency_stop(const double & emergency_stop) + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); } - double get_emergency_stop() const { return emergency_stop_->get_value(); } + bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - double get_error_code() const { return error_signal_->get_value(); } + std::vector get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message(std::vector error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::vector get_error_message() const + { + return error_signal_message_->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::vector warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::vector get_warning_code() const + { + return warning_signal_->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::vector get_warning_message() const + { + return warning_signal_message_->get_value>(); + } protected: /// Get the logger of the SystemInterface. diff --git a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp new file mode 100644 index 0000000000..0b84f6262f --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp @@ -0,0 +1,48 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ +#define HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" + +namespace hardware_interface +{ + +// std::monostate is that we have a well defined alternative empty default initialization +// !!! IF YOU ADD TYPES TO HANDLE_DATATYPE, std::monostate MUST ALWAYS REMAIN AT THE FIRST POSITION +// This i needed so that e.g.: HANDLE_DATATYPE our_variant = {}; -> defaults to std::monostate +using HANDLE_DATATYPE = std::variant< + std::monostate, bool, int, double, std::vector, std::vector, + std::vector>; + +// Define a type trait for allowed types +template +struct HANDLE_DATATYPE_TYPES +: std::disjunction< + std::is_same, std::is_same, std::is_same, + std::is_same>, std::is_same>, + std::is_same>> +{ +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp index d5159f29c8..b40011eb3a 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ #include +#include #include namespace hardware_interface @@ -23,82 +24,12 @@ namespace hardware_interface // Count of how many different error signals there are that can be reported. const size_t error_signal_count = 32; +using ERROR_SIGNALS = std::vector; constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; -// Available error signal names -enum class error_signal : std::uint8_t -{ - ERROR_SIGNAL_0 = 0, - ERROR_SIGNAL_1 = 1, - ERROR_SIGNAL_2 = 2, - ERROR_SIGNAL_3 = 3, - ERROR_SIGNAL_4 = 4, - ERROR_SIGNAL_5 = 5, - ERROR_SIGNAL_6 = 6, - ERROR_SIGNAL_7 = 7, - ERROR_SIGNAL_8 = 8, - ERROR_SIGNAL_9 = 9, - ERROR_SIGNAL_10 = 10, - ERROR_SIGNAL_11 = 11, - ERROR_SIGNAL_12 = 12, - ERROR_SIGNAL_13 = 13, - ERROR_SIGNAL_14 = 14, - ERROR_SIGNAL_15 = 15, - ERROR_SIGNAL_16 = 16, - ERROR_SIGNAL_17 = 17, - ERROR_SIGNAL_18 = 18, - ERROR_SIGNAL_19 = 19, - ERROR_SIGNAL_20 = 20, - ERROR_SIGNAL_21 = 21, - ERROR_SIGNAL_22 = 22, - ERROR_SIGNAL_23 = 23, - ERROR_SIGNAL_24 = 24, - ERROR_SIGNAL_25 = 25, - ERROR_SIGNAL_26 = 26, - ERROR_SIGNAL_27 = 27, - ERROR_SIGNAL_28 = 28, - ERROR_SIGNAL_29 = 29, - ERROR_SIGNAL_30 = 30, - ERROR_SIGNAL_31 = 31 -}; +using ERROR_MESSAGES = std::vector; constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; -// Available WARNING signal message names -enum class error_signal_message : std::uint8_t -{ - ERROR_SIGNAL_MESSAGE_0 = 0, - ERROR_SIGNAL_MESSAGE_1 = 1, - ERROR_SIGNAL_MESSAGE_2 = 2, - ERROR_SIGNAL_MESSAGE_3 = 3, - ERROR_SIGNAL_MESSAGE_4 = 4, - ERROR_SIGNAL_MESSAGE_5 = 5, - ERROR_SIGNAL_MESSAGE_6 = 6, - ERROR_SIGNAL_MESSAGE_7 = 7, - ERROR_SIGNAL_MESSAGE_8 = 8, - ERROR_SIGNAL_MESSAGE_9 = 9, - ERROR_SIGNAL_MESSAGE_10 = 10, - ERROR_SIGNAL_MESSAGE_11 = 11, - ERROR_SIGNAL_MESSAGE_12 = 12, - ERROR_SIGNAL_MESSAGE_13 = 13, - ERROR_SIGNAL_MESSAGE_14 = 14, - ERROR_SIGNAL_MESSAGE_15 = 15, - ERROR_SIGNAL_MESSAGE_16 = 16, - ERROR_SIGNAL_MESSAGE_17 = 17, - ERROR_SIGNAL_MESSAGE_18 = 18, - ERROR_SIGNAL_MESSAGE_19 = 19, - ERROR_SIGNAL_MESSAGE_20 = 20, - ERROR_SIGNAL_MESSAGE_21 = 21, - ERROR_SIGNAL_MESSAGE_22 = 22, - ERROR_SIGNAL_MESSAGE_23 = 23, - ERROR_SIGNAL_MESSAGE_24 = 24, - ERROR_SIGNAL_MESSAGE_25 = 25, - ERROR_SIGNAL_MESSAGE_26 = 26, - ERROR_SIGNAL_MESSAGE_27 = 27, - ERROR_SIGNAL_MESSAGE_28 = 28, - ERROR_SIGNAL_MESSAGE_29 = 29, - ERROR_SIGNAL_MESSAGE_30 = 30, - ERROR_SIGNAL_MESSAGE_31 = 31 -}; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp index 58af04b80a..cf4b5a8c0e 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ #include +#include #include namespace hardware_interface @@ -23,81 +24,11 @@ namespace hardware_interface // Count of how many different warn signals there are that can be reported. const size_t warning_signal_count = 32; +using WARNING_SIGNALS = std::vector; constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; -// Available warning signals names mapping to position in the interface -enum class warning_signal : std::uint8_t -{ - WARNING_SIGNAL_0 = 0, - WARNING_SIGNAL_1 = 1, - WARNING_SIGNAL_2 = 2, - WARNING_SIGNAL_3 = 3, - WARNING_SIGNAL_4 = 4, - WARNING_SIGNAL_5 = 5, - WARNING_SIGNAL_6 = 6, - WARNING_SIGNAL_7 = 7, - WARNING_SIGNAL_8 = 8, - WARNING_SIGNAL_9 = 9, - WARNING_SIGNAL_10 = 10, - WARNING_SIGNAL_11 = 11, - WARNING_SIGNAL_12 = 12, - WARNING_SIGNAL_13 = 13, - WARNING_SIGNAL_14 = 14, - WARNING_SIGNAL_15 = 15, - WARNING_SIGNAL_16 = 16, - WARNING_SIGNAL_17 = 17, - WARNING_SIGNAL_18 = 18, - WARNING_SIGNAL_19 = 19, - WARNING_SIGNAL_20 = 20, - WARNING_SIGNAL_21 = 21, - WARNING_SIGNAL_22 = 22, - WARNING_SIGNAL_23 = 23, - WARNING_SIGNAL_24 = 24, - WARNING_SIGNAL_25 = 25, - WARNING_SIGNAL_26 = 26, - WARNING_SIGNAL_27 = 27, - WARNING_SIGNAL_28 = 28, - WARNING_SIGNAL_29 = 29, - WARNING_SIGNAL_30 = 30, - WARNING_SIGNAL_31 = 31 -}; +using WARNING_MESSAGES = std::vector; constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; -// Available WARNING signal message names -enum class warning_signal_message : std::uint8_t -{ - WARNING_SIGNAL_MESSAGE_0 = 0, - WARNING_SIGNAL_MESSAGE_1 = 1, - WARNING_SIGNAL_MESSAGE_2 = 2, - WARNING_SIGNAL_MESSAGE_3 = 3, - WARNING_SIGNAL_MESSAGE_4 = 4, - WARNING_SIGNAL_MESSAGE_5 = 5, - WARNING_SIGNAL_MESSAGE_6 = 6, - WARNING_SIGNAL_MESSAGE_7 = 7, - WARNING_SIGNAL_MESSAGE_8 = 8, - WARNING_SIGNAL_MESSAGE_9 = 9, - WARNING_SIGNAL_MESSAGE_10 = 10, - WARNING_SIGNAL_MESSAGE_11 = 11, - WARNING_SIGNAL_MESSAGE_12 = 12, - WARNING_SIGNAL_MESSAGE_13 = 13, - WARNING_SIGNAL_MESSAGE_14 = 14, - WARNING_SIGNAL_MESSAGE_15 = 15, - WARNING_SIGNAL_MESSAGE_16 = 16, - WARNING_SIGNAL_MESSAGE_17 = 17, - WARNING_SIGNAL_MESSAGE_18 = 18, - WARNING_SIGNAL_MESSAGE_19 = 19, - WARNING_SIGNAL_MESSAGE_20 = 20, - WARNING_SIGNAL_MESSAGE_21 = 21, - WARNING_SIGNAL_MESSAGE_22 = 22, - WARNING_SIGNAL_MESSAGE_23 = 23, - WARNING_SIGNAL_MESSAGE_24 = 24, - WARNING_SIGNAL_MESSAGE_25 = 25, - WARNING_SIGNAL_MESSAGE_26 = 26, - WARNING_SIGNAL_MESSAGE_27 = 27, - WARNING_SIGNAL_MESSAGE_28 = 28, - WARNING_SIGNAL_MESSAGE_29 = 29, - WARNING_SIGNAL_MESSAGE_30 = 30, - WARNING_SIGNAL_MESSAGE_31 = 31 -}; } // namespace hardware_interface diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 7987780ba0..d1861991ff 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -18,6 +18,8 @@ #define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ #include +#include +#include #include #include "hardware_interface/handle.hpp" @@ -42,9 +44,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste public: CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + std::vector export_command_interfaces_2() override; return_type prepare_command_mode_switch( const std::vector & start_interfaces, @@ -72,54 +72,43 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste const std::vector standard_interfaces_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; + // added dynamically during on_init + std::vector non_standard_interfaces_; - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_commands_; - std::vector> joint_states_; - - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_commands_; - std::vector> other_states_; - - std::vector sensor_interfaces_; - /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_mock_commands_; - std::vector> sensor_states_; - - std::vector gpio_interfaces_; - /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_mock_commands_; - std::vector> gpio_commands_; - std::vector> gpio_states_; + struct MimicJoint + { + std::string joint_name; + std::string mimic_joint_name; + double multiplier = 1.0; + }; + std::vector mimic_joints_; + + // All the joints that are of type defined by standard_interfaces_ vector -> In {pos, vel, acc, + // effort} + std::vector std_joint_names_; + std::unordered_set std_joint_command_interface_names_; + std::unordered_set std_joint_state_interface_names_; + + // All the joints that are of not of a type defined by standard_interfaces_ vector -> Not in {pos, + // vel, acc, effort} + std::vector other_joint_names_; + std::unordered_set other_joint_command_interface_names_; + std::unordered_set other_joint_state_interface_names_; private: - template - bool get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces); - - void initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos); - - template - bool populate_interfaces( + void search_and_add_interface_names( const std::vector & components, - std::vector & interfaces, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces); + const std::vector & interface_list, std::vector & vector_to_add); bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; std::string custom_interface_with_following_offset_; - size_t index_custom_interface_with_following_offset_; + std::string custom_interface_name_with_following_offset_; bool calculate_dynamics_; - std::vector joint_control_mode_; + std::unordered_map joint_control_mode_; bool command_propagation_disabled_; }; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index abe36ba262..df7e1d71d3 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -57,7 +57,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } }; - // check if to create mock command interface for sensor auto it = info_.hardware_parameters.find("mock_sensor_commands"); if (it != info_.hardware_parameters.end()) @@ -117,49 +116,90 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i custom_interface_with_following_offset_ = it->second; } } - // it's extremely improbable that std::distance results int this value - therefore default - index_custom_interface_with_following_offset_ = std::numeric_limits::max(); - - // Initialize storage for standard interfaces - initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints); - // set all values without initial values to 0 - for (auto i = 0u; i < info_.joints.size(); i++) - { - for (auto j = 0u; j < standard_interfaces_.size(); j++) - { - if (std::isnan(joint_states_[j][i])) - { - joint_states_[j][i] = 0.0; - } - } - } // search for non-standard joint interfaces for (const auto & joint : info_.joints) { // populate non-standard command interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); + populate_non_standard_interfaces(joint.command_interfaces, non_standard_interfaces_); // populate non-standard state interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_); + populate_non_standard_interfaces(joint.state_interfaces, non_standard_interfaces_); } - // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); + // search for standard joint interfaces and add them to std_joint_names_ and + // search for non-standard joint interfaces and add them to other_joint_names_ + for (const auto & joint : info.joints) + { + for (const auto & state_inteface : joint.state_interfaces) + { + if ( + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), state_inteface.name) == + standard_interfaces_.end()) + { + std_joint_names_.push_back(joint.name); + std_joint_state_interface_names_.insert(joint.name + "/" + state_inteface.name); + } + else if ( + std::find( + non_standard_interfaces_.begin(), non_standard_interfaces_.end(), state_inteface.name) == + non_standard_interfaces_.end()) + { + other_joint_names_.push_back(joint.name); + other_joint_state_interface_names_.insert(joint.name + "/" + state_inteface.name); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "The state_interface: '%s' of the joint: '%s' is neither part of the std_interfaces " + "(pos, vel, acc, eff), nor of any other use " + "defined.", + state_inteface.name.c_str(), joint.name.c_str()); + } + } + + // Check that for all the available state_interfaces a command_interface exists + // We don't need to add name of the joint again since it has already been added for the + // state_interface + for (const auto & command_interface : joint.command_interfaces) + { + if ( + std::find( + std_joint_state_interface_names_.begin(), std_joint_state_interface_names_.end(), + command_interface.name) == std_joint_state_interface_names_.end()) + { + std_joint_command_interface_names_.insert(joint.name + "/" + command_interface.name); + } + else if ( + std::find( + other_joint_state_interface_names_.begin(), other_joint_state_interface_names_.end(), + command_interface.name) == other_joint_state_interface_names_.end()) + { + other_joint_command_interface_names_.insert(joint.name + "/" + command_interface.name); + } + else + { + throw std::runtime_error( + std::string("For command_interface: '") + command_interface.name + "' of the joint: '" + + joint.name + "' exists no state_interface"); + } + } + } // when following offset is used on custom interface then find its index + custom_interface_name_with_following_offset_ = ""; if (!custom_interface_with_following_offset_.empty()) { auto if_it = std::find( - other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_); - if (if_it != other_interfaces_.end()) + non_standard_interfaces_.begin(), non_standard_interfaces_.end(), + custom_interface_with_following_offset_); + if (if_it != non_standard_interfaces_.end()) { - index_custom_interface_with_following_offset_ = - std::distance(other_interfaces_.begin(), if_it); - RCLCPP_INFO( - get_logger(), "Custom interface with following offset '%s' found at index: %zu.", + custom_interface_name_with_following_offset_ = *if_it; + RCUTILS_LOG_INFO_NAMED( + "mock_generic_system", "Custom interface with following offset '%s' found at index: %s.", custom_interface_with_following_offset_.c_str(), - index_custom_interface_with_following_offset_); + custom_interface_name_with_following_offset_.c_str()); } else { @@ -170,149 +210,79 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - for (const auto & sensor : info_.sensors) + // Search for mimic joints + for (auto i = 0u; i < info_.joints.size(); ++i) { - for (const auto & interface : sensor.state_interfaces) + const auto & joint = info_.joints.at(i); + if (joint.parameters.find("mimic") != joint.parameters.cend()) { - if ( - std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) == - sensor_interfaces_.end()) + const auto mimicked_joint_it = std::find_if( + info_.joints.begin(), info_.joints.end(), + [&mimicked_joint = + joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) + { return joint_info.name == mimicked_joint; }); + if (mimicked_joint_it == info_.joints.cend()) { - sensor_interfaces_.emplace_back(interface.name); + throw std::runtime_error( + std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); } - } - } - initialize_storage_vectors( - sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); - - // search for gpio interfaces - for (const auto & gpio : info_.gpios) - { - // populate non-standard command interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); - - // populate non-standard state interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.state_interfaces, gpio_interfaces_); - } - - // Mock gpio command interfaces - if (use_mock_gpio_command_interfaces_) - { - initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); - } - // Real gpio command interfaces - else - { - initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); - } - - return CallbackReturn::SUCCESS; -} - -std::vector GenericSystem::export_state_interfaces() -{ - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.state_interfaces) - { - // Add interface: if not in the standard list then use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces)) + MimicJoint mimic_joint; + mimic_joint.joint_name = joint.name; + mimic_joint.mimic_joint_name = mimicked_joint_it->name; + auto param_it = joint.parameters.find("multiplier"); + if (param_it != joint.parameters.end()) { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); } + mimic_joints_.push_back(mimic_joint); } } - // Sensor state interfaces - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - }; - - // GPIO state interfaces - if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) + // set all values without initial values to 0 + for (auto [name, descr] : joint_state_interfaces_) { - throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); + if (std::isnan(get_state(name))) + { + set_state(name, 0.0); + } } - return state_interfaces; + return CallbackReturn::SUCCESS; } -std::vector GenericSystem::export_command_interfaces() +std::vector GenericSystem::export_command_interfaces_2() { - std::vector command_interfaces; - - // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.command_interfaces) - { - // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_commands_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_commands_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - // Set position control mode per default - joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); - + // we check if we should mock command interfaces or not. After they have been exported we can then + // use them as we would normally via (set/get)_(state/command) + std::vector descriptions; // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) + for (const auto & sensor : info_.sensors) { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); + for (const auto & state_interface : sensor.state_interfaces) + { + hardware_interface::InterfaceInfo info = state_interface; + hardware_interface::InterfaceDescription descr(sensor.name, info); + descriptions.push_back(descr); + } } } // Mock gpio command interfaces (consider all state interfaces for command interfaces) if (use_mock_gpio_command_interfaces_) { - if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) + for (const auto & gpio : info_.gpios) { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } - } - // GPIO command interfaces (real command interfaces) - else - { - if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); + for (const auto & state_interface : gpio.state_interfaces) + { + hardware_interface::InterfaceInfo info = state_interface; + hardware_interface::InterfaceDescription descr(gpio.name, info); + descriptions.push_back(descr); + } } } - - return command_interfaces; + return descriptions; } return_type GenericSystem::prepare_command_mode_switch( @@ -328,8 +298,8 @@ return_type GenericSystem::prepare_command_mode_switch( const size_t FOUND_ONCE_FLAG = 1000000; - std::vector joint_found_in_x_requests_; - joint_found_in_x_requests_.resize(info_.joints.size(), 0); + std::vector joint_found_in_x_requests; + joint_found_in_x_requests.resize(info_.joints.size(), 0); for (const auto & key : start_interfaces) { @@ -341,14 +311,14 @@ return_type GenericSystem::prepare_command_mode_switch( if (joint_it_found != info_.joints.end()) { const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); - if (joint_found_in_x_requests_[joint_index] == 0) + if (joint_found_in_x_requests[joint_index] == 0) { - joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; + joint_found_in_x_requests[joint_index] = FOUND_ONCE_FLAG; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { @@ -360,7 +330,7 @@ return_type GenericSystem::prepare_command_mode_switch( "might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); } - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { @@ -372,7 +342,7 @@ return_type GenericSystem::prepare_command_mode_switch( "this might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); } - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } } else @@ -385,7 +355,7 @@ return_type GenericSystem::prepare_command_mode_switch( for (size_t i = 0; i < info_.joints.size(); ++i) { // There has to always be at least one control mode from the above three set - if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) + if (joint_found_in_x_requests[i] == FOUND_ONCE_FLAG) { RCLCPP_ERROR( get_logger(), "Joint '%s' has to have '%s', '%s', or '%s' interface!", @@ -395,13 +365,13 @@ return_type GenericSystem::prepare_command_mode_switch( } // Currently we don't support multiple interface request - if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + if (joint_found_in_x_requests[i] > (FOUND_ONCE_FLAG + 1)) { RCLCPP_ERROR( get_logger(), "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", - joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + joint_found_in_x_requests[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); ret_val = hardware_interface::return_type::ERROR; } } @@ -431,15 +401,15 @@ return_type GenericSystem::perform_command_mode_switch( if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { - joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + joint_control_mode_[key] = POSITION_INTERFACE_INDEX; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { - joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + joint_control_mode_[key] = VELOCITY_INTERFACE_INDEX; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { - joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + joint_control_mode_[key] = ACCELERATION_INTERFACE_INDEX; } } } @@ -455,18 +425,20 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; } - auto mirror_command_to_state = - [](auto & states_, auto commands_, size_t start_index = 0) -> return_type + auto mirror_command_to_state = [this](const auto & joint_names, const auto & interface_names) { - for (size_t i = start_index; i < states_.size(); ++i) + for (const auto & joint_name : joint_names) { - for (size_t j = 0; j < states_[i].size(); ++j) + for (const auto & interface_name : interface_names) { - if (!std::isnan(commands_[i][j])) + const auto & joint_state_name = joint_name + "/" + interface_name; + if ( + this->std_joint_command_interface_names_.find(joint_state_name) != + this->std_joint_command_interface_names_.end()) { - states_[i][j] = commands_[i][j]; + this->set_state(joint_state_name, this->get_command(joint_state_name)); } - if (std::isinf(commands_[i][j])) + if (std::isinf(this->get_command(joint_state_name))) { return return_type::ERROR; } @@ -475,219 +447,162 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; }; - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + auto mirror_all_available_commands_to_states = [this](const auto & interfaces) { - if (calculate_dynamics_) + for (const auto & [name, descr] : interfaces) { - switch (joint_control_mode_[j]) + // TODO(Manuel) should this be checked if interface exists??? + this->set_state(name, this->get_command(name)); + } + }; + + if (calculate_dynamics_) + { + for (const auto & joint_name : std_joint_names_) + { + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; + const auto joint_vel = joint_name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto joint_acc = joint_name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto joint_eff = joint_name + "/" + hardware_interface::HW_IF_EFFORT; + + switch (joint_control_mode_.at(joint_name)) { case ACCELERATION_INTERFACE_INDEX: { // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + // apply offset to positions only + const auto old_pos = get_state(joint_pos); + const auto new_pos = + get_state(joint_vel) * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + set_state(joint_pos, old_pos + new_pos); - joint_states_[VELOCITY_INTERFACE_INDEX][j] += - joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + const auto old_vel = get_state(joint_vel); + const auto new_vel = get_state(joint_acc) * period.seconds(); + set_state(joint_vel, old_vel + new_vel); - if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_acc) != + std_joint_command_interface_names_.end()) { - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + set_state(joint_acc, get_command(joint_acc)); } break; } case VELOCITY_INTERFACE_INDEX: { // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + const auto old_pos = get_state(joint_pos); + const auto new_pos = + get_state(joint_vel) * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + set_state(joint_pos, old_pos + new_pos); - if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_vel) != + std_joint_command_interface_names_.end()) { - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - joint_commands_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + const auto old_vel = get_state(joint_vel); + set_state(joint_vel, get_command(joint_vel)); + set_state(joint_acc, (get_state(joint_vel) - old_vel) / period.seconds()); } break; } case POSITION_INTERFACE_INDEX: { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end()) { - const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + const double old_pos = get_state(joint_pos); + const double old_vel = get_state(joint_vel); + + set_state( + joint_pos, get_command(joint_pos) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); + set_state(joint_vel, (get_state(joint_pos) - old_pos) / period.seconds()); + set_state(joint_acc, (get_state(joint_vel) - old_vel) / period.seconds()); } break; } } } - else + } + else + { + for (const auto & joint_name : std_joint_names_) { - for (size_t k = 0; k < joint_states_[POSITION_INTERFACE_INDEX].size(); ++k) + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; + if ( + std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end()) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][k])) - { - joint_states_[POSITION_INTERFACE_INDEX][k] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][k] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - } + set_state( + joint_pos, get_command(joint_pos) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); } } } // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, // velocity, and acceleration interface - if ( - mirror_command_to_state(joint_states_, joint_commands_, calculate_dynamics_ ? 3 : 1) != - return_type::OK) + if (calculate_dynamics_) { - return return_type::ERROR; + std::vector interfaces( + standard_interfaces_.begin() + 3, standard_interfaces_.end()); + mirror_command_to_state(std_joint_names_, interfaces); + } + else + { + std::vector interfaces( + standard_interfaces_.begin() + 1, standard_interfaces_.end()); + mirror_command_to_state(std_joint_names_, interfaces); } - for (const auto & mimic_joint : info_.mimic_joints) + for (const auto & mimic_joint : mimic_joints_) { - for (auto i = 0u; i < joint_states_.size(); ++i) - { - joint_states_[i][mimic_joint.joint_index] = - mimic_joint.offset + - mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; - } + set_state( + mimic_joint.joint_name, get_state(mimic_joint.mimic_joint_name) * mimic_joint.multiplier); } - for (size_t i = 0; i < other_states_.size(); ++i) + for (const auto & joint_name : other_joint_names_) { - for (size_t j = 0; j < other_states_[i].size(); ++j) + for (const auto & interface_name : non_standard_interfaces_) { + const auto joint_inteface = joint_name + "/" + interface_name; + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; if ( - i == index_custom_interface_with_following_offset_ && - !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + interface_name == custom_interface_name_with_following_offset_ && + (std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end())) { - other_states_[i][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_; + set_state(joint_inteface, get_command(joint_pos) + position_state_following_offset_); } - else if (!std::isnan(other_commands_[i][j])) + else if ( + other_joint_command_interface_names_.find(joint_inteface) != + other_joint_command_interface_names_.end()) { - other_states_[i][j] = other_commands_[i][j]; + set_state(joint_inteface, get_command(joint_inteface)); } } } + // do loopback on all sensor interfaces if (use_mock_sensor_command_interfaces_) { - mirror_command_to_state(sensor_states_, sensor_mock_commands_); + mirror_all_available_commands_to_states(sensor_state_interfaces_); } - if (use_mock_gpio_command_interfaces_) - { - mirror_command_to_state(gpio_states_, gpio_mock_commands_); - } - else - { - // do loopback on all gpio interfaces - mirror_command_to_state(gpio_states_, gpio_commands_); - } + // do loopback on all gpio interfaces + mirror_all_available_commands_to_states(gpio_state_interfaces_); return return_type::OK; } -// Private methods -template -bool GenericSystem::get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces) -{ - auto it = std::find(interface_list.begin(), interface_list.end(), interface_name); - if (it != interface_list.end()) - { - auto j = std::distance(interface_list.begin(), it); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; - } - return false; -} - -void GenericSystem::initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos) -{ - // Initialize storage for all joints, regardless of their existence - commands.resize(interfaces.size()); - states.resize(interfaces.size()); - for (auto i = 0u; i < interfaces.size(); i++) - { - commands[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - states[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - } - - // Initialize with values from URDF - for (auto i = 0u; i < component_infos.size(); i++) - { - const auto & component = component_infos[i]; - for (const auto & interface : component.state_interfaces) - { - auto it = std::find(interfaces.begin(), interfaces.end(), interface.name); - - // If interface name is found in the interfaces list - if (it != interfaces.end()) - { - auto index = std::distance(interfaces.begin(), it); - - // Check the initial_value param is used - if (!interface.initial_value.empty()) - { - states[index][i] = hardware_interface::stod(interface.initial_value); - } - } - } - } -} - -template -bool GenericSystem::populate_interfaces( - const std::vector & components, - std::vector & interface_names, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces) -{ - for (auto i = 0u; i < components.size(); i++) - { - const auto & component = components[i]; - const auto interfaces = - (using_state_interfaces) ? component.state_interfaces : component.command_interfaces; - for (const auto & interface : interfaces) - { - if (!get_interface( - component.name, interface_names, interface.name, i, storage, target_interfaces)) - { - return false; - } - } - } - - return true; -} } // namespace mock_components #include "pluginlib/class_list_macros.hpp" diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 5dc009b894..f92f2d75b4 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -58,115 +58,6 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -// BEGIN (Handle export change): for backward compatibility -class DummyActuator : public hardware_interface::ActuatorInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - position_state_ = 0.0; - velocity_state_ = 0.0; - - if (recoverable_error_happened_) - { - velocity_command_ = 0.0; - } - - read_calls_ = 0; - write_calls_ = 0; - - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read a position and a velocity - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_)); - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - // We can command in velocity - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_)); - - return command_interfaces; - } - - std::string get_name() const override { return "DummyActuator"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, state is getting propagated within write. - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++write_calls_; - if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - position_state_ += velocity_command_; - velocity_state_ = velocity_command_; - - return hardware_interface::return_type::OK; - } - - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override - { - velocity_state_ = 0; - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - double position_state_ = std::numeric_limits::quiet_NaN(); - double velocity_state_ = std::numeric_limits::quiet_NaN(); - double velocity_command_ = 0.0; - - // Helper variables to initiate error on read - unsigned int read_calls_ = 0; - unsigned int write_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummyActuatorDefault : public hardware_interface::ActuatorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -254,72 +145,6 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface bool recoverable_error_happened_ = false; }; -// BEGIN (Handle export change): for backward compatibility -class DummySensor : public hardware_interface::SensorInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - voltage_level_ = 0.0; - read_calls_ = 0; - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read some voltage level - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface("joint1", "voltage", &voltage_level_)); - - return state_interfaces; - } - - std::string get_name() const override { return "DummySensor"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, static value - voltage_level_ = voltage_level_hw_value_; - return hardware_interface::return_type::OK; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - double voltage_level_ = std::numeric_limits::quiet_NaN(); - double voltage_level_hw_value_ = 0x666; - - // Helper variables to initiate error on read - int read_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummySensorDefault : public hardware_interface::SensorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -382,143 +207,6 @@ class DummySensorDefault : public hardware_interface::SensorInterface int read_calls_ = 0; bool recoverable_error_happened_ = false; }; - -// BEGIN (Handle export change): for backward compatibility -class DummySystem : public hardware_interface::SystemInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - for (auto i = 0ul; i < 3; ++i) - { - position_state_[i] = 0.0; - velocity_state_[i] = 0.0; - } - // reset command only if error is initiated - if (recoverable_error_happened_) - { - for (auto i = 0ul; i < 3; ++i) - { - velocity_command_[i] = 0.0; - } - } - - read_calls_ = 0; - write_calls_ = 0; - - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read a position and a velocity - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_[0])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_[0])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_POSITION, &position_state_[1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_state_[1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_POSITION, &position_state_[2])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_state_[2])); - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - // We can command in velocity - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_[0])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_command_[1])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_command_[2])); - - return command_interfaces; - } - - std::string get_name() const override { return "DummySystem"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, state is getting propagated within write. - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++write_calls_; - if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - for (auto i = 0; i < 3; ++i) - { - position_state_[i] += velocity_command_[0]; - velocity_state_[i] = velocity_command_[0]; - } - return hardware_interface::return_type::OK; - } - - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override - { - for (auto i = 0ul; i < 3; ++i) - { - velocity_state_[i] = 0.0; - } - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - std::array position_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_command_ = {{0.0, 0.0, 0.0}}; - - // Helper variables to initiate error on read - unsigned int read_calls_ = 0; - unsigned int write_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummySystemDefault : public hardware_interface::SystemInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -683,135 +371,41 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface }; } // namespace test_components - -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator) +TEST(TestComponentInterfaces, dummy_actuator_default) { - hardware_interface::Actuator actuator_hw(std::make_unique()); + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - - auto command_interfaces = actuator_hw.export_command_interfaces(); - ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - - double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - - // Read and Write are working because it is INACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Read and Write are working because it is ACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.shutdown(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); -} -// END - -TEST(TestComponentInterfaces, dummy_actuator_default) -{ - hardware_interface::Actuator actuator_hw( - std::make_unique()); - const std::string urdf_to_test = - std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + - ros2_control_test_assets::urdf_tail; - const std::vector control_resources = - hardware_interface::parse_control_resources_from_urdf(urdf_to_test); - const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); - auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); - - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(2u + report_signals_size, state_interfaces.size()); - { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/position"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - } - { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/velocity"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - } + ASSERT_EQ(2u + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); @@ -837,8 +431,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -853,8 +448,10 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -870,8 +467,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -885,8 +482,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -897,40 +495,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default) hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_sensor) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); - EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); - - // Not updated because is is UNCONFIGURED - sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); - - // Updated because is is INACTIVE - state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0]->get_value()); - - // It can read now - sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); -} -// END - TEST(TestComponentInterfaces, dummy_sensor_default) { hardware_interface::Sensor sensor_hw(std::make_unique()); @@ -956,152 +520,25 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } // Not updated because is is UNCONFIGURED auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(6u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); - EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); - - auto command_interfaces = system_hw.export_command_interfaces(); - ASSERT_EQ(3u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); - - double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - command_interfaces[1]->set_value(velocity_value); // velocity - command_interfaces[2]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - - // Read and Write are working because it is INACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Read and Write are working because it is ACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.shutdown(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); -} -// END - TEST(TestComponentInterfaces, dummy_system_default) { hardware_interface::System system_hw(std::make_unique()); @@ -1222,12 +659,15 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1242,14 +682,20 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1265,16 +711,16 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1288,12 +734,15 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1334,68 +783,6 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) system_hw.perform_command_mode_switch(two_keys, one_key)); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) -{ - hardware_interface::Actuator actuator_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = actuator_hw.export_state_interfaces(); - auto command_interfaces = actuator_hw.export_command_interfaces(); - state = actuator_hw.configure(); - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) { hardware_interface::Actuator actuator_hw( @@ -1440,8 +827,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1467,68 +854,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) -{ - hardware_interface::Actuator actuator_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); - auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = actuator_hw.export_state_interfaces(); - auto command_interfaces = actuator_hw.export_command_interfaces(); - state = actuator_hw.configure(); - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) { hardware_interface::Actuator actuator_hw( @@ -1572,8 +897,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1599,73 +924,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); - auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); - - auto state_interfaces = sensor_hw.export_state_interfaces(); - // Updated because is is INACTIVE - state = sensor_hw.configure(); - state = sensor_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - - // Initiate recoverable error - call read 99 times OK and on 100-time will return error - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - - state = sensor_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - - // activate again and expect reset values - state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - - state = sensor_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error - for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - - state = sensor_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - - // can not change state anymore - state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) { hardware_interface::Sensor sensor_hw(std::make_unique()); @@ -1703,7 +961,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) // activate again and expect reset values auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1726,73 +984,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system_read_error_behavior) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = system_hw.export_state_interfaces(); - auto command_interfaces = system_hw.export_command_interfaces(); - state = system_hw.configure(); - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = system_hw.configure(); - for (auto index = 0ul; index < 6; ++index) - { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); - } - for (auto index = 0ul; index < 3; ++index) - { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); - } - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -1834,11 +1025,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1864,73 +1055,6 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system_write_error_behavior) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); - auto state = system_hw.initialize(mock_hw_info, logger, nullptr); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - auto state_interfaces = system_hw.export_state_interfaces(); - auto command_interfaces = system_hw.export_command_interfaces(); - state = system_hw.configure(); - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = system_hw.configure(); - for (auto index = 0ul; index < 6; ++index) - { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); - } - for (auto index = 0ul; index < 3; ++index) - { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); - } - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -1972,11 +1096,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index ad9cbc34ab..f14f301038 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -249,10 +249,10 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); const auto listed_interface_size = 1u; - const auto interfaces_sizeze = listed_interface_size + error_signals_size + warnig_signals_size; + const auto interfaces_sizes = listed_interface_size + error_signals_size + warnig_signals_size; auto state_interfaces = sensor_hw.export_state_interfaces(); // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" - ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); + ASSERT_EQ(interfaces_sizes + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/voltage"); @@ -260,7 +260,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } { auto [contains, position] = @@ -289,10 +289,10 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); const auto listed_interface_size = 6u; - const auto interfaces_sizeze = listed_interface_size + report_signals_size; + const auto interfaces_sizes = listed_interface_size + report_signals_size; auto state_interfaces = system_hw.export_state_interfaces(); // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" - ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); + ASSERT_EQ(interfaces_sizes + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index 06e13a9fa3..d90b616a4b 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -52,6 +52,17 @@ const auto warnig_signals_size = 2; const auto error_signals_size = 2; const auto report_signals_size = emergency_stop_signal_size + warnig_signals_size + error_signals_size; + +const hardware_interface::WARNING_MESSAGES empty_error_msg_vec(32, ""); +const hardware_interface::ERROR_MESSAGES empty_warn_msg_vec(32, ""); +const hardware_interface::ERROR_SIGNALS empty_error_code_vec(32, 0); +const hardware_interface::WARNING_SIGNALS empty_warn_code_vec(32, 0); +const hardware_interface::WARNING_MESSAGES error_msg_vec{ + "Some", "error", "msgs", "so vector is", "not empty."}; +const hardware_interface::ERROR_MESSAGES warn_msg_vec{"Some", "warning", "warn", + "msgs", "so vector is", "not empty."}; +const hardware_interface::ERROR_SIGNALS error_code_vec{1, 2, 3, 4, 5, 6, 7}; +const hardware_interface::WARNING_SIGNALS warn_code_vec{1, 2, 3, 4, 5, 6, 7}; } // namespace using namespace ::testing; // NOLINT @@ -78,11 +89,11 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { set_state("joint1/position", 0.0); set_state("joint1/velocity", 0.0); - set_emergency_stop(0.0); - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_emergency_stop(false); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); if (recoverable_error_happened_) { @@ -103,11 +114,11 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -121,11 +132,11 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface ++write_calls_; if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } auto position_state = get_state("joint1/position"); @@ -183,10 +194,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface { set_state(name, 0.0); } - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); read_calls_ = 0; return CallbackReturn::SUCCESS; } @@ -199,10 +210,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -254,11 +265,11 @@ class DummySystemDefault : public hardware_interface::SystemInterface set_state(position_states_[i], 0.0); set_state(velocity_states_[i], 0.0); } - set_emergency_stop(0.0); - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_emergency_stop(false); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); // reset command only if error is initiated if (recoverable_error_happened_) { @@ -282,11 +293,11 @@ class DummySystemDefault : public hardware_interface::SystemInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -300,11 +311,11 @@ class DummySystemDefault : public hardware_interface::SystemInterface ++write_calls_; if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -372,9 +383,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 2; + const auto listed_interface_size = 2; auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + ASSERT_EQ(listed_interface_size + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -489,10 +500,10 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 1; + const auto listed_interface_size = 1; auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ( - state_interface_offset + warnig_signals_size + error_signals_size, state_interfaces.size()); + listed_interface_size + warnig_signals_size + error_signals_size, state_interfaces.size()); // check that the normal interfaces get exported as expected { auto [contains, position] = @@ -501,7 +512,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } // ERROR_SIGNAL { @@ -575,9 +586,9 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 6; + const auto listed_interface_size = 6; auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + ASSERT_EQ(listed_interface_size + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -743,7 +754,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 2; auto state_interfaces = actuator_hw.export_state_interfaces(); auto command_interfaces = actuator_hw.export_command_interfaces(); state = actuator_hw.configure(); @@ -783,18 +793,26 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -805,8 +823,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -819,18 +837,26 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -858,7 +884,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 2; auto state_interfaces = actuator_hw.export_state_interfaces(); auto command_interfaces = actuator_hw.export_command_interfaces(); state = actuator_hw.configure(); @@ -882,8 +907,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -921,18 +946,26 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -957,7 +990,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; auto state = sensor_hw.initialize(voltage_sensor_res); - const auto state_interface_offset = 1; auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE state = sensor_hw.configure(); @@ -991,16 +1023,24 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = sensor_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -1008,7 +1048,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) // activate again and expect reset values state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1046,7 +1086,6 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 6; auto state_interfaces = system_hw.export_state_interfaces(); auto command_interfaces = system_hw.export_command_interfaces(); state = system_hw.configure(); @@ -1086,18 +1125,26 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -1107,11 +1154,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1124,18 +1171,26 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -1162,7 +1217,6 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 6; auto state_interfaces = system_hw.export_state_interfaces(); auto command_interfaces = system_hw.export_command_interfaces(); state = system_hw.configure(); @@ -1202,18 +1256,26 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -1223,11 +1285,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 7d79c032f0..8184a6210f 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -13,8 +13,12 @@ // limitations under the License. #include +#include + #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" using hardware_interface::CommandInterface; using hardware_interface::InterfaceDescription; @@ -27,45 +31,759 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace -TEST(TestHandle, command_interface) +TEST(TestHandle, ci_empty_handle_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, true); +} + +TEST(TestHandle, si_empty_handle_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, true); +} + +TEST(TestHandle, ci_empty_handle_prefix_empty_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_empty_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_inteface_name_empty_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_inteface_name_empty_value) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, ci_empty_handle_prefix_and_inteface_name) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_and_inteface_name) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_prefix_and_inteface_name_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_and_inteface_name_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_prefix) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_interface_name) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_interface_name) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_bool_initialization) { - double value = 1.337; - CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, ci_true_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, ci_empty_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, ci_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), 1.5); EXPECT_NO_THROW(interface.set_value(0.0)); - EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, ci_negative_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, ci_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::invalid_argument); +} + +TEST(TestHandle, ci_int8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); } -TEST(TestHandle, state_interface) +TEST(TestHandle, ci_int8_t_vector_wrong_size_initialization) { - double value = 1.337; - StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); - // interface.set_value(5); compiler error, no set_value function + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_int8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, ci_throw_on_get_wrong_type_bool_from_int8_vec) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, ci_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, ci_throw_on_get_double_from_monostate) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, si_empty_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, si_true_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, si_empty_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, si_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), 1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_negative_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::invalid_argument); +} + +TEST(TestHandle, si_int8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_int8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_int8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, si_throw_on_get_wrong_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, si_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, si_throw_on_empty_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); } TEST(TestHandle, name_getters_work) { - StateInterface handle{JOINT_NAME, FOO_INTERFACE}; + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + StateInterface handle{descr}; + + EXPECT_EQ(handle, false); EXPECT_EQ(handle.get_name(), std::string(JOINT_NAME) + "/" + std::string(FOO_INTERFACE)); EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME); } -TEST(TestHandle, value_methods_throw_for_nullptr) +TEST(TestHandle, ci_value_methods) { - CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); - EXPECT_ANY_THROW(handle.set_value(0.0)); + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + CommandInterface handle{descr}; + + EXPECT_EQ(handle, true); + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); + EXPECT_NO_THROW(handle.set_value(0.0)); + EXPECT_EQ(handle.get_value(), 0.0); } -TEST(TestHandle, value_methods_work_on_non_nullptr) +TEST(TestHandle, si_value_methods) { - double value = 1.337; - CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + StateInterface handle{descr}; + + EXPECT_EQ(handle, true); + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); + EXPECT_EQ(handle.get_value(), 0.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) @@ -74,12 +792,14 @@ TEST(TestHandle, interface_description_state_interface_name_getters_work) const std::string JOINT_NAME_1 = "joint1"; InterfaceInfo info; info.name = POSITION_INTERFACE; + info.data_type = "double"; InterfaceDescription interface_descr(JOINT_NAME_1, info); StateInterface handle{interface_descr}; + EXPECT_EQ(handle, true); EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); - EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); + (handle.get_prefix_name(), JOINT_NAME_1); } TEST(TestHandle, interface_description_command_interface_name_getters_work) @@ -87,10 +807,12 @@ TEST(TestHandle, interface_description_command_interface_name_getters_work) const std::string POSITION_INTERFACE = "position"; const std::string JOINT_NAME_1 = "joint1"; InterfaceInfo info; + info.data_type = "double"; info.name = POSITION_INTERFACE; InterfaceDescription interface_descr(JOINT_NAME_1, info); CommandInterface handle{interface_descr}; + EXPECT_EQ(handle, true); EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index ff57d6a63d..ec7a29f40b 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -18,9 +18,9 @@ #include "hardware_interface/sensor_interface.hpp" +using hardware_interface::InterfaceDescription; using hardware_interface::return_type; using hardware_interface::SensorInterface; -using hardware_interface::StateInterface; namespace test_hardware_components { @@ -49,54 +49,41 @@ class TestForceTorqueSensor : public SensorInterface } } + sensor_name_ = info_.sensors[0].name; fprintf(stderr, "TestForceTorqueSensor configured successfully.\n"); return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; - const auto & sensor_name = info_.sensors[0].name; - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fy", &values_.fy)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fz", &values_.fz)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tx", &values_.tx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "ty", &values_.ty)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tz", &values_.tz)); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + for (const auto & interface_name : inteface_names_) + { + info.name = interface_name; + state_interfaces.push_back(hardware_interface::InterfaceDescription(sensor_name_, info)); + } return state_interfaces; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - values_.fx = fmod((values_.fx + 1.0), 10); - values_.fy = fmod((values_.fy + 1.0), 10); - values_.fz = fmod((values_.fz + 1.0), 10); - values_.tx = fmod((values_.tx + 1.0), 10); - values_.ty = fmod((values_.ty + 1.0), 10); - values_.tz = fmod((values_.tz + 1.0), 10); + for (const auto & interface_name : inteface_names_) + { + const auto name = sensor_name_ + "/" + interface_name; + + set_state(name, fmod((get_state(name) + 1.0), 10)); + } return return_type::OK; } private: - struct FTValues - { - double fx = 0.0; - double fy = 0.0; - double fz = 0.0; - double tx = 0.0; - double ty = 0.0; - double tz = 0.0; - }; - - FTValues values_; + std::vector inteface_names_{"fx", "fy", "fz", "tx", "ty", "tz"}; + std::string sensor_name_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index dacbf308fa..a52d356c5e 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -63,30 +63,35 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::ERROR; } } + joint_name_ = info_.joints[0].name; + joint_pos_ = joint_name_ + "/" + hardware_interface::HW_IF_POSITION; + joint_vel_ = joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY; fprintf(stderr, "TestSingleJointActuator configured successfully.\n"); return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; - const auto & joint_name = info_.joints[0].name; - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_VELOCITY, &velocity_state_)); + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + state_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; - const auto & joint_name = info_.joints[0].name; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); return command_interfaces; } @@ -98,15 +103,15 @@ class TestSingleJointActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - velocity_state_ = position_command_ - position_state_; - position_state_ = position_command_; + set_state(joint_vel_, get_command(joint_pos_) - get_state(joint_pos_)); + set_state(joint_pos_, get_command(joint_pos_)); return return_type::OK; } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double position_command_ = 0.0; + std::string joint_name_; + std::string joint_pos_; + std::string joint_vel_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index aba2f86fe5..f9cba77394 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -74,31 +74,42 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_ACCELERATION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return command_interfaces; @@ -120,7 +131,10 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { - acceleration_state_[0] += 1.0; + const auto first_joint_acc = + info_.joints[0].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto old_acc = get_state(first_joint_acc); + set_state(first_joint_acc, old_acc + 1.0); // Starting interfaces start_modes_.clear(); @@ -168,7 +182,10 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & /*stop_interfaces*/) override { - acceleration_state_[0] += 100.0; + const auto first_joint_acc = + info_.joints[0].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto old_acc = get_state(first_joint_acc); + set_state(first_joint_acc, old_acc + 100.0); // Test of failure in perform command mode switch // Fail if given an empty list. // This should never occur in a real system as the same start_interfaces list is sent to both @@ -183,12 +200,6 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface private: std::vector start_modes_ = {"position", "position"}; std::vector stop_modes_ = {false, false}; - - std::array position_command_ = {{0.0, 0.0}}; - std::array velocity_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; - std::array velocity_state_ = {{0.0, 0.0}}; - std::array acceleration_state_ = {{0.0, 0.0}}; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index 5942f14d42..6bbdc75150 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -67,25 +67,33 @@ class TestTwoJointSystem : public SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return command_interfaces; @@ -100,10 +108,6 @@ class TestTwoJointSystem : public SystemInterface { return return_type::OK; } - -private: - std::array position_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; }; } // namespace test_hardware_components diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index b7149369ed..d34b431cf5 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -49,42 +49,29 @@ class TestActuator : public ActuatorInterface * CallbackReturn::ERROR;} */ - return CallbackReturn::SUCCESS; - } + pos_state_ = info_.joints[0].name + "/position"; + vel_state_ = info_.joints[0].name + "/velocity"; + vel_command_ = info_.joints[0].name + "/velocity"; - std::vector export_state_interfaces() override - { - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[0].name, "some_unlisted_interface", nullptr)); - - return state_interfaces; + return CallbackReturn::SUCCESS; } - std::vector export_command_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_)); - - if (info_.joints[0].command_interfaces.size() > 1) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_)); - } + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); + interfaces.push_back(unlisted_state_interface); - return command_interfaces; + return interfaces; } hardware_interface::return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { - position_state_ += 1.0; + set_state(pos_state_, get_state(pos_state_) + 1.0); return hardware_interface::return_type::OK; } @@ -92,22 +79,22 @@ class TestActuator : public ActuatorInterface const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { - position_state_ += 100.0; + set_state(pos_state_, get_state(pos_state_) + 100.0); return hardware_interface::return_type::OK; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_ == test_constants::READ_FAIL_VALUE) + if (get_command(vel_command_) == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + set_command(vel_command_, 0.0); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + if (get_command(vel_command_) == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -116,22 +103,22 @@ class TestActuator : public ActuatorInterface // working as it should. This makes value checks clearer and confirms there // is no "state = command" line or some other mixture of interfaces // somewhere in the test stack. - velocity_state_ = velocity_command_ / 2; + set_state(vel_state_, get_command(vel_command_) / 2); return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) + if (get_command(vel_command_) == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + set_command(vel_command_, 0.0); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + if (get_command(vel_command_) == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -139,10 +126,9 @@ class TestActuator : public ActuatorInterface } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double velocity_command_ = 0.0; - double max_velocity_command_ = 0.0; + std::string pos_state_; + std::string vel_state_; + std::string vel_command_; }; class TestUninitializableActuator : public TestActuator diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 7f5a476c14..740e5fe10e 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -36,22 +36,10 @@ class TestSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override - { - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_)); - - return state_interfaces; - } - return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { return return_type::OK; } - -private: - double velocity_state_ = 0.0; }; class TestUninitializableSensor : public TestSensor diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 6724a1d019..bbfbce6fbf 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -91,15 +91,15 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + set_command(info_.joints[0].name + "/velocity", 0.0); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -109,15 +109,15 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + set_command(info_.joints[0].name + "/velocity", 0.0); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -125,10 +125,6 @@ class TestSystem : public SystemInterface } private: - std::array velocity_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; - std::array velocity_state_ = {{0.0, 0.0}}; - std::array acceleration_state_ = {{0.0, 0.0}}; double max_acceleration_command_ = 0.0; double configuration_state_ = 0.0; double configuration_command_ = 0.0; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index f932c610fe..9a23d0d3c8 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -353,22 +353,26 @@ TEST_F(ResourceManagerTest, resource_claiming) class ExternalComponent : public hardware_interface::ActuatorInterface { - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface("external_joint", "external_state_interface", nullptr)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "external_state_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface("external_joint", info); + interfaces.push_back(unlisted_state_interface); - return state_interfaces; + return interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "external_joint", "external_command_interface", nullptr)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "external_command_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface("external_joint", info); + interfaces.push_back(unlisted_state_interface); - return command_interfaces; + return interfaces; } std::string get_name() const override { return "ExternalComponent"; } @@ -1235,8 +1239,12 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(std::make_shared( - CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); + hardware_interface::InterfaceInfo info; + info.name = REFERENCE_INTERFACE_NAMES[i]; + info.initial_value = std::to_string(reference_interface_values[i]); + hardware_interface::InterfaceDescription ref_interface(CONTROLLER_NAME, info); + reference_interfaces.push_back( + std::make_shared(ref_interface)); } rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces); diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index b6b4353dd8..22723d3897 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -265,10 +265,12 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); joint_pos[0].set_value( - (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + + (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / + (2.0 * jr[0]) + joint_offset_[0]); joint_pos[1].set_value( - (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) + + (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / + (2.0 * jr[1]) + joint_offset_[1]); } @@ -279,9 +281,11 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); joint_vel[0].set_value( - (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0])); + (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / + (2.0 * jr[0])); joint_vel[1].set_value( - (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / (2.0 * jr[1])); + (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / + (2.0 * jr[1])); } auto & act_eff = actuator_effort_; @@ -291,9 +295,9 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); joint_eff[0].set_value( - jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); + jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); joint_eff[1].set_value( - jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); + jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); } } @@ -309,7 +313,8 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; + joint_pos[0].get_value() - joint_offset_[0], + joint_pos[1].get_value() - joint_offset_[1]}; act_pos[0].set_value( (joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1]) * ar[0]); act_pos[1].set_value( @@ -323,9 +328,11 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); act_vel[0].set_value( - (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]); + (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * + ar[0]); act_vel[1].set_value( - (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * ar[1]); + (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * + ar[1]); } auto & act_eff = actuator_effort_; @@ -335,9 +342,11 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); act_eff[0].set_value( - (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0])); + (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / + (2.0 * ar[0])); act_eff[1].set_value( - (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1])); + (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / + (2.0 * ar[1])); } } diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index a8757e9c02..c886145350 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -262,9 +262,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); + joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); joint_pos[1].set_value( - (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / jr[1] + + (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / + jr[1] + joint_offset_[1]); } @@ -275,9 +276,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); + joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); joint_vel[1].set_value( - (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / jr[1]); + (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / + jr[1]); } // effort @@ -287,9 +289,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); + joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); + jr[1] * + (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); } } @@ -306,7 +309,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; + joint_pos[0].get_value() - joint_offset_[0], + joint_pos[1].get_value() - joint_offset_[1]}; act_pos[0].set_value(joints_offset_applied[0] * jr[0] * ar[0]); act_pos[1].set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]); } @@ -318,8 +322,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); - act_vel[1].set_value((joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); + act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); + act_vel[1].set_value( + (joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); } // effort @@ -329,8 +334,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); - act_eff[1].set_value((joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); + act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); + act_eff[1].set_value( + (joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); } } diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 809472ab75..229a2bbdb3 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -20,6 +20,7 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "transmission_interface/exception.hpp" #include "transmission_interface/transmission.hpp" @@ -128,13 +129,13 @@ class SimpleTransmission : public Transmission double reduction_; double jnt_offset_; - JointHandle joint_position_ = {"", "", nullptr}; - JointHandle joint_velocity_ = {"", "", nullptr}; - JointHandle joint_effort_ = {"", "", nullptr}; + JointHandle joint_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + JointHandle joint_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + JointHandle joint_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; - ActuatorHandle actuator_position_ = {"", "", nullptr}; - ActuatorHandle actuator_velocity_ = {"", "", nullptr}; - ActuatorHandle actuator_effort_ = {"", "", nullptr}; + ActuatorHandle actuator_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + ActuatorHandle actuator_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + ActuatorHandle actuator_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; }; inline SimpleTransmission::SimpleTransmission( @@ -156,7 +157,9 @@ HandleType get_by_interface( [&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; }); if (result == handles.cend()) { - return HandleType(handles.cbegin()->get_prefix_name(), interface_name, nullptr); + // return new Handle where data is set to std::monotype by default + return HandleType(hardware_interface::InterfaceDescription( + handles.cbegin()->get_prefix_name(), interface_name)); } return *result; } @@ -218,17 +221,17 @@ inline void SimpleTransmission::actuator_to_joint() { if (joint_effort_ && actuator_effort_) { - joint_effort_.set_value(actuator_effort_.get_value() * reduction_); + joint_effort_.set_value(actuator_effort_.get_value() * reduction_); } if (joint_velocity_ && actuator_velocity_) { - joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); + joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); } if (joint_position_ && actuator_position_) { - joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); + joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); } } @@ -236,17 +239,17 @@ inline void SimpleTransmission::joint_to_actuator() { if (joint_effort_ && actuator_effort_) { - actuator_effort_.set_value(joint_effort_.get_value() / reduction_); + actuator_effort_.set_value(joint_effort_.get_value() / reduction_); } if (joint_velocity_ && actuator_velocity_) { - actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); + actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); } if (joint_position_ && actuator_position_) { - actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); + actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); } } diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 14ffe968bc..8c3c01e30c 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -23,6 +23,8 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using testing::DoubleNear; using transmission_interface::ActuatorHandle; using transmission_interface::DifferentialTransmission; @@ -92,14 +94,22 @@ void testConfigureWithBadHandles(std::string interface_name) DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0}); double dummy; - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + auto a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a2_handle = + ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a3_handle = + ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j3_handle = + JointHandle(InterfaceDescription("joint3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -129,12 +139,6 @@ TEST(ConfigureTest, FailsWithBadHandles) class TransmissionSetup : public ::testing::Test { -protected: - // Input/output transmission data - double a_val[2]; - double j_val[2]; - std::vector a_vec = {&a_val[0], &a_val[1]}; - std::vector j_vec = {&j_val[0], &j_val[1]}; }; /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -151,23 +155,29 @@ class BlackBoxTest : public TransmissionSetup const std::string & interface_name) { // set actuator values to reference - a_val[0] = ref_val[0]; - a_val[1] = ref_val[1]; + auto a_val = ref_val[0]; + auto a_val_1 = ref_val[1]; + // create handles and configure - auto a1_handle = ActuatorHandle("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle.set_value(EXTREMAL_VALUE); + a2_handle.set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(ref_val[0], DoubleNear(a1_handle.get_value(), EPS)); + EXPECT_THAT(ref_val[1], DoubleNear(a2_handle.get_value(), EPS)); } // Generate a set of transmission instances @@ -237,43 +247,57 @@ TEST_F(WhiteBoxTest, DontMoveJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 0.0; - *a_vec[1] = 0.0; + auto a_val = 0.0; + auto a_val_1 = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -285,43 +309,58 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = 10.0; + auto a_val = 10.0; + auto a_val_1 = 10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(400.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -333,43 +372,56 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = -10.0; + auto a_val = 10.0; + auto a_val_1 = -10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(400.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -386,42 +438,54 @@ TEST_F(WhiteBoxTest, MoveBothJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 3.0; - *a_vec[1] = 5.0; + auto a_val = 3.0; + auto a_val_1 = 5.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(140.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(520.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.01250, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.06875, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.01250, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(4.06875, DoubleNear(joint2_handle.get_value(), EPS)); } } diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index 08d98ec734..c4ab04986c 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -25,6 +25,8 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using testing::DoubleNear; using transmission_interface::ActuatorHandle; using transmission_interface::Exception; @@ -93,16 +95,17 @@ TEST(PreconditionsTest, AccessorValidation) void testConfigureWithBadHandles(std::string interface_name) { FourBarLinkageTransmission trans({1.0, 1.0}, {1.0, 1.0}); - double dummy; - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(interface_name))); + auto a3_handle = ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(interface_name))); + auto j1_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); + auto j2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name))); + auto j3_handle = JointHandle(InterfaceDescription("joint3", InterfaceInfo(interface_name))); + auto invalid_a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); + auto invalid_j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -133,11 +136,6 @@ TEST(ConfigureTest, FailsWithBadHandles) class TransmissionSetup : public ::testing::Test { protected: - // Input/output transmission data - double a_val[2]; - double j_val[2]; - std::vector a_vec = {&a_val[0], &a_val[1]}; - std::vector j_vec = {&j_val[0], &j_val[1]}; }; /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -154,23 +152,28 @@ class BlackBoxTest : public TransmissionSetup const std::string & interface_name) { // set actuator values to reference - a_val[0] = ref_val[0]; - a_val[1] = ref_val[1]; + auto a_val = ref_val[0]; + auto a_val_1 = ref_val[1]; // create handles and configure - auto a1_handle = ActuatorHandle("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle.set_value(EXTREMAL_VALUE); + a2_handle.set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(a_val, DoubleNear(a1_handle.get_value(), EPS)); + EXPECT_THAT(a_val_1, DoubleNear(a2_handle.get_value(), EPS)); } // Generate a set of transmission instances @@ -240,43 +243,55 @@ TEST_F(WhiteBoxTest, DontMoveJoints) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - a_val[0] = 0.0; - a_val[1] = 0.0; + auto a_val = 0.0; + auto a_val_1 = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -289,44 +304,56 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Effort interface { - a_val[0] = 5.0; - a_val[1] = 10.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a_val = 5.0; + auto a_val_1 = 10.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(100.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a_val = 10.0; + auto a_val_1 = 5.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a_val = 10.0; + auto a_val_1 = 5.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -338,43 +365,55 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - a_val[0] = 0.0; - a_val[1] = 10.0; + auto a_val = 0.0; + auto a_val_1 = 10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(200.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -391,42 +430,54 @@ TEST_F(WhiteBoxTest, MoveBothJoints) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - a_val[0] = 3.0; - a_val[1] = 5.0; + auto a_val = 3.0; + auto a_val_1 = 5.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-60.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-160.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-60.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(-160.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-0.025, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.15, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(-0.025, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(3.975, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.15, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(3.975, DoubleNear(joint2_handle.get_value(), EPS)); } } diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index d687fbfd21..026264e79b 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -15,12 +15,16 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "transmission_interface/simple_transmission.hpp" using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; +using std::vector; using transmission_interface::ActuatorHandle; using transmission_interface::Exception; using transmission_interface::JointHandle; @@ -61,12 +65,13 @@ TEST(PreconditionsTest, AccessorValidation) TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) { SimpleTransmission trans(2.0, -1.0); - double dummy; - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &dummy); - auto actuator2_handle = ActuatorHandle("act2", HW_IF_POSITION, &dummy); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &dummy); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, &dummy); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION))); + auto actuator2_handle = + ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(HW_IF_POSITION))); + auto joint_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION))); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION))); EXPECT_THROW(trans.configure({}, {}), transmission_interface::Exception); EXPECT_THROW(trans.configure({joint_handle}, {}), transmission_interface::Exception); @@ -79,8 +84,10 @@ TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) trans.configure({joint_handle, joint2_handle}, {actuator_handle}), transmission_interface::Exception); - auto invalid_actuator_handle = ActuatorHandle("act1", HW_IF_VELOCITY, nullptr); - auto invalid_joint_handle = JointHandle("joint1", HW_IF_VELOCITY, nullptr); + auto invalid_actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY))); + auto invalid_joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY))); EXPECT_THROW( trans.configure({invalid_joint_handle}, {invalid_actuator_handle}), transmission_interface::Exception); @@ -94,15 +101,6 @@ TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) class TransmissionSetup { protected: - // Input/output transmission data - double a_val = 0.0; - double j_val = 0.0; - - void reset_values() - { - a_val = 0.0; - j_val = 0.0; - } }; /// Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -120,15 +118,19 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam(), EPS)); + actuator_handle.set_value(0.0); + joint_handle.set_value(0.0); } } }; @@ -140,23 +142,15 @@ TEST_P(BlackBoxTest, IdentityMap) // Test transmission for positive, zero, and negative inputs testIdentityMap(trans, HW_IF_POSITION, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_POSITION, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_POSITION, -1.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, -1.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, -1.0); } @@ -179,53 +173,61 @@ TEST_F(WhiteBoxTest, MoveJoint) SimpleTransmission trans(10.0, 1.0); - a_val = 1.0; - // Effort interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_EFFORT, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_EFFORT, &j_val); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, "1.0", "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(10.0, DoubleNear(j_val, EPS)); + EXPECT_THAT(10.0, DoubleNear(joint_handle.get_value(), EPS)); } // Velocity interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_VELOCITY, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_VELOCITY, &j_val); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, "1.0", "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.1, DoubleNear(j_val, EPS)); + EXPECT_THAT(0.1, DoubleNear(joint_handle.get_value(), EPS)); } // Position interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &j_val); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(1.1, DoubleNear(j_val, EPS)); + EXPECT_THAT(1.1, DoubleNear(joint_handle.get_value(), EPS)); } // Mismatched interface is ignored { double unique_value = 13.37; - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &a_val); - auto actuator_handle2 = ActuatorHandle("act1", HW_IF_VELOCITY, &unique_value); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &j_val); - auto joint_handle2 = JointHandle("joint1", HW_IF_VELOCITY, &unique_value); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + auto actuator_handle2 = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(unique_value), "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + auto joint_handle2 = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(unique_value), "double"))); trans.configure({joint_handle, joint_handle2}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(unique_value, DoubleNear(13.37, EPS)); + EXPECT_THAT(joint_handle.get_value(), DoubleNear(13.37, EPS)); trans.configure({joint_handle}, {actuator_handle, actuator_handle2}); trans.actuator_to_joint(); - EXPECT_THAT(unique_value, DoubleNear(13.37, EPS)); + EXPECT_THAT(joint_handle.get_value(), DoubleNear(13.37, EPS)); } } diff --git a/transmission_interface/test/utils_test.cpp b/transmission_interface/test/utils_test.cpp index 9afaccaa47..808faa20d7 100644 --- a/transmission_interface/test/utils_test.cpp +++ b/transmission_interface/test/utils_test.cpp @@ -25,8 +25,11 @@ using transmission_interface::JointHandle; TEST(UtilsTest, AccessorTest) { const std::string NAME = "joint"; - double joint_value = 0.0; - const JointHandle joint_handle(NAME, HW_IF_POSITION, &joint_value); + hardware_interface::InterfaceInfo info; + info.name = HW_IF_POSITION; + info.initial_value = "0.0"; + hardware_interface::InterfaceDescription joint_handle_desc(NAME, info); + const JointHandle joint_handle(joint_handle_desc); const std::vector joint_handles = {joint_handle}; ASSERT_EQ(transmission_interface::get_names(joint_handles), std::vector{NAME}); From 395c50c5a27fedc233c5a2e64e5165d73a84ce8a Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 2 Aug 2024 09:17:37 +0200 Subject: [PATCH 30/34] remove old functionallity and continue adapting tests --- .../chainable_controller_interface.hpp | 25 +- .../force_torque_sensor.hpp | 4 +- .../semantic_components/imu_sensor.hpp | 6 +- .../semantic_component_interface.hpp | 2 +- .../src/chainable_controller_interface.cpp | 149 ++- .../test_chainable_controller_interface.cpp | 162 +++- .../test_chainable_controller_interface.hpp | 124 ++- controller_manager/package.xml | 1 + .../test_chainable_controller.cpp | 78 +- .../test_chainable_controller.hpp | 12 +- .../test/test_controller/test_controller.cpp | 2 +- ...llers_chaining_with_controller_manager.cpp | 143 ++- doc/Iron.rst | 2 +- .../hardware_interface/actuator_interface.hpp | 16 +- .../hardware_interface/hardware_info.hpp | 40 +- .../loaned_command_interface.hpp | 12 +- .../loaned_state_interface.hpp | 7 +- .../hardware_interface/sensor_interface.hpp | 10 +- .../hardware_interface/system_interface.hpp | 16 +- .../mock_components/generic_system.hpp | 3 +- .../src/mock_components/generic_system.cpp | 3 +- .../mock_components/test_generic_system.cpp | 904 +++++++++--------- .../test/test_error_warning_codes.cpp | 24 +- .../test_force_torque_sensor.cpp | 3 +- .../test_imu_sensor.cpp | 131 ++- .../test_single_joint_actuator.cpp | 6 +- .../test_system_with_command_modes.cpp | 6 +- .../test_two_joint_system.cpp | 6 +- .../test/test_components/test_actuator.cpp | 3 +- .../test/test_components/test_system.cpp | 44 +- .../test/test_resource_manager.cpp | 14 +- ...esource_manager_prepare_perform_switch.cpp | 240 ++--- 32 files changed, 1264 insertions(+), 934 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 4820afd817..c45e86a8cd 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -80,9 +80,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase * exported. The method has the same meaning as `export_state_interfaces` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * - * \returns list of StateInterfaces that other controller can use as their inputs. + * \returns list of StateInterfacesDescriptions that other controller can use as their inputs. */ - virtual std::vector on_export_state_interfaces(); + virtual std::vector + export_state_interface_descriptions() = 0; /// Virtual method that each chainable controller should implement to export its read/write /// chainable interfaces. @@ -91,9 +92,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase * exported. The method has the same meaning as `export_command_interface` method from * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. * - * \returns list of CommandInterfaces that other controller can use as their outputs. + * \returns list of CommandInterfacesDescriptions that other controller can use as their outputs. */ - virtual std::vector on_export_reference_interfaces(); + virtual std::vector + export_reference_interface_descriptions() = 0; /// Virtual method that each chainable controller should implement to switch chained mode. /** @@ -133,18 +135,17 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for state interfaces + // interface_names are in order they have been exported std::vector exported_state_interface_names_; - std::vector state_interfaces_values_; + // storage for the exported StateInterfaces + std::unordered_map> + exported_state_interfaces_; - /// Storage of values for reference interfaces + // interface_names are in order they have been exported std::vector exported_reference_interface_names_; - // BEGIN (Handle export change): for backward compatibility - std::vector reference_interfaces_; - std::unordered_map> ref_interface_to_value_; - // END + // storage for the exported CommandInterfaces std::unordered_map> - reference_interfaces_ptrs_; + reference_interfaces_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/semantic_components/force_torque_sensor.hpp b/controller_interface/include/semantic_components/force_torque_sensor.hpp index ab55a537ad..3db643aaf8 100644 --- a/controller_interface/include/semantic_components/force_torque_sensor.hpp +++ b/controller_interface/include/semantic_components/force_torque_sensor.hpp @@ -102,7 +102,7 @@ class ForceTorqueSensor : public SemanticComponentInterface(); ++interface_counter; } } @@ -126,7 +126,7 @@ class ForceTorqueSensor : public SemanticComponentInterface(); ++torque_interface_counter; } } diff --git a/controller_interface/include/semantic_components/imu_sensor.hpp b/controller_interface/include/semantic_components/imu_sensor.hpp index 74c5524826..c7e7056958 100644 --- a/controller_interface/include/semantic_components/imu_sensor.hpp +++ b/controller_interface/include/semantic_components/imu_sensor.hpp @@ -59,7 +59,7 @@ class IMUSensor : public SemanticComponentInterface size_t interface_offset = 0; for (size_t i = 0; i < orientation_.size(); ++i) { - orientation_[i] = state_interfaces_[interface_offset + i].get().get_value(); + orientation_[i] = state_interfaces_[interface_offset + i].get().get_value(); } return orientation_; } @@ -75,7 +75,7 @@ class IMUSensor : public SemanticComponentInterface size_t interface_offset = orientation_.size(); for (size_t i = 0; i < angular_velocity_.size(); ++i) { - angular_velocity_[i] = state_interfaces_[interface_offset + i].get().get_value(); + angular_velocity_[i] = state_interfaces_[interface_offset + i].get().get_value(); } return angular_velocity_; } @@ -91,7 +91,7 @@ class IMUSensor : public SemanticComponentInterface size_t interface_offset = orientation_.size() + angular_velocity_.size(); for (size_t i = 0; i < linear_acceleration_.size(); ++i) { - linear_acceleration_[i] = state_interfaces_[interface_offset + i].get().get_value(); + linear_acceleration_[i] = state_interfaces_[interface_offset + i].get().get_value(); } return linear_acceleration_; } diff --git a/controller_interface/include/semantic_components/semantic_component_interface.hpp b/controller_interface/include/semantic_components/semantic_component_interface.hpp index e763aa5d93..421edc2af9 100644 --- a/controller_interface/include/semantic_components/semantic_component_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_interface.hpp @@ -87,7 +87,7 @@ class SemanticComponentInterface // insert all the values for (size_t i = 0; i < state_interfaces_.size(); ++i) { - values.emplace_back(state_interfaces_[i].get().get_value()); + values.emplace_back(state_interfaces_[i].get().get_value()); } return true; } diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 057065a31d..3e99b50c91 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -47,17 +47,17 @@ return_type ChainableControllerInterface::update( std::vector> ChainableControllerInterface::export_state_interfaces() { - auto state_interfaces = on_export_state_interfaces(); + auto state_interfaces_descr = export_state_interface_descriptions(); std::vector> state_interfaces_ptrs_vec; - state_interfaces_ptrs_vec.reserve(state_interfaces.size()); + state_interfaces_ptrs_vec.reserve(state_interfaces_descr.size()); // check if the names of the controller state interfaces begin with the controller's name - for (auto & interface : state_interfaces) + for (auto & descr : state_interfaces_descr) { - if (interface.get_prefix_name() != get_node()->get_name()) + if (descr.prefix_name != get_node()->get_name()) { std::string error_msg = - "The prefix of the interface '" + interface.get_prefix_name() + + "The prefix of the interface description'" + descr.prefix_name + "' does not equal the controller's name '" + get_node()->get_name() + "'. This is mandatory for state interfaces. No state interface will be exported. Please " "correct and recompile the controller with name '" + @@ -65,56 +65,59 @@ ChainableControllerInterface::export_state_interfaces() throw std::runtime_error(error_msg); } - state_interfaces_ptrs_vec.push_back( - std::make_shared(interface)); + auto state_interface = std::make_shared(descr); + const auto inteface_name = state_interface->get_name(); + // check the exported interface name is unique + auto [it, succ] = exported_state_interfaces_.insert({inteface_name, state_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert StateInterface<" + inteface_name + + "> into exported_state_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + exported_state_interfaces_.clear(); + exported_state_interface_names_.clear(); + state_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); + } + exported_state_interface_names_.push_back(inteface_name); + state_interfaces_ptrs_vec.push_back(state_interface); } - return state_interfaces_ptrs_vec; -} - -std::vector> -ChainableControllerInterface::export_reference_interfaces() -{ - auto reference_interfaces = on_export_reference_interfaces(); - std::vector> reference_interfaces_ptrs_vec; - reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); - std::vector> reference_interfaces_ptrs_vec; - reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); - - // BEGIN (Handle export change): for backward compatibility - // BEGIN (Handle export change): for backward compatibility - // check if the "reference_interfaces_" variable is resized to number of interfaces - if (reference_interfaces_.size() != reference_interfaces.size()) + if (exported_state_interfaces_.size() != state_interfaces_descr.size()) { - // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the - // framework std::string error_msg = - "The internal storage for reference values 'reference_interfaces_' variable has size '" + - std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + - std::to_string(reference_interfaces.size()) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; - throw std::runtime_error(error_msg); - std::string error_msg = - "The internal storage for reference values 'reference_interfaces_' variable has size '" + - std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + - std::to_string(reference_interfaces.size()) + + "The internal storage for reference ptrs 'exported_state_interfaces_' variable has size '" + + std::to_string(exported_state_interfaces_.size()) + + "', but it is expected to have the size '" + std::to_string(state_interfaces_descr.size()) + "' equal to the number of exported reference interfaces. Please correct and recompile the " "controller with name '" + get_node()->get_name() + "' and try again."; throw std::runtime_error(error_msg); } - // END + + return state_interfaces_ptrs_vec; +} + +std::vector> +ChainableControllerInterface::export_reference_interfaces() +{ + auto reference_interface_descr = export_reference_interface_descriptions(); + std::vector> reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interface_descr.size()); // check if the names of the reference interfaces begin with the controller's name - const auto ref_interface_size = reference_interfaces.size(); - for (auto & interface : reference_interfaces) + for (auto & descr : reference_interface_descr) { - auto & interface = reference_interfaces[i]; - if (interface.get_prefix_name() != get_node()->get_name()) + if (descr.prefix_name != get_node()->get_name()) { - std::string error_msg = "The name of the interface " + interface.get_name() + + std::string error_msg = "The name of the interface descr " + descr.prefix_name + " does not begin with the controller's name. This is mandatory for " "reference interfaces. Please " "correct and recompile the controller with name " + @@ -122,18 +125,37 @@ ChainableControllerInterface::export_reference_interfaces() throw std::runtime_error(error_msg); } - std::shared_ptr interface_ptr = - std::make_shared(std::move(interface)); - reference_interfaces_ptrs_vec.push_back(interface_ptr); - reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr)); + auto reference_interface = std::make_shared(descr); + const auto inteface_name = reference_interface->get_name(); + // check the exported interface name is unique + auto [it, succ] = reference_interfaces_.insert({inteface_name, reference_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert Reference interface<" + inteface_name + + "> into reference_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + reference_interfaces_.clear(); + exported_reference_interface_names_.clear(); + reference_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); + } + exported_reference_interface_names_.push_back(inteface_name); + reference_interfaces_ptrs_vec.push_back(reference_interface); } - if (reference_interfaces_ptrs_.size() != ref_interface_size) + if (reference_interfaces_.size() != reference_interface_descr.size()) { std::string error_msg = - "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + - std::to_string(reference_interfaces_ptrs_.size()) + - "', but it is expected to have the size '" + std::to_string(ref_interface_size) + + "The internal storage for reference ptrs 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interface_descr.size()) + "' equal to the number of exported reference interfaces. Please correct and recompile the " "controller with name '" + get_node()->get_name() + "' and try again."; @@ -141,7 +163,6 @@ ChainableControllerInterface::export_reference_interfaces() } return reference_interfaces_ptrs_vec; - return reference_interfaces_ptrs_vec; } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) @@ -173,30 +194,4 @@ bool ChainableControllerInterface::is_in_chained_mode() const { return in_chaine bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } -std::vector -ChainableControllerInterface::on_export_state_interfaces() -{ - state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); - std::vector state_interfaces; - for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) - { - state_interfaces.emplace_back( - get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]); - } - return state_interfaces; -} - -std::vector -ChainableControllerInterface::on_export_reference_interfaces() -{ - reference_interfaces_.resize(exported_reference_interface_names_.size(), 0.0); - std::vector reference_interfaces; - for (size_t i = 0; i < exported_reference_interface_names_.size(); ++i) - { - reference_interfaces.emplace_back(hardware_interface::CommandInterface( - get_node()->get_name(), exported_reference_interface_names_[i], &reference_interfaces_[i])); - } - return reference_interfaces; -} - } // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 56890a4ce2..c40aae1630 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -20,10 +20,29 @@ using ::testing::IsEmpty; using ::testing::SizeIs; +const std::vector ChainableControllerInterfaceTest::ref_itf_names{ + "test_ref_1", "test_ref_2"}; +const std::vector ChainableControllerInterfaceTest::state_itf_names{ + "test_state_1", "test_state_2"}; + +// I think in this case std::string should be ok. Its only used for testing and there should be no +// static initialization order fiasco risk +const std::string ChainableControllerInterfaceTest::full_ref_itf_name_1 = // NOLINT + std::string(TEST_CONTROLLER_NAME) + "/" + ref_itf_names[0]; // NOLINT +const std::string ChainableControllerInterfaceTest::full_ref_itf_name_2 = // NOLINT + std::string(TEST_CONTROLLER_NAME) + "/" + ref_itf_names[1]; // NOLINT +const std::string ChainableControllerInterfaceTest::full_state_itf_name_1 = // NOLINT + std::string(TEST_CONTROLLER_NAME) + "/" + state_itf_names[0]; // NOLINT +const std::string ChainableControllerInterfaceTest::full_state_itf_name_2 = // NOLINT + std::string(TEST_CONTROLLER_NAME) + "/" + state_itf_names[1]; // NOLINT +const std::vector ChainableControllerInterfaceTest::ref_itf_names_duplicate{ + "test_ref_1", "test_ref_1"}; +const std::vector ChainableControllerInterfaceTest::state_itf_names_duplicate{ + "test_state_1", "test_state_1"}; + TEST_F(ChainableControllerInterfaceTest, default_returns) { - TestableChainableControllerInterface controller; - + TestableChainableControllerInterface controller(ref_itf_names, state_itf_names); // initialize, create node const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( @@ -35,10 +54,25 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) EXPECT_FALSE(controller.is_in_chained_mode()); } -TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) +TEST_F(ChainableControllerInterfaceTest, export_empty_ref_and_state_interfaces) { TestableChainableControllerInterface controller; + // initialize, create node + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_state_interfaces, SizeIs(0)); + auto exported_reference_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_reference_interfaces, SizeIs(0)); +} + +TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) +{ + TestableChainableControllerInterface controller(ref_itf_names, state_itf_names); // initialize, create node const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( @@ -48,17 +82,19 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_THAT(exported_state_interfaces, SizeIs(1)); + ASSERT_THAT(exported_state_interfaces, SizeIs(2)); EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); + EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), state_itf_names[0]); + EXPECT_EQ(exported_state_interfaces[1]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[1]->get_interface_name(), state_itf_names[1]); EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[DEFAULT_INIT_POS]->get_value(), 0.0); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) { - TestableChainableControllerInterface controller; - + TestableChainableControllerInterface controller(ref_itf_names, state_itf_names); // initialize, create node const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( @@ -68,17 +104,19 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_THAT(reference_interfaces, SizeIs(1)); + ASSERT_THAT(reference_interfaces, SizeIs(2)); EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), ref_itf_names[0]); + EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[1]->get_interface_name(), ref_itf_names[1]); - EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[DEFAULT_INIT_POS]->get_value(), 0.0); } TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) { - TestableChainableControllerInterface controller; - + TestableChainableControllerInterface controller(ref_itf_names, state_itf_names); // initialize, create node const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( @@ -101,10 +139,33 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) ASSERT_THAT(exported_state_interfaces, IsEmpty()); } -TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) +TEST_F(ChainableControllerInterfaceTest, export_duplicate_iterface_names) { - TestableChainableControllerInterface controller; + TestableChainableControllerInterface controller( + ref_itf_names_duplicate, state_itf_names_duplicate); + // initialize, create node + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + // expect empty return because interface prefix is not equal to the node name + std::vector> exported_reference_interfaces; + EXPECT_THROW( + { exported_reference_interfaces = controller.export_reference_interfaces(); }, + std::runtime_error); + ASSERT_THAT(exported_reference_interfaces, IsEmpty()); + // expect empty return because interface prefix is not equal to the node name + std::vector> exported_state_interfaces; + EXPECT_THROW( + { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error); + ASSERT_THAT(exported_state_interfaces, IsEmpty()); +} + +TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) +{ + TestableChainableControllerInterface controller(ref_itf_names, state_itf_names); // initialize, create node const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( @@ -113,14 +174,17 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_THAT(reference_interfaces, SizeIs(1)); + ASSERT_THAT(reference_interfaces, SizeIs(2)); auto exported_state_interfaces = controller.export_state_interfaces(); - ASSERT_THAT(exported_state_interfaces, SizeIs(1)); + ASSERT_THAT(exported_state_interfaces, SizeIs(2)); EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[1]->get_value(), 0.0); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[1]->get_value(), 0.0); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -133,7 +197,12 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); - EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ( + reference_interfaces[0]->get_value(), EXPORTED_REF_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(reference_interfaces[1]->get_value(), 0.0); + EXPECT_EQ( + exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(exported_state_interfaces[1]->get_value(), 0.0); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); @@ -156,8 +225,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) TEST_F(ChainableControllerInterfaceTest, test_update_logic) { - TestableChainableControllerInterface controller; - + TestableChainableControllerInterface controller(ref_itf_names, state_itf_names); // initialize, create node const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( @@ -165,6 +233,11 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); + auto reference_interfaces = controller.export_reference_interfaces(); + ASSERT_THAT(reference_interfaces, SizeIs(2)); + auto exported_state_interfaces = controller.export_state_interfaces(); + ASSERT_THAT(exported_state_interfaces, SizeIs(2)); + EXPECT_FALSE(controller.set_chained_mode(false)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -172,47 +245,66 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) ASSERT_EQ( controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ( + controller.reference_interfaces_[full_ref_itf_name_1]->get_value(), + INTERFACE_VALUE_INITIAL_REF - 1); + ASSERT_EQ(controller.reference_interfaces_[full_ref_itf_name_2]->get_value(), 0.0); + ASSERT_EQ( + controller.exported_state_interfaces_[full_state_itf_name_1]->get_value(), + EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_[full_state_itf_name_2]->get_value(), 0.0); // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. - controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); + reference_interfaces[0]->set_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); ASSERT_EQ( controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); - ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); - ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ( + controller.reference_interfaces_[full_ref_itf_name_1]->get_value(), + INTERFACE_VALUE_INITIAL_REF - 1); + ASSERT_EQ(controller.reference_interfaces_[full_ref_itf_name_2]->get_value(), 0.0); + ASSERT_EQ( + controller.exported_state_interfaces_[full_state_itf_name_1]->get_value(), + EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.exported_state_interfaces_[full_state_itf_name_2]->get_value(), 0.0); // Provoke error from update - return ERROR, but reference interface is updated and not reduced - controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); + reference_interfaces[0]->set_value(INTERFACE_VALUE_UPDATE_ERROR); ASSERT_EQ( controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); - ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); - ASSERT_EQ(controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE + 1); + ASSERT_EQ(controller.reference_interfaces_[0]->get_value(), INTERFACE_VALUE_UPDATE_ERROR); + ASSERT_EQ( + controller.exported_state_interfaces_[0]->get_value(), + EXPORTED_STATE_INTERFACE_VALUE + 1); - controller.reference_interfaces_[0] = 0.0; + reference_interfaces[0]->set_value(0.0); EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); // Provoke error in update from subscribers - return OK because update of subscribers is not used // reference interface is not updated (updated directly because in chained mode) - controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); + reference_interfaces[0]->set_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); ASSERT_EQ( controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(controller.reference_interfaces_[0], -1.0); + ASSERT_EQ(controller.reference_interfaces_[full_ref_itf_name_1]->get_value(), -1.0); + ASSERT_EQ(controller.reference_interfaces_[full_ref_itf_name_2]->get_value(), 0.0); ASSERT_EQ( - controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + controller.exported_state_interfaces_[full_state_itf_name_1]->get_value(), + EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + ASSERT_EQ(controller.exported_state_interfaces_[full_state_itf_name_2]->get_value(), 0.0); // Provoke error from update - return ERROR, but reference interface is updated directly - controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); - controller.reference_interfaces_[0] = INTERFACE_VALUE_UPDATE_ERROR; + controller.reference_interfaces_[full_ref_itf_name_1]->set_value(INTERFACE_VALUE_UPDATE_ERROR); ASSERT_EQ( controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::ERROR); - ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); + ASSERT_EQ(controller.reference_interfaces_[0]->get_value(), INTERFACE_VALUE_UPDATE_ERROR); + ASSERT_EQ(controller.reference_interfaces_[full_ref_itf_name_2]->get_value(), 0.0); ASSERT_EQ( - controller.state_interfaces_values_[0], EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + controller.exported_state_interfaces_[full_state_itf_name_1]->get_value(), + EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE + 1); + ASSERT_EQ(controller.exported_state_interfaces_[full_state_itf_name_2]->get_value(), 0.0); } diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index ebd268504c..7a5c9da773 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -32,6 +32,8 @@ constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; constexpr double EXPORTED_STATE_INTERFACE_VALUE = 21833.0; constexpr double EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE = 82802.0; +constexpr double EXPORTED_REF_INTERFACE_VALUE_IN_CHAINMODE = 8280.0; +constexpr int DEFAULT_INIT_POS = 1; using hardware_interface::InterfaceDescription; using hardware_interface::InterfaceInfo; @@ -44,17 +46,23 @@ class TestableChainableControllerInterface FRIEND_TEST(ChainableControllerInterfaceTest, test_update_logic); TestableChainableControllerInterface() + : ref_itf_names_({}), state_itf_names_({}), val_for_default_init_(-1) + { + } + + TestableChainableControllerInterface( + const std::vector & ref_itf_names, + const std::vector & state_itf_names) + : ref_itf_names_(ref_itf_names), + state_itf_names_(state_itf_names), + val_for_default_init_(DEFAULT_INIT_POS) { - reference_interfaces_.reserve(1); - reference_interfaces_.push_back(INTERFACE_VALUE); - state_interfaces_values_.reserve(1); - state_interfaces_values_.push_back(EXPORTED_STATE_INTERFACE_VALUE); } controller_interface::CallbackReturn on_init() override { // set default value - name_prefix_of_interfaces_ = get_node()->get_name(); + prefix_of_interfaces_ = get_node()->get_name(); return controller_interface::CallbackReturn::SUCCESS; } @@ -72,33 +80,70 @@ class TestableChainableControllerInterface } // Implementation of ChainableController virtual methods - std::vector on_export_state_interfaces() override + std::vector export_state_interface_descriptions() + override { - std::vector state_interfaces; + using hardware_interface::InterfaceDescription; + using hardware_interface::InterfaceInfo; + std::vector exported_state_interfaces_descr; - state_interfaces.push_back(hardware_interface::StateInterface( - name_prefix_of_interfaces_, "test_state", &state_interfaces_values_[0])); + int i = 0; + for (const auto & state_itf_name : state_itf_names_) + { + if (i == val_for_default_init_) + { + exported_state_interfaces_descr.emplace_back( + prefix_of_interfaces_, InterfaceInfo(state_itf_name, "double")); + } + else + { + exported_state_interfaces_descr.emplace_back( + prefix_of_interfaces_, + InterfaceInfo(state_itf_name, "double", std::to_string(EXPORTED_STATE_INTERFACE_VALUE))); + } + ++i; + } - return state_interfaces; + return exported_state_interfaces_descr; } // Implementation of ChainableController virtual methods - std::vector on_export_reference_interfaces() override + std::vector export_reference_interface_descriptions() + override { - std::vector command_interfaces; + using hardware_interface::InterfaceDescription; + using hardware_interface::InterfaceInfo; + std::vector reference_interface_descr; - command_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription( - name_prefix_of_reference_interfaces_, - InterfaceInfo(ref_itf_name_, std::to_string(INTERFACE_VALUE), "double")))); + int i = 0; + for (const auto & ref_itf_name : ref_itf_names_) + { + if (i == val_for_default_init_) + { + reference_interface_descr.emplace_back( + prefix_of_interfaces_, InterfaceInfo(ref_itf_name, "double")); + } + else + { + reference_interface_descr.emplace_back( + prefix_of_interfaces_, + InterfaceInfo(ref_itf_name, "double", std::to_string(INTERFACE_VALUE))); + } + ++i; + } - return command_interfaces; + return reference_interface_descr; } bool on_set_chained_mode(bool /*chained_mode*/) override { - if (reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == 0.0) + if (reference_interfaces_[exported_reference_interface_names_[0]]->get_value() == 0.0) { - state_interfaces_values_[0] = EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE; + reference_interfaces_[exported_reference_interface_names_[1]]->set_value( + EXPORTED_REF_INTERFACE_VALUE_IN_CHAINMODE); + exported_state_interfaces_[exported_state_interface_names_[0]]->set_value( + EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + return true; } else @@ -111,13 +156,16 @@ class TestableChainableControllerInterface const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { if ( - reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == + reference_interfaces_[exported_reference_interface_names_[0]]->get_value() == INTERFACE_VALUE_SUBSCRIBER_ERROR) { return controller_interface::return_type::ERROR; } - reference_interfaces_ptrs_[ref_itf_full_name_]->set_value(reference_interface_value_); + reference_interfaces_[exported_reference_interface_names_[0]]->set_value( + INTERFACE_VALUE_INITIAL_REF); + exported_state_interfaces_[exported_reference_interface_names_[0]]->set_value( + INTERFACE_VALUE_INITIAL_REF); return controller_interface::return_type::OK; } @@ -125,43 +173,41 @@ class TestableChainableControllerInterface const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { if ( - reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == + reference_interfaces_[exported_reference_interface_names_[0]]->get_value() == INTERFACE_VALUE_UPDATE_ERROR) { return controller_interface::return_type::ERROR; } - reference_interfaces_ptrs_[ref_itf_full_name_]->operator-=(1); + reference_interfaces_[exported_reference_interface_names_[0]]->operator-=(1); + exported_state_interfaces_[exported_state_interface_names_[0]]->operator+=(1); return controller_interface::return_type::OK; } void set_name_prefix_of_reference_interfaces(const std::string & prefix) { - name_prefix_of_reference_interfaces_ = prefix; - update_ref_itf_full_name(); + prefix_of_interfaces_ = prefix; } - void set_new_reference_interface_value(const double ref_itf_value) - { - reference_interface_value_ = ref_itf_value; - update_ref_itf_full_name(); - } - - void update_ref_itf_full_name() - { - ref_itf_full_name_ = name_prefix_of_reference_interfaces_ + "/" + ref_itf_name_; - } - - std::string name_prefix_of_interfaces_; - std::string ref_itf_name_ = "test_itf"; - std::string ref_itf_full_name_; - double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF; + std::string prefix_of_interfaces_; + const std::vector ref_itf_names_; + const std::vector state_itf_names_; + const int val_for_default_init_; }; class ChainableControllerInterfaceTest : public ::testing::Test { public: + static const std::vector ref_itf_names; + static const std::vector state_itf_names; + static const std::vector ref_itf_names_duplicate; + static const std::vector state_itf_names_duplicate; + static const std::string full_ref_itf_name_1; + static const std::string full_ref_itf_name_2; + static const std::string full_state_itf_name_1; + static const std::string full_state_itf_name_2; + static void SetUpTestCase() { rclcpp::init(0, nullptr); } static void TearDownTestCase() { rclcpp::shutdown(); } diff --git a/controller_manager/package.xml b/controller_manager/package.xml index f969b76834..4498778d1c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -15,6 +15,7 @@ ament_index_cpp backward_ros controller_interface + control_msgs controller_manager_msgs diagnostic_updater hardware_interface diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index df270aeb35..ce9709747a 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -77,24 +77,27 @@ TestChainableController::state_interface_configuration() const controller_interface::return_type TestChainableController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - for (size_t i = 0; i < reference_interfaces_.size(); ++i) + auto joint_commands = rt_command_ptr_.readFromRT(); + const auto itf_names = (*joint_commands)->interface_names; + const auto itf_values = (*joint_commands)->values; + + assert(exported_reference_interface_names_.size() == itf_names.size()); + + for (size_t i = 0; i < (*joint_commands)->interface_names.size(); ++i) { RCLCPP_INFO( get_node()->get_logger(), "Value of reference interface '%s' before checking external input is %f", - (std::string(get_node()->get_name()) + "/" + reference_interface_names_[i]).c_str(), - reference_interfaces_[i]); + itf_names[i].c_str(), reference_interfaces_[itf_names[i]]->get_value()); + reference_interfaces_[itf_names[i]]->set_value(itf_values[i]); } - auto joint_commands = rt_command_ptr_.readFromRT(); - reference_interfaces_ = (*joint_commands)->data; - for (size_t i = 0; i < reference_interfaces_.size(); ++i) + for (const auto & ref_itf_name : exported_reference_interface_names_) { RCLCPP_INFO( get_node()->get_logger(), "Updated value of reference interface '%s' after applying external input is %f", - (std::string(get_node()->get_name()) + "/" + reference_interface_names_[i]).c_str(), - reference_interfaces_[i]); + ref_itf_name.c_str(), reference_interfaces_[ref_itf_name]->get_value()); } return controller_interface::return_type::OK; @@ -107,13 +110,16 @@ controller_interface::return_type TestChainableController::update_and_write_comm for (size_t i = 0; i < command_interfaces_.size(); ++i) { - command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); + command_interfaces_[i].set_value( + reference_interfaces_[exported_reference_interface_names_[i]]->get_value() - + state_interfaces_[i].get_value()); } // If there is a command interface then integrate and set it to the exported state interface data for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size(); ++i) { - state_interfaces_values_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT; + exported_state_interfaces_[exported_state_interface_names_[i]]->set_value( + command_interfaces_[i].get_value() * CONTROLLER_DT); } // If there is no command interface and if there is a state interface then just forward the same // value as in the state interface @@ -121,7 +127,8 @@ controller_interface::return_type TestChainableController::update_and_write_comm command_interfaces_.empty(); ++i) { - state_interfaces_values_[i] = state_interfaces_[i].get_value(); + exported_state_interfaces_[exported_state_interface_names_[i]]->set_value( + state_interfaces_[i].get_value()); } return controller_interface::return_type::OK; @@ -138,7 +145,7 @@ CallbackReturn TestChainableController::on_configure( { auto joint_commands = rt_command_ptr_.readFromNonRT(); - if (msg->data.size() != (*joint_commands)->data.size()) + if (msg->interface_names.size() != (*joint_commands)->interface_names.size()) { rt_command_ptr_.writeFromNonRT(msg); } @@ -147,12 +154,13 @@ CallbackReturn TestChainableController::on_configure( RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(), 1000, "command size (%zu) does not match number of reference interfaces (%zu)", - (*joint_commands)->data.size(), reference_interfaces_.size()); + (*joint_commands)->interface_names.size(), reference_interfaces_.size()); } }); auto msg = std::make_shared(); - msg->data.resize(reference_interfaces_.size()); + msg->interface_names.resize(exported_reference_interface_names_.size()); + msg->values.resize(exported_reference_interface_names_.size()); rt_command_ptr_.writeFromNonRT(msg); return CallbackReturn::SUCCESS; @@ -164,7 +172,12 @@ CallbackReturn TestChainableController::on_activate( if (!is_in_chained_mode()) { auto msg = rt_command_ptr_.readFromRT(); - (*msg)->data = reference_interfaces_; + for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + { + (*msg)->interface_names[i] = exported_reference_interface_names_[i]; + (*msg)->values[i] = + reference_interfaces_[exported_reference_interface_names_[i]]->get_value(); + } } return CallbackReturn::SUCCESS; @@ -177,29 +190,32 @@ CallbackReturn TestChainableController::on_cleanup( return CallbackReturn::SUCCESS; } -std::vector -TestChainableController::on_export_state_interfaces() +std::vector +TestChainableController::export_state_interface_descriptions() { - std::vector state_interfaces; + using hardware_interface::InterfaceDescription; + using hardware_interface::InterfaceInfo; + std::vector state_interfaces; - for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) + for (const auto & interface_name : state_interface_names_) { - state_interfaces.push_back(hardware_interface::StateInterface( - get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); + state_interfaces.emplace_back(get_node()->get_name(), InterfaceInfo(interface_name, "double")); } return state_interfaces; } -std::vector -TestChainableController::on_export_reference_interfaces() +std::vector +TestChainableController::export_reference_interface_descriptions() { - std::vector reference_interfaces; + using hardware_interface::InterfaceDescription; + using hardware_interface::InterfaceInfo; + std::vector reference_interfaces; - for (size_t i = 0; i < reference_interface_names_.size(); ++i) + for (const auto & interface_name : reference_interface_names_) { - reference_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription( - get_node()->get_name(), InterfaceInfo(reference_interface_names_[i], "double")))); + reference_interfaces.emplace_back( + get_node()->get_name(), InterfaceInfo(interface_name, "double")); } return reference_interfaces; @@ -221,16 +237,12 @@ void TestChainableController::set_reference_interface_names( const std::vector & reference_interface_names) { reference_interface_names_ = reference_interface_names; - - reference_interfaces_.resize(reference_interface_names.size(), 0.0); } void TestChainableController::set_exported_state_interface_names( const std::vector & state_interface_names) { - exported_state_interface_names_ = state_interface_names; - - state_interfaces_values_.resize(exported_state_interface_names_.size(), 0.0); + state_interface_names_ = state_interface_names; } void TestChainableController::set_imu_sensor_name(const std::string & name) @@ -246,7 +258,7 @@ std::vector TestChainableController::get_state_interface_data() const std::vector state_intr_data; for (const auto & interface : state_interfaces_) { - state_intr_data.push_back(interface.get_value()); + state_intr_data.push_back(interface.get_value()); } return state_intr_data; } diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp index f4f59ad9df..9b3f846dbc 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -19,16 +19,16 @@ #include #include +#include "control_msgs/msg/interface_value.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "controller_manager/visibility_control.h" #include "rclcpp/subscription.hpp" #include "realtime_tools/realtime_buffer.h" #include "semantic_components/imu_sensor.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" namespace test_chainable_controller { -using CmdType = std_msgs::msg::Float64MultiArray; +using CmdType = control_msgs::msg::InterfaceValue; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; // indicating the node name under which the controller node @@ -63,10 +63,12 @@ class TestChainableController : public controller_interface::ChainableController CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; CONTROLLER_MANAGER_PUBLIC - std::vector on_export_state_interfaces() override; + std::vector export_state_interface_descriptions() + override; CONTROLLER_MANAGER_PUBLIC - std::vector on_export_reference_interfaces() override; + std::vector export_reference_interface_descriptions() + override; controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -98,7 +100,7 @@ class TestChainableController : public controller_interface::ChainableController controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; std::vector reference_interface_names_; - std::vector exported_state_interface_names_; + std::vector state_interface_names_; std::unique_ptr imu_sensor_; realtime_tools::RealtimeBuffer> rt_command_ptr_; diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7e3419fbc9..a6d4d3b218 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -132,7 +132,7 @@ std::vector TestController::get_state_interface_data() const std::vector state_intr_data; for (const auto & interface : state_interfaces_) { - state_intr_data.push_back(interface.get_value()); + state_intr_data.push_back(interface.get_value()); } return state_intr_data; } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 4a81f3bf12..d0e384c5dd 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -512,21 +512,40 @@ class TestControllerChainingWithControllerManager // check if values are set properly in controllers ASSERT_EQ( - diff_drive_controller->reference_interfaces_[0], reference[0]); // command from Position to + diff_drive_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + reference[0]); // command from Position to ASSERT_EQ( - diff_drive_controller->reference_interfaces_[1], reference[1]); // DiffDrive is forwarded + diff_drive_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[1]] + ->get_value(), + reference[1]); // DiffDrive is forwarded // Command of DiffDrive controller are references of PID controllers EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(diff_drive_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_REF); - ASSERT_EQ(diff_drive_controller->command_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_REF); - ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); - ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + ASSERT_EQ( + diff_drive_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_REF); + ASSERT_EQ( + diff_drive_controller->command_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_REF); + ASSERT_EQ( + pid_left_wheel_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + EXP_LEFT_WHEEL_REF); + ASSERT_EQ( + pid_right_wheel_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + EXP_RIGHT_WHEEL_REF); EXP_STATE_ODOM_X = chained_estimate_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); EXP_STATE_ODOM_Y = chained_estimate_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); - ASSERT_EQ(sensor_fusion_controller->state_interfaces_values_.size(), 3u); + ASSERT_EQ(sensor_fusion_controller->exported_state_interfaces_.size(), 3u); + ASSERT_EQ( + sensor_fusion_controller->exported_state_interfaces_.size(), + sensor_fusion_controller->exported_state_interface_names_.size()); ASSERT_EQ(robot_localization_controller->get_state_interface_data().size(), 3u); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); @@ -536,21 +555,27 @@ class TestControllerChainingWithControllerManager EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ( + pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); ASSERT_EQ( - pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value(), + EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); @@ -759,17 +784,25 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) std::vector reference = {32.0, 128.0}; // update 'Position Tracking' controller - for (auto & value : diff_drive_controller->reference_interfaces_) + for (auto & [key, ref_itf] : diff_drive_controller->reference_interfaces_) { - ASSERT_EQ(value, 0.0); // default reference values are 0.0 + ASSERT_EQ(ref_itf->get_value(), 0.0); // default reference values are 0.0 } position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(position_tracking_controller->internal_counter, 8u); - ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller - ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through + ASSERT_EQ( + diff_drive_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + reference[0]); // position_controller + ASSERT_EQ( + diff_drive_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[1]] + ->get_value(), + reference[1]); // is pass-through // update 'Diff Drive' Controller diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -777,8 +810,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 - ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); - ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + ASSERT_EQ( + pid_left_wheel_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + EXP_LEFT_WHEEL_REF); + ASSERT_EQ( + pid_right_wheel_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + EXP_RIGHT_WHEEL_REF); // run the update cycles of the robot localization and odom publisher controller sensor_fusion_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -810,10 +851,13 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); // 32 / 2 EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ( + pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X); @@ -822,13 +866,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); // 128 / 2 EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); ASSERT_EQ(odom_publisher_controller->internal_counter, 2u); ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u); ASSERT_EQ(robot_localization_controller->internal_counter, 4u); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // The state doesn't change wrt to any data from the hardware calculation ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y); @@ -1880,17 +1927,25 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add odom_publisher_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // update 'Position Tracking' controller - for (auto & value : diff_drive_controller->reference_interfaces_) + for (auto & [key, ref_itf] : diff_drive_controller->reference_interfaces_) { - ASSERT_EQ(value, 0.0); // default reference values are 0.0 + ASSERT_EQ(ref_itf->get_value(), 0.0); // default reference values are 0.0 } position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); ASSERT_EQ(position_tracking_controller->internal_counter, 8u); - ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller - ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through + ASSERT_EQ( + diff_drive_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + reference[0]); // position_controller + ASSERT_EQ( + diff_drive_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[1]] + ->get_value(), + reference[1]); // is pass-through // update 'Diff Drive' Controller diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -1898,8 +1953,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add // default reference values are 0.0 - they should be changed now EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 - ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); - ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + ASSERT_EQ( + pid_left_wheel_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + EXP_LEFT_WHEEL_REF); + ASSERT_EQ( + pid_right_wheel_controller + ->reference_interfaces_[diff_drive_controller->exported_reference_interface_names_[0]] + ->get_value(), + EXP_RIGHT_WHEEL_REF); // update PID controllers that are writing to hardware pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); @@ -1913,19 +1976,25 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); // 32 / 2 EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); - ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ( + pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); // 128 - 0 EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); // 128 / 2 EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); - ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // DiffDrive uses the same state - ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ( + diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); // update all controllers at once and see that all have expected values --> also checks the order // of controller execution diff --git a/doc/Iron.rst b/doc/Iron.rst index 3a463c736a..eba360273f 100644 --- a/doc/Iron.rst +++ b/doc/Iron.rst @@ -113,7 +113,7 @@ Migration of unlisted Command-/StateInterfaces not defined in ``ros2_control`` X -------------------------------------------------------------------------------------- If you want some unlisted ``Command-/StateInterfaces`` not included in the ``ros2_control`` XML-tag you can follow those steps: -1. Override the ``virtual std::vector export_command_interfaces_2()`` or ``virtual std::vector export_state_interfaces_2()`` +1. Override the ``virtual std::vector export_command_interface_descriptions()`` or ``virtual std::vector export_state_interface_descriptions()`` 2. Create the InterfaceDescription for each of the interfaces you want to create in the override ``export_command_interfaces_2()`` or ``export_state_interfaces_2()`` function, add it to a vector and return the vector: .. code-block:: c++ diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 6f318f8e09..ffc0ea1607 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -210,6 +210,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -233,14 +235,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // and if so call on_export_state_interfaces() return {}; } + // END /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_state_interfaces_ map. * - * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated - * version is removed. - * * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector @@ -295,6 +295,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return state_interfaces; } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed /// Exports all command interfaces for this hardware interface. /** * Old way of exporting the CommandInterfaces. If a empty vector is returned then @@ -318,14 +320,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // and if so call on_export_command_interfaces() return {}; } + // END /** * Override this method to export custom CommandInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_command_interfaces_ map. * - * Note method name is going to be changed to export_command_interfaces() as soon as the - * deprecated version is removed. - * * \return vector of descriptions to the unlisted CommandInterfaces */ virtual std::vector @@ -544,7 +544,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp_lifecycle::State lifecycle_state_; rclcpp::Logger actuator_logger_; + std::unordered_map> actuator_states_; std::unordered_map> actuator_commands_; @@ -553,8 +555,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::shared_ptr error_signal_message_; std::shared_ptr warning_signal_; std::shared_ptr warning_signal_message_; - - rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 04dccd74ac..2aa39f8bd5 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -39,25 +39,49 @@ struct InterfaceInfo initial_value = ""; data_type = ""; size = 0; + enable_limits = false; } - explicit InterfaceInfo(const std::string & name_in) : InterfaceInfo() { name = name_in; } + explicit InterfaceInfo(const std::string & name_in, const bool & enable_limits_in = false) + : InterfaceInfo() + { + name = name_in; + enable_limits = enable_limits_in; + } - explicit InterfaceInfo(const std::string & name_in, const std::string & data_type_in) + explicit InterfaceInfo( + const std::string & name_in, const std::string & data_type_in, + const bool & enable_limits_in = false) : InterfaceInfo() { name = name_in; data_type = data_type_in; + enable_limits = enable_limits_in; } explicit InterfaceInfo( - const std::string & name_in, const std::string & initial_value_in, - const std::string & data_type_in) + const std::string & name_in, const std::string & data_type_in, + const std::string & initial_value_in, const bool & enable_limits_in = false) : InterfaceInfo() { name = name_in; + data_type = data_type_in; initial_value = initial_value_in; + enable_limits = enable_limits_in; + } + + explicit InterfaceInfo( + const std::string & name_in, const std::string & data_type_in, + const std::string & initial_value_in, const std::string & min_in, const std::string & max_in, + const bool & enable_limits_in = false) + : InterfaceInfo() + { + name = name_in; data_type = data_type_in; + initial_value = initial_value_in; + min = min_in; + max = max_in; + enable_limits = enable_limits_in; } /** @@ -65,14 +89,14 @@ struct InterfaceInfo * Used by joints and GPIOs. */ std::string name; + /// (Optional) The datatype of the interface, e.g. "bool", "int". + std::string data_type; + /// (Optional) Initial value of the interface. + std::string initial_value; /// (Optional) Minimal allowed values of the interface. std::string min; /// (Optional) Maximal allowed values of the interface. std::string max; - /// (Optional) Initial value of the interface. - std::string initial_value; - /// (Optional) The datatype of the interface, e.g. "bool", "int". - std::string data_type; /// (Optional) If the handle is an array, the size of the array. int size; /// (Optional) enable or disable the limits for the command interfaces diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 6fc445ac29..0c58b75dd4 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -63,9 +63,17 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + template ::value, int>::type = 0> + void set_value(T val) + { + command_interface_.set_value(val); + } - double get_value() const { return command_interface_.get_value(); } + template ::value, int>::type = 0> + T get_value() const + { + return command_interface_.get_value(); + } protected: CommandInterface & command_interface_; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 11abae41d0..92e626d3e5 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -20,6 +20,7 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/types/handle_datatype.hpp" namespace hardware_interface { @@ -63,7 +64,11 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + template ::value, int>::type = 0> + T get_value() const + { + return state_interface_.get_value(); + } protected: StateInterface & state_interface_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index ab27a5c718..df318f359e 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -187,6 +187,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -209,14 +211,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // and if so call on_export_state_interfaces() return {}; } + // END /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_state_interfaces_ map. * - * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated - * version is removed. - * * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector @@ -375,15 +375,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp_lifecycle::State lifecycle_state_; rclcpp::Logger sensor_logger_; + std::unordered_map> sensor_states_; std::shared_ptr error_signal_; std::shared_ptr error_signal_message_; std::shared_ptr warning_signal_; std::shared_ptr warning_signal_message_; - - rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 4871123664..2181aa1b50 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -230,6 +230,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -252,14 +254,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // and if so call on_export_state_interfaces() return {}; } + // END /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_state_interfaces_ map. * - * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated - * version is removed. - * * \return vector of descriptions to the unlisted StateInterfaces */ virtual std::vector @@ -327,6 +327,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return state_interfaces; } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed /// Exports all command interfaces for this hardware interface. /** * Old way of exporting the CommandInterfaces. If a empty vector is returned then @@ -350,14 +352,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // and if so call on_export_command_interfaces() return {}; } + // END /** * Override this method to export custom CommandInterfaces which are not defined in the URDF file. * Those interfaces will be added to the unlisted_command_interfaces_ map. * - * Note method name is going to be changed to export_command_interfaces() as soon as the - * deprecated version is removed. - * * \return vector of descriptions to the unlisted CommandInterfaces */ virtual std::vector @@ -588,7 +588,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp_lifecycle::State lifecycle_state_; rclcpp::Logger system_logger_; + std::unordered_map> system_states_; std::unordered_map> system_commands_; @@ -597,8 +599,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::shared_ptr error_signal_message_; std::shared_ptr warning_signal_; std::shared_ptr warning_signal_message_; - - rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index d1861991ff..1c72726382 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -44,7 +44,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste public: CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - std::vector export_command_interfaces_2() override; + std::vector export_state_interface_descriptions() + override; return_type prepare_command_mode_switch( const std::vector & start_interfaces, diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index df7e1d71d3..5b1eabb188 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -250,7 +250,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i return CallbackReturn::SUCCESS; } -std::vector GenericSystem::export_command_interfaces_2() +std::vector +GenericSystem::export_state_interface_descriptions() { // we check if we should mock command interfaces or not. After they have been exported we can then // use them as we would normally via (set/get)_(state/command) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 854b35c8d3..b2b01face4 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -776,10 +776,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.7854, j2p_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_EQ(1.57, j1p_s.get_value()); + ASSERT_EQ(0.7854, j2p_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); } // Test inspired by hardware_interface/test_resource_manager.cpp @@ -825,10 +825,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) hardware_interface::LoanedCommandInterface j2a_c = rm.claim_command_interface("joint2/acceleration"); - ASSERT_EQ(1.57, j1v_s.get_value()); - ASSERT_EQ(0.7854, j2p_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2a_c.get_value())); + ASSERT_EQ(1.57, j1v_s.get_value()); + ASSERT_EQ(0.7854, j2p_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value())); } void generic_system_functional_test( @@ -862,14 +862,14 @@ void generic_system_functional_test( hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); @@ -878,36 +878,36 @@ void generic_system_functional_test( j2v_c.set_value(0.44); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // write() does not change values ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // read() mirrors commands + offset to states ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - ASSERT_EQ(0.11 + offset, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(0.11 + offset, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33 + offset, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // set some new values in commands j1p_c.set_value(0.55); @@ -916,14 +916,14 @@ void generic_system_functional_test( j2v_c.set_value(0.88); // state values should not be changed - ASSERT_EQ(0.11 + offset, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33 + offset, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.55, j1p_c.get_value()); - ASSERT_EQ(0.66, j1v_c.get_value()); - ASSERT_EQ(0.77, j2p_c.get_value()); - ASSERT_EQ(0.88, j2v_c.get_value()); + ASSERT_EQ(0.11 + offset, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33 + offset, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.55, j1p_c.get_value()); + ASSERT_EQ(0.66, j1v_c.get_value()); + ASSERT_EQ(0.77, j2p_c.get_value()); + ASSERT_EQ(0.88, j2v_c.get_value()); deactivate_components(rm, {component_name}); status_map = rm.get_components_status(); @@ -965,14 +965,14 @@ void generic_system_error_group_test( hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); @@ -981,36 +981,36 @@ void generic_system_error_group_test( j2v_c.set_value(0.44); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // write() does not change values ASSERT_TRUE(rm.write(TIME, PERIOD).ok); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // read() mirrors commands to states ASSERT_TRUE(rm.read(TIME, PERIOD).ok); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // set some new values in commands j1p_c.set_value(0.55); @@ -1019,14 +1019,14 @@ void generic_system_error_group_test( j2v_c.set_value(0.88); // state values should not be changed - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.55, j1p_c.get_value()); - ASSERT_EQ(0.66, j1v_c.get_value()); - ASSERT_EQ(0.77, j2p_c.get_value()); - ASSERT_EQ(0.88, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.55, j1p_c.get_value()); + ASSERT_EQ(0.66, j1v_c.get_value()); + ASSERT_EQ(0.77, j2p_c.get_value()); + ASSERT_EQ(0.88, j2v_c.get_value()); // Error testing j1p_c.set_value(std::numeric_limits::infinity()); @@ -1146,14 +1146,14 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) hardware_interface::LoanedCommandInterface vo_c = rm.claim_command_interface("voltage_output/voltage"); - ASSERT_EQ(1.55, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.65, j2p_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.5, vo_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(vo_c.get_value())); + ASSERT_EQ(1.55, j1p_s.get_value()); + ASSERT_EQ(0.1, j1v_s.get_value()); + ASSERT_EQ(0.65, j2p_s.get_value()); + ASSERT_EQ(0.2, j2v_s.get_value()); + ASSERT_EQ(0.5, vo_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_TRUE(std::isnan(vo_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); @@ -1161,36 +1161,36 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) vo_c.set_value(0.99); // State values should not be changed - ASSERT_EQ(1.55, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.65, j2p_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.5, vo_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.99, vo_c.get_value()); + ASSERT_EQ(1.55, j1p_s.get_value()); + ASSERT_EQ(0.1, j1v_s.get_value()); + ASSERT_EQ(0.65, j2p_s.get_value()); + ASSERT_EQ(0.2, j2v_s.get_value()); + ASSERT_EQ(0.5, vo_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.99, vo_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(1.55, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.65, j2p_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.5, vo_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.99, vo_c.get_value()); + ASSERT_EQ(1.55, j1p_s.get_value()); + ASSERT_EQ(0.1, j1v_s.get_value()); + ASSERT_EQ(0.65, j2p_s.get_value()); + ASSERT_EQ(0.2, j2v_s.get_value()); + ASSERT_EQ(0.5, vo_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.99, vo_c.get_value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.1, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.99, vo_s.get_value()); - ASSERT_EQ(0.2, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.99, vo_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.1, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.99, vo_s.get_value()); + ASSERT_EQ(0.2, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.99, vo_c.get_value()); } TEST_F(TestGenericSystem, generic_system_2dof_sensor) @@ -1239,58 +1239,58 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) EXPECT_ANY_THROW(rm.claim_command_interface("tcp_force_sensor/tx")); EXPECT_ANY_THROW(rm.claim_command_interface("tcp_force_sensor/ty")); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_EQ(0.0, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value())); + EXPECT_TRUE(std::isnan(stx_s.get_value())); + EXPECT_TRUE(std::isnan(sty_s.get_value())); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); j2p_c.set_value(0.33); // State values should not be changed - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value())); + EXPECT_TRUE(std::isnan(stx_s.get_value())); + EXPECT_TRUE(std::isnan(sty_s.get_value())); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value())); + EXPECT_TRUE(std::isnan(stx_s.get_value())); + EXPECT_TRUE(std::isnan(sty_s.get_value())); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value())); + EXPECT_TRUE(std::isnan(stx_s.get_value())); + EXPECT_TRUE(std::isnan(sty_s.get_value())); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); } void TestGenericSystem::test_generic_system_with_mock_sensor_commands( @@ -1342,20 +1342,20 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( hardware_interface::LoanedCommandInterface sty_c = rm.claim_command_interface("tcp_force_sensor/ty"); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - EXPECT_TRUE(std::isnan(sfx_c.get_value())); - EXPECT_TRUE(std::isnan(sfy_c.get_value())); - EXPECT_TRUE(std::isnan(stx_c.get_value())); - EXPECT_TRUE(std::isnan(sty_c.get_value())); + ASSERT_EQ(0.0, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value())); + EXPECT_TRUE(std::isnan(stx_s.get_value())); + EXPECT_TRUE(std::isnan(sty_s.get_value())); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); + EXPECT_TRUE(std::isnan(sfx_c.get_value())); + EXPECT_TRUE(std::isnan(sfy_c.get_value())); + EXPECT_TRUE(std::isnan(stx_c.get_value())); + EXPECT_TRUE(std::isnan(sty_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); @@ -1366,54 +1366,54 @@ void TestGenericSystem::test_generic_system_with_mock_sensor_commands( sty_c.set_value(4.44); // State values should not be changed - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(1.11, sfx_c.get_value()); - ASSERT_EQ(2.22, sfy_c.get_value()); - ASSERT_EQ(3.33, stx_c.get_value()); - ASSERT_EQ(4.44, sty_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value())); + EXPECT_TRUE(std::isnan(stx_s.get_value())); + EXPECT_TRUE(std::isnan(sty_s.get_value())); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(1.11, sfx_c.get_value()); + ASSERT_EQ(2.22, sfy_c.get_value()); + ASSERT_EQ(3.33, stx_c.get_value()); + ASSERT_EQ(4.44, sty_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(0.0, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - EXPECT_TRUE(std::isnan(sfx_s.get_value())); - EXPECT_TRUE(std::isnan(sfy_s.get_value())); - EXPECT_TRUE(std::isnan(stx_s.get_value())); - EXPECT_TRUE(std::isnan(sty_s.get_value())); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(1.11, sfx_c.get_value()); - ASSERT_EQ(2.22, sfy_c.get_value()); - ASSERT_EQ(3.33, stx_c.get_value()); - ASSERT_EQ(4.44, sty_c.get_value()); + ASSERT_EQ(0.0, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + EXPECT_TRUE(std::isnan(sfx_s.get_value())); + EXPECT_TRUE(std::isnan(sfy_s.get_value())); + EXPECT_TRUE(std::isnan(stx_s.get_value())); + EXPECT_TRUE(std::isnan(sty_s.get_value())); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(1.11, sfx_c.get_value()); + ASSERT_EQ(2.22, sfy_c.get_value()); + ASSERT_EQ(3.33, stx_c.get_value()); + ASSERT_EQ(4.44, sty_c.get_value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(1.11, sfx_s.get_value()); - ASSERT_EQ(2.22, sfy_s.get_value()); - ASSERT_EQ(3.33, stx_s.get_value()); - ASSERT_EQ(4.44, sty_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(1.11, sfx_c.get_value()); - ASSERT_EQ(2.22, sfy_c.get_value()); - ASSERT_EQ(3.33, stx_c.get_value()); - ASSERT_EQ(4.44, sty_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(1.11, sfx_s.get_value()); + ASSERT_EQ(2.22, sfy_s.get_value()); + ASSERT_EQ(3.33, stx_s.get_value()); + ASSERT_EQ(4.44, sty_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(1.11, sfx_c.get_value()); + ASSERT_EQ(2.22, sfy_c.get_value()); + ASSERT_EQ(3.33, stx_c.get_value()); + ASSERT_EQ(4.44, sty_c.get_value()); } TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) @@ -1460,42 +1460,42 @@ void TestGenericSystem::test_generic_system_with_mimic_joint( hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_EQ(1.57, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); j1v_c.set_value(0.05); // State values should not be changed - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.05, j1v_c.get_value()); + ASSERT_EQ(1.57, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.05, j1v_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.0, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.05, j1v_c.get_value()); + ASSERT_EQ(1.57, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.0, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.05, j1v_c.get_value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.05, j1v_s.get_value()); - ASSERT_EQ(-0.22, j2p_s.get_value()); - ASSERT_EQ(-0.1, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.05, j1v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.05, j1v_s.get_value()); + ASSERT_EQ(-0.22, j2p_s.get_value()); + ASSERT_EQ(-0.1, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.05, j1v_c.get_value()); } TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) @@ -1569,14 +1569,14 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i rm.claim_state_interface("joint2/actual_position"); // State interfaces without initial value are set to 0 - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2p_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); @@ -1585,38 +1585,38 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i j2v_c.set_value(0.44); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(2.78, j2p_s.get_value()); - ASSERT_EQ(0.0, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(2.78, j2p_s.get_value()); + ASSERT_EQ(0.0, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // read() mirrors commands + offset to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.11 + offset, c_j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.33 + offset, c_j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.22, j1v_c.get_value()); - ASSERT_EQ(0.33, j2p_c.get_value()); - ASSERT_EQ(0.44, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.11 + offset, c_j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.33 + offset, c_j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.22, j1v_c.get_value()); + ASSERT_EQ(0.33, j2p_c.get_value()); + ASSERT_EQ(0.44, j2v_c.get_value()); // set some new values in commands j1p_c.set_value(0.55); @@ -1625,16 +1625,16 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i j2v_c.set_value(0.88); // state values should not be changed - ASSERT_EQ(0.11, j1p_s.get_value()); - ASSERT_EQ(0.11 + offset, c_j1p_s.get_value()); - ASSERT_EQ(0.22, j1v_s.get_value()); - ASSERT_EQ(0.33, j2p_s.get_value()); - ASSERT_EQ(0.33 + offset, c_j2p_s.get_value()); - ASSERT_EQ(0.44, j2v_s.get_value()); - ASSERT_EQ(0.55, j1p_c.get_value()); - ASSERT_EQ(0.66, j1v_c.get_value()); - ASSERT_EQ(0.77, j2p_c.get_value()); - ASSERT_EQ(0.88, j2v_c.get_value()); + ASSERT_EQ(0.11, j1p_s.get_value()); + ASSERT_EQ(0.11 + offset, c_j1p_s.get_value()); + ASSERT_EQ(0.22, j1v_s.get_value()); + ASSERT_EQ(0.33, j2p_s.get_value()); + ASSERT_EQ(0.33 + offset, c_j2p_s.get_value()); + ASSERT_EQ(0.44, j2v_s.get_value()); + ASSERT_EQ(0.55, j1p_c.get_value()); + ASSERT_EQ(0.66, j1v_c.get_value()); + ASSERT_EQ(0.77, j2p_c.get_value()); + ASSERT_EQ(0.88, j2v_c.get_value()); deactivate_components(rm, {hardware_name}); status_map = rm.get_components_status(); @@ -1697,44 +1697,44 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) rm.claim_command_interface("flange_vacuum/vacuum"); // State interfaces without initial value are set to 0 - ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_TRUE(std::isnan(gpio1_a_o1_c.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_c.get_value())); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); + ASSERT_TRUE(std::isnan(gpio1_a_o1_c.get_value())); + ASSERT_TRUE(std::isnan(gpio2_vac_c.get_value())); // set some new values in commands gpio1_a_o1_c.set_value(0.111); gpio2_vac_c.set_value(0.222); // State values should not be changed - ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.222, gpio2_vac_c.get_value()); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.222, gpio2_vac_c.get_value()); + ASSERT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); + ASSERT_TRUE(std::isnan(gpio2_vac_s.get_value())); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_value()); // read() mirrors commands + offset to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.111, gpio1_a_o1_s.get_value()); - ASSERT_EQ(0.222, gpio2_vac_s.get_value()); - ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.222, gpio2_vac_c.get_value()); + ASSERT_EQ(0.111, gpio1_a_o1_s.get_value()); + ASSERT_EQ(0.222, gpio2_vac_s.get_value()); + ASSERT_EQ(0.111, gpio1_a_o1_c.get_value()); + ASSERT_EQ(0.222, gpio2_vac_c.get_value()); // set some new values in commands gpio1_a_o1_c.set_value(0.333); gpio2_vac_c.set_value(0.444); // state values should not be changed - ASSERT_EQ(0.111, gpio1_a_o1_s.get_value()); - ASSERT_EQ(0.222, gpio2_vac_s.get_value()); - ASSERT_EQ(0.333, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.444, gpio2_vac_c.get_value()); + ASSERT_EQ(0.111, gpio1_a_o1_s.get_value()); + ASSERT_EQ(0.222, gpio2_vac_s.get_value()); + ASSERT_EQ(0.333, gpio1_a_o1_c.get_value()); + ASSERT_EQ(0.444, gpio2_vac_c.get_value()); // check other functionalities are working well generic_system_functional_test(urdf, hardware_name); @@ -1799,14 +1799,14 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( hardware_interface::LoanedCommandInterface gpio2_vac_c = rm.claim_command_interface("flange_vacuum/vacuum"); - EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_c.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_o1_c.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_c.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_i2_c.get_value())); + EXPECT_TRUE(std::isnan(gpio2_vac_c.get_value())); // set some new values in commands gpio1_a_o1_c.set_value(0.11); @@ -1815,36 +1815,36 @@ void TestGenericSystem::test_generic_system_with_mock_gpio_commands( gpio2_vac_c.set_value(2.22); // State values should not be changed - EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); - ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_value()); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); - EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); - EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); - ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); - ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_value()); + EXPECT_TRUE(std::isnan(gpio1_a_o1_s.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_i1_s.get_value())); + EXPECT_TRUE(std::isnan(gpio1_a_o2_s.get_value())); + EXPECT_TRUE(std::isnan(gpio2_vac_s.get_value())); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_value()); // read() mirrors commands to states rm.read(TIME, PERIOD); - ASSERT_EQ(0.11, gpio1_a_o1_s.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_s.get_value()); - ASSERT_EQ(1.11, gpio1_a_o2_s.get_value()); - ASSERT_EQ(2.22, gpio2_vac_s.get_value()); - ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); - ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); - ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); - ASSERT_EQ(2.22, gpio2_vac_c.get_value()); + ASSERT_EQ(0.11, gpio1_a_o1_s.get_value()); + ASSERT_EQ(0.33, gpio1_a_i1_s.get_value()); + ASSERT_EQ(1.11, gpio1_a_o2_s.get_value()); + ASSERT_EQ(2.22, gpio2_vac_s.get_value()); + ASSERT_EQ(0.11, gpio1_a_o1_c.get_value()); + ASSERT_EQ(0.33, gpio1_a_i1_c.get_value()); + ASSERT_EQ(1.11, gpio1_a_i2_c.get_value()); + ASSERT_EQ(2.22, gpio2_vac_c.get_value()); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command) @@ -1888,9 +1888,9 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) hardware_interface::LoanedStateInterface force_z_s = rm.claim_state_interface("force_sensor/force.z"); - ASSERT_EQ(0.0, force_x_s.get_value()); - ASSERT_EQ(0.0, force_y_s.get_value()); - ASSERT_EQ(0.0, force_z_s.get_value()); + ASSERT_EQ(0.0, force_x_s.get_value()); + ASSERT_EQ(0.0, force_y_s.get_value()); + ASSERT_EQ(0.0, force_z_s.get_value()); } TEST_F(TestGenericSystem, gpio_with_initial_value) @@ -1909,7 +1909,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) // Check initial values hardware_interface::LoanedStateInterface state = rm.claim_state_interface("sample_io/output_1"); - ASSERT_EQ(1, state.get_value()); + ASSERT_EQ(1, state.get_value()); } TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) @@ -1953,16 +1953,16 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) hardware_interface::LoanedCommandInterface j2a_c = rm.claim_command_interface("joint2/acceleration"); - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_TRUE(std::isnan(j2a_c.get_value())); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value())); // Test error management in prepare mode switch ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface @@ -1985,68 +1985,68 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) j2a_c.set_value(3.5); // State values should not be changed - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(-33.4, j1v_s.get_value()); - EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(-33.4, j1v_s.get_value()); + EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); // read() mirrors commands to states and calculate dynamics rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); // switch controller mode as controller manager is doing ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); @@ -2057,55 +2057,55 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) j2v_c.set_value(2.0); // State values should not be changed - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); // read() mirrors commands to states and calculate dynamics (both velocity mode) rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.5, j1v_s.get_value()); - EXPECT_EQ(5.0, j1a_s.get_value()); - EXPECT_EQ(2.885, j2p_s.get_value()); - EXPECT_EQ(2.0, j2v_s.get_value()); - EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(5.0, j1a_s.get_value()); + EXPECT_EQ(2.885, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); // read() mirrors commands to states and calculate dynamics (both velocity mode) rm.read(TIME, PERIOD); - EXPECT_EQ(0.16, j1p_s.get_value()); - EXPECT_EQ(0.5, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(3.085, j2p_s.get_value()); - EXPECT_EQ(2.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); + EXPECT_EQ(0.16, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(3.085, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); } TEST_F(TestGenericSystem, disabled_commands_flag_is_active) @@ -2130,29 +2130,29 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); // set some new values in commands j1p_c.set_value(0.11); // State values should not be changed - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); // write() does not change values rm.write(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); // read() also does not change values rm.read(TIME, PERIOD); - ASSERT_EQ(3.45, j1p_s.get_value()); - ASSERT_EQ(0.0, j1v_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(3.45, j1p_s.get_value()); + ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); } TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index d90b616a4b..0a07ba2fba 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -378,7 +378,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -496,7 +497,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - auto state = sensor_hw.initialize(voltage_sensor_res); + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -582,7 +584,8 @@ TEST(TestComponentInterfaces, dummy_system_default) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - auto state = system_hw.initialize(dummy_system); + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -749,7 +752,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -880,7 +884,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - auto state = actuator_hw.initialize(dummy_actuator); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_component"); + auto state = actuator_hw.initialize(dummy_actuator, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -988,7 +993,8 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; - auto state = sensor_hw.initialize(voltage_sensor_res); + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_component"); + auto state = sensor_hw.initialize(voltage_sensor_res, logger, nullptr); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -1082,7 +1088,8 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - auto state = system_hw.initialize(dummy_system); + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1213,7 +1220,8 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) const std::vector control_resources = hardware_interface::parse_control_resources_from_urdf(urdf_to_test); const hardware_interface::HardwareInfo dummy_system = control_resources[0]; - auto state = system_hw.initialize(dummy_system); + rclcpp::Logger logger = rclcpp::get_logger("test_system_component"); + auto state = system_hw.initialize(dummy_system, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index ec7a29f40b..5ea732b0b6 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -54,7 +54,8 @@ class TestForceTorqueSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector state_interfaces; diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index bb41f16228..0144f6afe3 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -43,10 +43,11 @@ class TestIMUSensor : public SensorInterface { return CallbackReturn::ERROR; } - for (const auto & imu_key : - {"orientation.x", "orientation.y", "orientation.z", "orientation.w", "angular_velocity.x", - "angular_velocity.y", "angular_velocity.z", "linear_acceleration.x", - "linear_acceleration.y", "linear_acceleration.z"}) + std::vector imu_keys; + imu_keys.insert(imu_keys.end(), quat_.begin(), quat_.end()); + imu_keys.insert(imu_keys.end(), ang_vel_.begin(), ang_vel_.end()); + imu_keys.insert(imu_keys.end(), lin_acc_.begin(), lin_acc_.end()); + for (const auto & imu_key : imu_keys) { if ( std::find_if( @@ -61,31 +62,34 @@ class TestIMUSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interface_descriptions() + override { - std::vector state_interfaces; + using hardware_interface::InterfaceInfo; + std::vector state_interfaces; const std::string & sensor_name = info_.sensors[0].name; + + state_interfaces.emplace_back( + sensor_name, InterfaceInfo(quat_.x, quat_.data_type, quat_.initial_x)); + state_interfaces.emplace_back( + sensor_name, InterfaceInfo(quat_.y, quat_.data_type, quat_.initial_y)); state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); + sensor_name, InterfaceInfo(quat_.z, quat_.data_type, quat_.initial_z)); state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.y", &orientation_.y)); + sensor_name, InterfaceInfo(quat_.w, quat_.data_type, quat_.initial_w)); state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.z", &orientation_.z)); + sensor_name, InterfaceInfo(ang_vel_.x, ang_vel_.data_type, ang_vel_.initial_x)); state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "orientation.w", &orientation_.w)); + sensor_name, InterfaceInfo(ang_vel_.y, ang_vel_.data_type, ang_vel_.initial_y)); state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "angular_velocity.x", &angular_velocity_.x)); + sensor_name, InterfaceInfo(ang_vel_.z, ang_vel_.data_type, ang_vel_.initial_z)); state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "angular_velocity.y", &angular_velocity_.y)); + sensor_name, InterfaceInfo(lin_acc_.x, lin_acc_.data_type, lin_acc_.initial_x)); state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "angular_velocity.z", &angular_velocity_.z)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - sensor_name, "linear_acceleration.x", &linear_acceleration_.x)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - sensor_name, "linear_acceleration.y", &linear_acceleration_.y)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - sensor_name, "linear_acceleration.z", &linear_acceleration_.z)); + sensor_name, InterfaceInfo(lin_acc_.y, lin_acc_.data_type, lin_acc_.initial_y)); + state_interfaces.emplace_back( + sensor_name, InterfaceInfo(lin_acc_.z, lin_acc_.data_type, lin_acc_.initial_z)); return state_interfaces; } @@ -97,42 +101,87 @@ class TestIMUSensor : public SensorInterface const double u1 = distribution_1(generator_); const double u2 = distribution_1(generator_); const double u3 = distribution_1(generator_); - orientation_.w = std::sqrt(1. - u1) * std::sin(2 * M_PI * u2); - orientation_.x = std::sqrt(1. - u1) * std::cos(2 * M_PI * u2); - orientation_.y = std::sqrt(u1) * std::sin(2 * M_PI * u3); - orientation_.z = std::sqrt(u1) * std::cos(2 * M_PI * u3); + set_state(quat_.w, std::sqrt(1. - u1) * std::sin(2 * M_PI * u2)); + set_state(quat_.x, std::sqrt(1. - u1) * std::cos(2 * M_PI * u2)); + set_state(quat_.y, std::sqrt(u1) * std::sin(2 * M_PI * u3)); + set_state(quat_.z, std::sqrt(u1) * std::cos(2 * M_PI * u3)); // generate random angular velocities and linear accelerations std::uniform_real_distribution distribution_2(0.0, 0.1); - angular_velocity_.x = distribution_2(generator_); - angular_velocity_.y = distribution_2(generator_); - angular_velocity_.z = distribution_2(generator_); + set_state(ang_vel_.x, distribution_2(generator_)); + set_state(ang_vel_.y, distribution_2(generator_)); + set_state(ang_vel_.z, distribution_2(generator_)); - linear_acceleration_.x = distribution_2(generator_); - linear_acceleration_.y = distribution_2(generator_); - linear_acceleration_.z = distribution_2(generator_); + set_state(lin_acc_.x, distribution_2(generator_)); + set_state(lin_acc_.y, distribution_2(generator_)); + set_state(lin_acc_.z, distribution_2(generator_)); return return_type::OK; } private: - struct QuaternionValues + struct Quaternion + { + const std::string x = "orientation.x"; + const std::string y = "orientation.y"; + const std::string z = "orientation.z"; + const std::string w = "orientation.w"; + + const std::string data_type = "double"; + + const std::string initial_x = "0.0"; + const std::string initial_y = "0.0"; + const std::string initial_z = "0.0"; + const std::string initial_w = "1.0"; + + std::vector::const_iterator begin() const { return component_names_.begin(); } + std::vector::const_iterator end() const { return component_names_.end(); } + + private: + const std::vector component_names_{x, y, z, w}; + }; + + struct AngularVel { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double w = 1.0; + const std::string x = "angular_velocity.x"; + const std::string y = "angular_velocity.y"; + const std::string z = "angular_velocity.z"; + + const std::string data_type = "double"; + + const std::string initial_x = "0.0"; + const std::string initial_y = "0.0"; + const std::string initial_z = "0.0"; + + std::vector::const_iterator begin() const { return component_names_.begin(); } + std::vector::const_iterator end() const { return component_names_.end(); } + + private: + const std::vector component_names_{x, y, z}; }; - struct AxisValues + + struct LinearAccel { - double x = 0.0; - double y = 0.0; - double z = 0.0; + const std::string x = "linear_acceleration.x"; + const std::string y = "linear_acceleration.y"; + const std::string z = "linear_acceleration.z"; + + const std::string data_type = "double"; + + const std::string initial_x = "0.0"; + const std::string initial_y = "0.0"; + const std::string initial_z = "0.0"; + + std::vector::const_iterator begin() const { return component_names_.begin(); } + std::vector::const_iterator end() const { return component_names_.end(); } + + private: + const std::vector component_names_{x, y, z}; }; std::default_random_engine generator_; - QuaternionValues orientation_; - AxisValues angular_velocity_; - AxisValues linear_acceleration_; + Quaternion quat_; + AngularVel ang_vel_; + LinearAccel lin_acc_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index a52d356c5e..6c1d84f85a 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -70,7 +70,8 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector state_interfaces; hardware_interface::InterfaceInfo info; @@ -84,7 +85,8 @@ class TestSingleJointActuator : public ActuatorInterface return state_interfaces; } - std::vector export_command_interfaces_2() override + std::vector export_command_interface_descriptions() + override { std::vector command_interfaces; hardware_interface::InterfaceInfo info; diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index f9cba77394..de580ffbee 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -74,7 +74,8 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) @@ -96,7 +97,8 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface return state_interfaces; } - std::vector export_command_interfaces_2() override + std::vector export_command_interface_descriptions() + override { std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index 6bbdc75150..cef5cd2e50 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -67,7 +67,8 @@ class TestTwoJointSystem : public SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) @@ -83,7 +84,8 @@ class TestTwoJointSystem : public SystemInterface return state_interfaces; } - std::vector export_command_interfaces_2() override + std::vector export_command_interface_descriptions() + override { std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index d34b431cf5..22a680ebdf 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -56,7 +56,8 @@ class TestActuator : public ActuatorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index bbfbce6fbf..4e0c40d0e5 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -42,47 +42,53 @@ class TestSystem : public SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interface_descriptions() + override { - std::vector state_interfaces; + using hardware_interface::InterfaceDescription; + using hardware_interface::InterfaceInfo; + std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + state_interfaces.emplace_back( + InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_POSITION))); + state_interfaces.emplace_back( + InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_VELOCITY))); + state_interfaces.emplace_back( + InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_ACCELERATION))); } if (info_.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); + state_interfaces.emplace_back(InterfaceDescription( + info_.gpios[0].name, InterfaceInfo(info_.gpios[0].state_interfaces[0].name))); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interface_descriptions() + override { - std::vector command_interfaces; + using hardware_interface::InterfaceDescription; + using hardware_interface::InterfaceInfo; + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); ++i) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + command_interfaces.emplace_back( + InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_VELOCITY))); } // Add max_acceleration command interface - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, - &max_acceleration_command_)); + command_interfaces.emplace_back(InterfaceDescription( + info_.joints[0].name, InterfaceInfo(info_.joints[0].command_interfaces[1].name))); if (info_.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); + command_interfaces.emplace_back(InterfaceDescription( + info_.gpios[0].name, InterfaceInfo(info_.gpios[0].command_interfaces[0].name))); } return command_interfaces; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 9a23d0d3c8..0a8837f51f 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -353,7 +353,8 @@ TEST_F(ResourceManagerTest, resource_claiming) class ExternalComponent : public hardware_interface::ActuatorInterface { - std::vector export_state_interfaces_2() override + std::vector export_state_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; @@ -364,7 +365,8 @@ class ExternalComponent : public hardware_interface::ActuatorInterface return interfaces; } - std::vector export_command_interfaces_2() override + std::vector export_command_interface_descriptions() + override { std::vector interfaces; hardware_interface::InterfaceInfo info; @@ -1289,13 +1291,13 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) EXPECT_TRUE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[2])); // access interface value - EXPECT_EQ(claimed_itf1.get_value(), 1.0); - EXPECT_EQ(claimed_itf3.get_value(), 3.0); + EXPECT_EQ(claimed_itf1.get_value(), 1.0); + EXPECT_EQ(claimed_itf3.get_value(), 3.0); claimed_itf1.set_value(11.1); claimed_itf3.set_value(33.3); - EXPECT_EQ(claimed_itf1.get_value(), 11.1); - EXPECT_EQ(claimed_itf3.get_value(), 33.3); + EXPECT_EQ(claimed_itf1.get_value(), 11.1); + EXPECT_EQ(claimed_itf3.get_value(), 33.3); EXPECT_EQ(reference_interface_values[0], 11.1); EXPECT_EQ(reference_interface_values[1], 2.0); diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index d730029d90..145233d172 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -128,47 +128,47 @@ TEST_F( // When TestSystemCommandModes is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); // When TestActuatorHardware is UNCONFIGURED expect OK EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 403.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 503.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 603.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 603.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); }; // System : ACTIVE @@ -182,47 +182,47 @@ TEST_F( // When TestSystemCommandModes is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); // When TestActuatorHardware is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); }; // System : INACTIVE @@ -236,47 +236,47 @@ TEST_F( // When TestSystemCommandModes is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 1.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 1.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 101.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 101.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 102.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 102.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 202.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 202.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 203.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 203.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 303.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 303.0); // When TestActuatorHardware is ACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 304.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 304.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 404.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 404.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 405.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 405.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 505.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 505.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 506.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 506.0); EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 606.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 606.0); }; // System : UNCONFIGURED @@ -291,47 +291,47 @@ TEST_F( // When TestSystemCommandModes is UNCONFIGURED expect error EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 100.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 200.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 300.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 300.0); // When TestActuatorHardware is INACTIVE expect OK EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 301.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 301.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 401.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 401.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 402.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 402.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 502.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 502.0); EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 503.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 503.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 603.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 603.0); }; // System : UNCONFIGURED @@ -346,47 +346,47 @@ TEST_F( // When TestSystemCommandModes is UNCONFIGURED expect error EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); // When TestActuatorHardware is INACTIVE expect OK EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); - EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); - EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); + EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); + EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); }; int main(int argc, char ** argv) From ae2ba280fc5c97fa4e326deac34fbbf4886c07b1 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 7 Aug 2024 14:33:41 +0200 Subject: [PATCH 31/34] continue adapting tests and small updates on handle and interfaces: * add function for checking if handle holds value --- .../semantic_components/range_sensor.hpp | 2 +- .../hardware_interface/actuator_interface.hpp | 10 + .../include/hardware_interface/handle.hpp | 2 + .../hardware_interface/sensor_interface.hpp | 5 + .../hardware_interface/system_interface.hpp | 10 + .../test/test_components/test_system.cpp | 16 +- .../transmission_interface/accessor.hpp | 39 ++- .../differential_transmission.hpp | 81 +++--- .../four_bar_linkage_transmission.hpp | 73 +++--- .../simple_transmission.hpp | 83 +++--- .../transmission_interface/transmission.hpp | 4 +- .../test/differential_transmission_test.cpp | 247 +++++++++--------- .../four_bar_linkage_transmission_test.cpp | 234 +++++++++-------- .../test/simple_transmission_test.cpp | 85 +++--- transmission_interface/test/utils_test.cpp | 5 +- 15 files changed, 490 insertions(+), 406 deletions(-) diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp index e4af8b7f86..6d6400c70f 100644 --- a/controller_interface/include/semantic_components/range_sensor.hpp +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -38,7 +38,7 @@ class RangeSensor : public SemanticComponentInterface * * \return value of the range in meters */ - float get_range() { return state_interfaces_[0].get().get_value(); } + float get_range() { return state_interfaces_[0].get().get_value(); } /// Return Range message with range in meters /** diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index ffc0ea1607..d67e466f89 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -478,6 +478,16 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return actuator_commands_.at(interface_name)->get_value(); } + bool state_holds_value(const std::string & interface_name) const + { + return actuator_states_.at(interface_name)->holds_value(); + } + + bool command_holds_value(const std::string & interface_name) const + { + return actuator_commands_.at(interface_name)->holds_value(); + } + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 97f4c3f167..66ceef14db 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -98,6 +98,8 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } + bool holds_value() const { return std::holds_alternative(value_); } + template ::value, int>::type = 0> T get_value() const { diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index df318f359e..528115ff62 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -318,6 +318,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return sensor_states_.at(interface_name)->get_value(); } + bool state_holds_value(const std::string & interface_name) const + { + return sensor_states_.at(interface_name)->holds_value(); + } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } std::vector get_error_code() const diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 2181aa1b50..d84fb46980 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -517,6 +517,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return system_commands_.at(interface_name)->get_value(); } + bool state_holds_value(const std::string & interface_name) const + { + return system_states_.at(interface_name)->holds_value(); + } + + bool command_holds_value(const std::string & interface_name) const + { + return system_commands_.at(interface_name)->holds_value(); + } + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 4e0c40d0e5..0bfc064cba 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -50,12 +50,12 @@ class TestSystem : public SystemInterface std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - state_interfaces.emplace_back( - InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_POSITION))); - state_interfaces.emplace_back( - InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_VELOCITY))); - state_interfaces.emplace_back( - InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_ACCELERATION))); + state_interfaces.emplace_back(InterfaceDescription( + info_.joints[i].name, InterfaceInfo(hardware_interface::HW_IF_POSITION))); + state_interfaces.emplace_back(InterfaceDescription( + info_.joints[i].name, InterfaceInfo(hardware_interface::HW_IF_VELOCITY))); + state_interfaces.emplace_back(InterfaceDescription( + info_.joints[i].name, InterfaceInfo(hardware_interface::HW_IF_ACCELERATION))); } if (info_.gpios.size() > 0) @@ -77,8 +77,8 @@ class TestSystem : public SystemInterface for (auto i = 0u; i < info_.joints.size(); ++i) { - command_interfaces.emplace_back( - InterfaceDescription(info_.joints[i].name, InterfaceInfo(HW_IF_VELOCITY))); + command_interfaces.emplace_back(InterfaceDescription( + info_.joints[i].name, InterfaceInfo(hardware_interface::HW_IF_VELOCITY))); } // Add max_acceleration command interface command_interfaces.emplace_back(InterfaceDescription( diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index a72b0f39b0..5c3ef0c91a 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -16,6 +16,7 @@ #define TRANSMISSION_INTERFACE__ACCESSOR_HPP_ #include +#include #include #include #include @@ -41,6 +42,24 @@ std::string to_string(const std::vector & list) return ss.str(); } +template +std::string to_string(const std::vector> & list) +{ + std::stringstream ss; + ss << "["; + for (const auto & elem : list) + { + ss << *elem << ", "; + } + + if (!list.empty()) + { + ss.seekp(-2, std::ios_base::end); // remove last ", " + } + ss << "]"; + return ss.str(); +} + template std::vector get_names(const std::vector & handles) { @@ -51,20 +70,30 @@ std::vector get_names(const std::vector & handles) return std::vector(names.begin(), names.end()); } +template +std::vector get_names(const std::vector> & handles) +{ + std::set names; + std::transform( + handles.cbegin(), handles.cend(), std::inserter(names, names.end()), + [](const auto & handle) { return handle->get_prefix_name(); }); + return std::vector(names.begin(), names.end()); +} + template -std::vector get_ordered_handles( - const std::vector & unordered_handles, const std::vector & names, +std::vector> get_ordered_handles( + const std::vector> & unordered_handles, const std::vector & names, const std::string & interface_type) { - std::vector result; + std::vector> result; for (const auto & name : names) { std::copy_if( unordered_handles.cbegin(), unordered_handles.cend(), std::back_inserter(result), [&](const auto & handle) { - return (handle.get_prefix_name() == name) && - (handle.get_interface_name() == interface_type) && handle; + return (handle->get_prefix_name() == name) && + (handle->get_interface_name() == interface_type) && handle; }); } return result; diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 22723d3897..3840695b6b 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -16,6 +16,7 @@ #define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_ #include +#include #include #include @@ -125,8 +126,8 @@ class DifferentialTransmission : public Transmission * \pre Handles are valid and matching in size */ void configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) override; + const std::vector> & joint_handles, + const std::vector> & actuator_handles) override; /// Transform variables from actuator to joint space. /** @@ -159,13 +160,13 @@ class DifferentialTransmission : public Transmission std::vector joint_reduction_; std::vector joint_offset_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; + std::vector> joint_position_; + std::vector> joint_velocity_; + std::vector> joint_effort_; - std::vector actuator_position_; - std::vector actuator_velocity_; - std::vector actuator_effort_; + std::vector> actuator_position_; + std::vector> actuator_velocity_; + std::vector> actuator_effort_; }; inline DifferentialTransmission::DifferentialTransmission( @@ -191,8 +192,8 @@ inline DifferentialTransmission::DifferentialTransmission( } void DifferentialTransmission::configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) + const std::vector> & joint_handles, + const std::vector> & actuator_handles) { if (joint_handles.empty()) { @@ -262,14 +263,14 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + assert(*act_pos[0] && *act_pos[1] && *joint_pos[0] && *joint_pos[1]); - joint_pos[0].set_value( - (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / + joint_pos[0]->set_value( + (act_pos[0]->get_value() / ar[0] + act_pos[1]->get_value() / ar[1]) / (2.0 * jr[0]) + joint_offset_[0]); - joint_pos[1].set_value( - (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / + joint_pos[1]->set_value( + (act_pos[0]->get_value() / ar[0] - act_pos[1]->get_value() / ar[1]) / (2.0 * jr[1]) + joint_offset_[1]); } @@ -278,13 +279,13 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + assert(*act_vel[0] && *act_vel[1] && *joint_vel[0] && *joint_vel[1]); - joint_vel[0].set_value( - (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / + joint_vel[0]->set_value( + (act_vel[0]->get_value() / ar[0] + act_vel[1]->get_value() / ar[1]) / (2.0 * jr[0])); - joint_vel[1].set_value( - (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / + joint_vel[1]->set_value( + (act_vel[0]->get_value() / ar[0] - act_vel[1]->get_value() / ar[1]) / (2.0 * jr[1])); } @@ -292,12 +293,12 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + assert(*act_eff[0] && *act_eff[1] && *joint_eff[0] && *joint_eff[1]); - joint_eff[0].set_value( - jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); - joint_eff[1].set_value( - jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); + joint_eff[0]->set_value( + jr[0] * (act_eff[0]->get_value() * ar[0] + act_eff[1]->get_value() * ar[1])); + joint_eff[1]->set_value( + jr[1] * (act_eff[0]->get_value() * ar[0] - act_eff[1]->get_value() * ar[1])); } } @@ -310,14 +311,14 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + assert(*act_pos[0] && *act_pos[1] && *joint_pos[0] && *joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], - joint_pos[1].get_value() - joint_offset_[1]}; - act_pos[0].set_value( + joint_pos[0]->get_value() - joint_offset_[0], + joint_pos[1]->get_value() - joint_offset_[1]}; + act_pos[0]->set_value( (joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1]) * ar[0]); - act_pos[1].set_value( + act_pos[1]->set_value( (joints_offset_applied[0] * jr[0] - joints_offset_applied[1] * jr[1]) * ar[1]); } @@ -325,13 +326,13 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + assert(*act_vel[0] && *act_vel[1] && *joint_vel[0] && *joint_vel[1]); - act_vel[0].set_value( - (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * + act_vel[0]->set_value( + (joint_vel[0]->get_value() * jr[0] + joint_vel[1]->get_value() * jr[1]) * ar[0]); - act_vel[1].set_value( - (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * + act_vel[1]->set_value( + (joint_vel[0]->get_value() * jr[0] - joint_vel[1]->get_value() * jr[1]) * ar[1]); } @@ -339,13 +340,13 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + assert(*act_eff[0] && *act_eff[1] && *joint_eff[0] && *joint_eff[1]); - act_eff[0].set_value( - (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / + act_eff[0]->set_value( + (joint_eff[0]->get_value() / jr[0] + joint_eff[1]->get_value() / jr[1]) / (2.0 * ar[0])); - act_eff[1].set_value( - (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / + act_eff[1]->set_value( + (joint_eff[0]->get_value() / jr[0] - joint_eff[1]->get_value() / jr[1]) / (2.0 * ar[1])); } } diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index c886145350..5cd7205b04 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -18,6 +18,7 @@ #define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_ #include +#include #include #include @@ -123,8 +124,8 @@ class FourBarLinkageTransmission : public Transmission * \pre Handles are valid and matching in size */ void configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) override; + const std::vector> & joint_handles, + const std::vector> & actuator_handles) override; /// Transform variables from actuator to joint space. /** @@ -157,13 +158,13 @@ class FourBarLinkageTransmission : public Transmission std::vector joint_reduction_; std::vector joint_offset_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; + std::vector> joint_position_; + std::vector> joint_velocity_; + std::vector> joint_effort_; - std::vector actuator_position_; - std::vector actuator_velocity_; - std::vector actuator_effort_; + std::vector> actuator_position_; + std::vector> actuator_velocity_; + std::vector> actuator_effort_; }; inline FourBarLinkageTransmission::FourBarLinkageTransmission( @@ -188,8 +189,8 @@ inline FourBarLinkageTransmission::FourBarLinkageTransmission( } void FourBarLinkageTransmission::configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) + const std::vector> & joint_handles, + const std::vector> & actuator_handles) { if (joint_handles.empty()) { @@ -260,11 +261,12 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + assert(*act_pos[0] && *act_pos[1] && *joint_pos[0] && *joint_pos[1]); - joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); - joint_pos[1].set_value( - (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / + joint_pos[0]->set_value(act_pos[0]->get_value() / (jr[0] * ar[0]) + joint_offset_[0]); + joint_pos[1]->set_value( + (act_pos[1]->get_value() / ar[1] - + act_pos[0]->get_value() / (jr[0] * ar[0])) / jr[1] + joint_offset_[1]); } @@ -274,11 +276,12 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + assert(*act_vel[0] && *act_vel[1] && *joint_vel[0] && *joint_vel[1]); - joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); - joint_vel[1].set_value( - (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / + joint_vel[0]->set_value(act_vel[0]->get_value() / (jr[0] * ar[0])); + joint_vel[1]->set_value( + (act_vel[1]->get_value() / ar[1] - + act_vel[0]->get_value() / (jr[0] * ar[0])) / jr[1]); } @@ -287,12 +290,12 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + assert(*act_eff[0] && *act_eff[1] && *joint_eff[0] && *joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); - joint_eff[1].set_value( + joint_eff[0]->set_value(jr[0] * act_eff[0]->get_value() * ar[0]); + joint_eff[1]->set_value( jr[1] * - (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); + (act_eff[1]->get_value() * ar[1] - act_eff[0]->get_value() * ar[0] * jr[0])); } } @@ -306,13 +309,13 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + assert(*act_pos[0] && *act_pos[1] && *joint_pos[0] && *joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], - joint_pos[1].get_value() - joint_offset_[1]}; - act_pos[0].set_value(joints_offset_applied[0] * jr[0] * ar[0]); - act_pos[1].set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]); + joint_pos[0]->get_value() - joint_offset_[0], + joint_pos[1]->get_value() - joint_offset_[1]}; + act_pos[0]->set_value(joints_offset_applied[0] * jr[0] * ar[0]); + act_pos[1]->set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]); } // velocity @@ -320,11 +323,11 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + assert(*act_vel[0] && *act_vel[1] && *joint_vel[0] && *joint_vel[1]); - act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); - act_vel[1].set_value( - (joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); + act_vel[0]->set_value(joint_vel[0]->get_value() * jr[0] * ar[0]); + act_vel[1]->set_value( + (joint_vel[0]->get_value() + joint_vel[1]->get_value() * jr[1]) * ar[1]); } // effort @@ -332,11 +335,11 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + assert(*act_eff[0] && *act_eff[1] && *joint_eff[0] && *joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); - act_eff[1].set_value( - (joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); + act_eff[0]->set_value(joint_eff[0]->get_value() / (ar[0] * jr[0])); + act_eff[1]->set_value( + (joint_eff[0]->get_value() + joint_eff[1]->get_value() / jr[1]) / ar[1]); } } diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 229a2bbdb3..d73619237b 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -94,14 +95,14 @@ class SimpleTransmission : public Transmission /// Set up the data the transmission operates on. /** - * \param[in] joint_handles Vector of interface handles of one joint - * \param[in] actuator_handles Vector of interface handles of one actuator + * \param[in] joint_handles Vector of shared ptr to interface handles of one joint + * \param[in] actuator_handles Vector of shared ptr to interface handles of one actuator * \pre Vectors are nonzero. * \pre Joint handles share the same joint name and actuator handles share the same actuator name. */ void configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) override; + const std::vector> & joint_handles, + const std::vector> & actuator_handles) override; /// Transform variables from actuator to joint space. /** @@ -129,18 +130,31 @@ class SimpleTransmission : public Transmission double reduction_; double jnt_offset_; - JointHandle joint_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; - JointHandle joint_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; - JointHandle joint_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + std::shared_ptr joint_position_; + std::shared_ptr joint_velocity_; + std::shared_ptr joint_effort_; - ActuatorHandle actuator_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; - ActuatorHandle actuator_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; - ActuatorHandle actuator_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + std::shared_ptr actuator_position_; + std::shared_ptr actuator_velocity_; + std::shared_ptr actuator_effort_; }; inline SimpleTransmission::SimpleTransmission( const double joint_to_actuator_reduction, const double joint_offset) -: reduction_(joint_to_actuator_reduction), jnt_offset_(joint_offset) +: reduction_(joint_to_actuator_reduction), + jnt_offset_(joint_offset), + joint_position_( + std::make_shared(hardware_interface::InterfaceDescription{{}, std::string{}})), + joint_velocity_( + std::make_shared(hardware_interface::InterfaceDescription{{}, std::string{}})), + joint_effort_( + std::make_shared(hardware_interface::InterfaceDescription{{}, std::string{}})), + actuator_position_( + std::make_shared(hardware_interface::InterfaceDescription{{}, std::string{}})), + actuator_velocity_( + std::make_shared(hardware_interface::InterfaceDescription{{}, std::string{}})), + actuator_effort_( + std::make_shared(hardware_interface::InterfaceDescription{{}, std::string{}})) { if (reduction_ == 0.0) { @@ -149,17 +163,17 @@ inline SimpleTransmission::SimpleTransmission( } template -HandleType get_by_interface( - const std::vector & handles, const std::string & interface_name) +std::shared_ptr get_by_interface( + const std::vector> & handles, const std::string & interface_name) { const auto result = std::find_if( - handles.cbegin(), handles.cend(), - [&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; }); + handles.cbegin(), handles.cend(), [&interface_name](const auto handle) + { return handle->get_interface_name() == interface_name; }); if (result == handles.cend()) { - // return new Handle where data is set to std::monotype by default - return HandleType(hardware_interface::InterfaceDescription( - handles.cbegin()->get_prefix_name(), interface_name)); + // return new Handle where data is set to std::monotype (uninitialized state) by default + return std::make_shared(hardware_interface::InterfaceDescription( + (*handles.cbegin())->get_prefix_name(), interface_name)); } return *result; } @@ -170,13 +184,13 @@ bool are_names_identical(const std::vector & handles) std::vector names; std::transform( handles.cbegin(), handles.cend(), std::back_inserter(names), - [](const auto & handle) { return handle.get_prefix_name(); }); + [](const auto & handle) { return handle->get_prefix_name(); }); return std::equal(names.cbegin() + 1, names.cend(), names.cbegin()); } -inline void SimpleTransmission::configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) +void SimpleTransmission::configure( + const std::vector> & joint_handles, + const std::vector> & actuator_handles) { if (joint_handles.empty()) { @@ -219,37 +233,38 @@ inline void SimpleTransmission::configure( inline void SimpleTransmission::actuator_to_joint() { - if (joint_effort_ && actuator_effort_) + if (*joint_effort_ && *actuator_effort_) { - joint_effort_.set_value(actuator_effort_.get_value() * reduction_); + joint_effort_->set_value(actuator_effort_->get_value() * reduction_); } - if (joint_velocity_ && actuator_velocity_) + if (*joint_velocity_ && *actuator_velocity_) { - joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); + joint_velocity_->set_value(actuator_velocity_->get_value() / reduction_); } - if (joint_position_ && actuator_position_) + if (*joint_position_ && *actuator_position_) { - joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); + joint_position_->set_value(actuator_position_->get_value() / reduction_ + jnt_offset_); } } inline void SimpleTransmission::joint_to_actuator() { - if (joint_effort_ && actuator_effort_) + if (*joint_effort_ && *actuator_effort_) { - actuator_effort_.set_value(joint_effort_.get_value() / reduction_); + actuator_effort_->set_value(joint_effort_->get_value() / reduction_); } - if (joint_velocity_ && actuator_velocity_) + if (*joint_velocity_ && *actuator_velocity_) { - actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); + actuator_velocity_->set_value(joint_velocity_->get_value() * reduction_); } - if (joint_position_ && actuator_position_) + if (*joint_position_ && *actuator_position_) { - actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); + actuator_position_->set_value( + (joint_position_->get_value() - jnt_offset_) * reduction_); } } diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 6275f37711..50c3ea5a44 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -48,8 +48,8 @@ class Transmission virtual ~Transmission() = default; virtual void configure( - const std::vector & joint_handles, - const std::vector & actuator_handles) = 0; + const std::vector> & joint_handles, + const std::vector> & actuator_handles) = 0; /// Transform \e effort variables from actuator to joint space. /** diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 8c3c01e30c..e0ad921dfa 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -92,24 +92,23 @@ TEST(PreconditionsTest, AccessorValidation) void testConfigureWithBadHandles(std::string interface_name) { DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0}); - double dummy; - - auto a1_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); - auto a2_handle = - ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(InterfaceInfo(interface_name)))); - auto a3_handle = - ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(InterfaceInfo(interface_name)))); - auto j1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); - auto j2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(InterfaceInfo(interface_name)))); - auto j3_handle = - JointHandle(InterfaceDescription("joint3", InterfaceInfo(InterfaceInfo(interface_name)))); - auto invalid_a1_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); - auto invalid_j1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); + + auto a1_handle = std::make_shared( + InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a2_handle = std::make_shared( + InterfaceDescription("act2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a3_handle = std::make_shared( + InterfaceDescription("act3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j3_handle = std::make_shared( + InterfaceDescription("joint3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_a1_handle = std::make_shared( + InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_j1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -159,25 +158,25 @@ class BlackBoxTest : public TransmissionSetup auto a_val_1 = ref_val[1]; // create handles and configure - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a1_handle.set_value(EXTREMAL_VALUE); - a2_handle.set_value(EXTREMAL_VALUE); + a1_handle->set_value(EXTREMAL_VALUE); + a2_handle->set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a1_handle.get_value(), EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a2_handle.get_value(), EPS)); + EXPECT_THAT(ref_val[0], DoubleNear(a1_handle->get_value(), EPS)); + EXPECT_THAT(ref_val[1], DoubleNear(a2_handle->get_value(), EPS)); } // Generate a set of transmission instances @@ -252,52 +251,52 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Effort interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle->get_value(), EPS)); } } @@ -314,53 +313,53 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Effort interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(400.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(400.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } } @@ -377,51 +376,51 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Effort interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(400.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(400.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle->get_value(), EPS)); } } @@ -443,49 +442,49 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Effort interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(140.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(520.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(140.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(520.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.01250, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.06875, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(-0.01250, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.06875, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.01250, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(4.06875, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(-2.01250, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(4.06875, DoubleNear(joint2_handle->get_value(), EPS)); } } diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index c4ab04986c..db7b1a2414 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -96,16 +96,22 @@ void testConfigureWithBadHandles(std::string interface_name) { FourBarLinkageTransmission trans({1.0, 1.0}, {1.0, 1.0}); - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(interface_name))); - auto a3_handle = ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(interface_name))); - auto j1_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); - auto j2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name))); - auto j3_handle = JointHandle(InterfaceDescription("joint3", InterfaceInfo(interface_name))); + auto a1_handle = + std::make_shared(InterfaceDescription("act1", InterfaceInfo(interface_name))); + auto a2_handle = + std::make_shared(InterfaceDescription("act2", InterfaceInfo(interface_name))); + auto a3_handle = + std::make_shared(InterfaceDescription("act3", InterfaceInfo(interface_name))); + auto j1_handle = + std::make_shared(InterfaceDescription("joint1", InterfaceInfo(interface_name))); + auto j2_handle = + std::make_shared(InterfaceDescription("joint2", InterfaceInfo(interface_name))); + auto j3_handle = + std::make_shared(InterfaceDescription("joint3", InterfaceInfo(interface_name))); auto invalid_a1_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); + std::make_shared(InterfaceDescription("act1", InterfaceInfo(interface_name))); auto invalid_j1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); + std::make_shared(InterfaceDescription("joint1", InterfaceInfo(interface_name))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -155,25 +161,25 @@ class BlackBoxTest : public TransmissionSetup auto a_val = ref_val[0]; auto a_val_1 = ref_val[1]; // create handles and configure - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a1_handle.set_value(EXTREMAL_VALUE); - a2_handle.set_value(EXTREMAL_VALUE); + a1_handle->set_value(EXTREMAL_VALUE); + a2_handle->set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(a_val, DoubleNear(a1_handle.get_value(), EPS)); - EXPECT_THAT(a_val_1, DoubleNear(a2_handle.get_value(), EPS)); + EXPECT_THAT(a_val, DoubleNear(a1_handle->get_value(), EPS)); + EXPECT_THAT(a_val_1, DoubleNear(a2_handle->get_value(), EPS)); } // Generate a set of transmission instances @@ -248,50 +254,50 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Effort interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle->get_value(), EPS)); } } @@ -306,54 +312,54 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) { auto a_val = 5.0; auto a_val_1 = 10.0; - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(100.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(100.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { auto a_val = 10.0; auto a_val_1 = 5.0; - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { auto a_val = 10.0; auto a_val_1 = 5.0; - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle->get_value(), EPS)); } } @@ -370,50 +376,50 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Effort interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(200.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(200.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle->get_value(), EPS)); } } @@ -435,49 +441,49 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Effort interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle( + auto a2_handle = std::make_shared( InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-60.0, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(-160.0, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(-60.0, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(-160.0, DoubleNear(joint2_handle->get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.15, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(-0.025, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(-0.15, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(-0.025, DoubleNear(joint2_handle->get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle( + auto a1_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( + auto a2_handle = std::make_shared(InterfaceDescription( "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); - auto joint1_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); - auto joint2_handle = - JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint1_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = std::make_shared( + InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.15, DoubleNear(joint1_handle.get_value(), EPS)); - EXPECT_THAT(3.975, DoubleNear(joint2_handle.get_value(), EPS)); + EXPECT_THAT(-2.15, DoubleNear(joint1_handle->get_value(), EPS)); + EXPECT_THAT(3.975, DoubleNear(joint2_handle->get_value(), EPS)); } } diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 026264e79b..22c2dd7674 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -66,12 +66,14 @@ TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) { SimpleTransmission trans(2.0, -1.0); - auto actuator_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION))); - auto actuator2_handle = - ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(HW_IF_POSITION))); - auto joint_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION))); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION))); + std::shared_ptr actuator_handle = + std::make_shared(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION))); + std::shared_ptr actuator2_handle = + std::make_shared(InterfaceDescription("act2", InterfaceInfo(HW_IF_POSITION))); + std::shared_ptr joint_handle = + std::make_shared(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION))); + std::shared_ptr joint2_handle = + std::make_shared(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION))); EXPECT_THROW(trans.configure({}, {}), transmission_interface::Exception); EXPECT_THROW(trans.configure({joint_handle}, {}), transmission_interface::Exception); @@ -84,10 +86,10 @@ TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) trans.configure({joint_handle, joint2_handle}, {actuator_handle}), transmission_interface::Exception); - auto invalid_actuator_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY))); - auto invalid_joint_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY))); + std::shared_ptr invalid_actuator_handle = + std::make_shared(InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY))); + std::shared_ptr invalid_joint_handle = + std::make_shared(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY))); EXPECT_THROW( trans.configure({invalid_joint_handle}, {invalid_actuator_handle}), transmission_interface::Exception); @@ -118,19 +120,19 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam actuator_handle = std::make_shared( InterfaceDescription("act1", InterfaceInfo(interface_name, "0.0", "double"))); - auto joint_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "0.0", "double"))); + std::shared_ptr joint_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(interface_name, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); - actuator_handle.set_value(ref_val); + actuator_handle->set_value(ref_val); trans.actuator_to_joint(); trans.joint_to_actuator(); - EXPECT_THAT(ref_val, DoubleNear(actuator_handle.get_value(), EPS)); - actuator_handle.set_value(0.0); - joint_handle.set_value(0.0); + EXPECT_THAT(ref_val, DoubleNear(actuator_handle->get_value(), EPS)); + actuator_handle->set_value(0.0); + joint_handle->set_value(0.0); } } }; @@ -175,59 +177,60 @@ TEST_F(WhiteBoxTest, MoveJoint) // Effort interface { - auto actuator_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, "1.0", "double"))); - auto joint_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "0.0", "double"))); + std::shared_ptr actuator_handle = std::make_shared( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, "1.0", "double"))); + std::shared_ptr joint_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(10.0, DoubleNear(joint_handle.get_value(), EPS)); + EXPECT_THAT(10.0, DoubleNear(joint_handle->get_value(), EPS)); } // Velocity interface { - auto actuator_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, "1.0", "double"))); - auto joint_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "0.0", "double"))); + std::shared_ptr actuator_handle = std::make_shared( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, "1.0", "double"))); + std::shared_ptr joint_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.1, DoubleNear(joint_handle.get_value(), EPS)); + EXPECT_THAT(0.1, DoubleNear(joint_handle->get_value(), EPS)); } // Position interface { - auto actuator_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); - auto joint_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "0.0", "double"))); + std::shared_ptr actuator_handle = std::make_shared( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + std::shared_ptr joint_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(1.1, DoubleNear(joint_handle.get_value(), EPS)); + EXPECT_THAT(1.1, DoubleNear(joint_handle->get_value(), EPS)); } // Mismatched interface is ignored { double unique_value = 13.37; - auto actuator_handle = - ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); - auto actuator_handle2 = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(unique_value), "double"))); - auto joint_handle = - JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); - auto joint_handle2 = JointHandle(InterfaceDescription( + std::shared_ptr actuator_handle = std::make_shared( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + std::shared_ptr actuator_handle2 = + std::make_shared(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(unique_value), "double"))); + std::shared_ptr joint_handle = std::make_shared( + InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + std::shared_ptr joint_handle2 = std::make_shared(InterfaceDescription( "joint1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(unique_value), "double"))); trans.configure({joint_handle, joint_handle2}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_handle.get_value(), DoubleNear(13.37, EPS)); + EXPECT_THAT(joint_handle->get_value(), DoubleNear(13.37, EPS)); trans.configure({joint_handle}, {actuator_handle, actuator_handle2}); trans.actuator_to_joint(); - EXPECT_THAT(joint_handle.get_value(), DoubleNear(13.37, EPS)); + EXPECT_THAT(joint_handle->get_value(), DoubleNear(13.37, EPS)); } } diff --git a/transmission_interface/test/utils_test.cpp b/transmission_interface/test/utils_test.cpp index 808faa20d7..7138724574 100644 --- a/transmission_interface/test/utils_test.cpp +++ b/transmission_interface/test/utils_test.cpp @@ -29,8 +29,9 @@ TEST(UtilsTest, AccessorTest) info.name = HW_IF_POSITION; info.initial_value = "0.0"; hardware_interface::InterfaceDescription joint_handle_desc(NAME, info); - const JointHandle joint_handle(joint_handle_desc); - const std::vector joint_handles = {joint_handle}; + const std::shared_ptr joint_handle = + std::make_shared(joint_handle_desc); + const std::vector> joint_handles = {joint_handle}; ASSERT_EQ(transmission_interface::get_names(joint_handles), std::vector{NAME}); ASSERT_EQ( From 9aa7c126f95c3e1094762fcc18906e6b75885d3c Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 23 Aug 2024 13:22:11 +0200 Subject: [PATCH 32/34] add orderd vector of StateInterfaces and CommandInterfaces --- .../hardware_interface/actuator_interface.hpp | 17 +++++++++++-- .../hardware_interface/sensor_interface.hpp | 22 ++++++++++------ .../hardware_interface/system_interface.hpp | 25 +++++++++++++++++-- 3 files changed, 53 insertions(+), 11 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index d67e466f89..6b4947cdd3 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -275,6 +275,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); actuator_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -282,6 +283,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { auto state_interface = std::make_shared(descr); actuator_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -360,6 +362,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod unlisted_command_interfaces_.insert(std::make_pair(name, description)); auto command_interface = std::make_shared(description); actuator_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -367,6 +370,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { auto command_interface = std::make_shared(descr); actuator_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -546,17 +550,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; + rclcpp_lifecycle::State lifecycle_state_; + + // interface names to InterfaceDescription std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> joint_states_; + std::vector> joint_commands_; + + std::vector> unlisted_states_; + std::vector> unlisted_commands_; + private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp_lifecycle::State lifecycle_state_; rclcpp::Logger actuator_logger_; - + // interface names to Handle accessed through getters/setters std::unordered_map> actuator_states_; std::unordered_map> actuator_commands_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 528115ff62..b12af2ac4d 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -250,7 +250,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto name = description.get_name(); unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); - sensor_states_.insert(std::make_pair(name, state_interface)); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -259,7 +260,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is // inserted auto state_interface = std::make_shared(descr); - sensor_states_.insert(std::make_pair(name, state_interface)); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -310,17 +312,17 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const std::string & interface_name, const double & value) { - sensor_states_.at(interface_name)->set_value(value); + sensor_states_map_.at(interface_name)->set_value(value); } double get_state(const std::string & interface_name) const { - return sensor_states_.at(interface_name)->get_value(); + return sensor_states_map_.at(interface_name)->get_value(); } bool state_holds_value(const std::string & interface_name) const { - return sensor_states_.at(interface_name)->holds_value(); + return sensor_states_map_.at(interface_name)->holds_value(); } void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } @@ -374,16 +376,22 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; + rclcpp_lifecycle::State lifecycle_state_; + // interface names to InterfaceDescription std::unordered_map sensor_state_interfaces_; std::unordered_map unlisted_state_interfaces_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> sensor_states_; + std::vector> unlisted_states_; + private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp_lifecycle::State lifecycle_state_; rclcpp::Logger sensor_logger_; - std::unordered_map> sensor_states_; + // interface names to Handle accessed through getters/setters + std::unordered_map> sensor_states_map_; std::shared_ptr error_signal_; std::shared_ptr error_signal_message_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index d84fb46980..c3bc8cef44 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -295,6 +295,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); system_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -302,18 +303,21 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : sensor_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : gpio_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + gpio_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -393,6 +397,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI unlisted_command_interfaces_.insert(std::make_pair(name, description)); auto command_interface = std::make_shared(description); system_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -400,6 +405,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto command_interface = std::make_shared(descr); system_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -407,6 +413,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto command_interface = std::make_shared(descr); system_commands_.insert(std::make_pair(name, command_interface)); + gpio_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } return command_interfaces; @@ -585,6 +592,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; + rclcpp_lifecycle::State lifecycle_state_; + + // interface names to InterfaceDescription std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; @@ -596,11 +606,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> joint_states_; + std::vector> joint_commands_; + + std::vector> sensor_states_; + + std::vector> gpio_states_; + std::vector> gpio_commands_; + + std::vector> unlisted_states_; + std::vector> unlisted_commands_; + private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp_lifecycle::State lifecycle_state_; rclcpp::Logger system_logger_; - + // interface names to Handle accessed through getters/setters std::unordered_map> system_states_; std::unordered_map> system_commands_; From 6c5e5cc330b8d32fb5a3981be9b65f66d21e05ee Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 23 Aug 2024 15:07:33 +0200 Subject: [PATCH 33/34] ordered refernce and exported stateinterfaecs --- .../chainable_controller_interface.hpp | 3 + .../src/chainable_controller_interface.cpp | 69 +++++++++++++------ 2 files changed, 52 insertions(+), 20 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index c45e86a8cd..c8a5941a6f 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -138,12 +138,15 @@ class ChainableControllerInterface : public ControllerInterfaceBase // interface_names are in order they have been exported std::vector exported_state_interface_names_; // storage for the exported StateInterfaces + std::vector> + ordered_exported_state_interfaces_; std::unordered_map> exported_state_interfaces_; // interface_names are in order they have been exported std::vector exported_reference_interface_names_; // storage for the exported CommandInterfaces + std::vector> ordered_reference_interfaces_; std::unordered_map> reference_interfaces_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 3e99b50c91..7f918c8e36 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -50,6 +50,9 @@ ChainableControllerInterface::export_state_interfaces() auto state_interfaces_descr = export_state_interface_descriptions(); std::vector> state_interfaces_ptrs_vec; state_interfaces_ptrs_vec.reserve(state_interfaces_descr.size()); + exported_state_interface_names_.reserve(state_interfaces_descr.size()); + ordered_exported_state_interfaces_.reserve(state_interfaces_descr.size()); + exported_state_interfaces_.reserve(state_interfaces_descr.size()); // check if the names of the controller state interfaces begin with the controller's name for (auto & descr : state_interfaces_descr) @@ -81,24 +84,36 @@ ChainableControllerInterface::export_state_interfaces() it->second->get_name() + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " "interface names are unique."; - exported_state_interfaces_.clear(); exported_state_interface_names_.clear(); - state_interfaces_ptrs_vec.clear(); + ordered_exported_state_interfaces_.clear(); + exported_state_interfaces_.clear(); throw std::runtime_error(error_msg); } - exported_state_interface_names_.push_back(inteface_name); state_interfaces_ptrs_vec.push_back(state_interface); + exported_state_interface_names_.push_back(inteface_name); + ordered_exported_state_interfaces_.push_back(state_interface); } - if (exported_state_interfaces_.size() != state_interfaces_descr.size()) + // check that all are equal + if ( + state_interfaces_descr.size() != state_interfaces_ptrs_vec.size() || + state_interfaces_descr.size() != exported_state_interface_names_.size() || + state_interfaces_descr.size() != ordered_exported_state_interfaces_.size() || + state_interfaces_descr.size() != exported_state_interfaces_.size()) { std::string error_msg = - "The internal storage for reference ptrs 'exported_state_interfaces_' variable has size '" + - std::to_string(exported_state_interfaces_.size()) + - "', but it is expected to have the size '" + std::to_string(state_interfaces_descr.size()) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; + "The size of the exported StateInterfaceDescriptions (" + + std::to_string(state_interfaces_descr.size()) + ") of controller <" + get_node()->get_name() + + "> does not match with the size of one of the following: state_interfaces_ptrs_vec (" + + std::to_string(state_interfaces_ptrs_vec.size()) + "), exported_state_interface_names_ (" + + std::to_string(exported_state_interface_names_.size()) + + "), ordered_exported_state_interfaces_ (" + + std::to_string(ordered_exported_state_interfaces_.size()) + + ") or exported_state_interfaces_ (" + std::to_string(exported_state_interfaces_.size()) + + ")."; + exported_state_interface_names_.clear(); + ordered_exported_state_interfaces_.clear(); + exported_state_interfaces_.clear(); throw std::runtime_error(error_msg); } @@ -111,6 +126,9 @@ ChainableControllerInterface::export_reference_interfaces() auto reference_interface_descr = export_reference_interface_descriptions(); std::vector> reference_interfaces_ptrs_vec; reference_interfaces_ptrs_vec.reserve(reference_interface_descr.size()); + exported_reference_interface_names_.reserve(reference_interface_descr.size()); + ordered_reference_interfaces_.reserve(reference_interface_descr.size()); + reference_interfaces_.reserve(reference_interface_descr.size()); // check if the names of the reference interfaces begin with the controller's name for (auto & descr : reference_interface_descr) @@ -141,24 +159,35 @@ ChainableControllerInterface::export_reference_interfaces() it->second->get_name() + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " "interface names are unique."; - reference_interfaces_.clear(); exported_reference_interface_names_.clear(); - reference_interfaces_ptrs_vec.clear(); + ordered_reference_interfaces_.clear(); + reference_interfaces_.clear(); throw std::runtime_error(error_msg); } - exported_reference_interface_names_.push_back(inteface_name); reference_interfaces_ptrs_vec.push_back(reference_interface); + exported_reference_interface_names_.push_back(inteface_name); + ordered_reference_interfaces_.push_back(reference_interface); } - if (reference_interfaces_.size() != reference_interface_descr.size()) + if ( + reference_interface_descr.size() != reference_interfaces_ptrs_vec.size() || + reference_interface_descr.size() != exported_reference_interface_names_.size() || + reference_interface_descr.size() != ordered_reference_interfaces_.size() || + reference_interface_descr.size() != reference_interfaces_.size()) { std::string error_msg = - "The internal storage for reference ptrs 'reference_interfaces_' variable has size '" + - std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + - std::to_string(reference_interface_descr.size()) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; + "The size of the exported ReferenceInterfaceDescriptions (" + + std::to_string(reference_interface_descr.size()) + ") of controller <" + + get_node()->get_name() + + "> does not match with the size of one of the following: reference_interfaces_ptrs_vec (" + + std::to_string(reference_interfaces_ptrs_vec.size()) + + "), exported_reference_interface_names_ (" + + std::to_string(exported_reference_interface_names_.size()) + + "), ordered_reference_interfaces_ (" + std::to_string(ordered_reference_interfaces_.size()) + + ") or reference_interfaces_ (" + std::to_string(reference_interfaces_.size()) + ")."; + exported_reference_interface_names_.clear(); + ordered_reference_interfaces_.clear(); + reference_interfaces_.clear(); throw std::runtime_error(error_msg); } From 53fb31d8d79761213100f8cd4cf7dbca8f06ee4d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 27 Aug 2024 14:21:34 +0200 Subject: [PATCH 34/34] include adaptions of InterfaceDescriptions and parsers from master --- .../hardware_interface/actuator_interface.hpp | 4 +- .../hardware_interface/component_parser.hpp | 4 +- .../hardware_interface/hardware_info.hpp | 15 ++++- .../hardware_interface/sensor_interface.hpp | 2 +- .../hardware_interface/system_interface.hpp | 10 +-- hardware_interface/src/component_parser.cpp | 4 +- .../test/test_component_parser.cpp | 66 +++++++++---------- 7 files changed, 57 insertions(+), 48 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 6b4947cdd3..32458c7b99 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -139,7 +139,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_state_interface_descriptions(hardware_info.joints); for (const auto & description : joint_state_interface_descriptions) { joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); @@ -153,7 +153,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_command_interface_descriptions = - parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_command_interface_descriptions(hardware_info.joints); for (const auto & description : joint_command_interface_descriptions) { joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index b9ca460800..2d0c067606 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -38,7 +38,7 @@ std::vector parse_control_resources_from_urdf(const std::string & * which are exported */ HARDWARE_INTERFACE_PUBLIC -std::vector parse_state_interface_descriptions_from_hardware_info( +std::vector parse_state_interface_descriptions( const std::vector & component_info); /** @@ -47,7 +47,7 @@ std::vector parse_state_interface_descriptions_from_hardwa * which are exported */ HARDWARE_INTERFACE_PUBLIC -std::vector parse_command_interface_descriptions_from_hardware_info( +std::vector parse_command_interface_descriptions( const std::vector & component_info); } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 2aa39f8bd5..ca6603f286 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -188,7 +188,9 @@ struct TransmissionInfo struct InterfaceDescription { InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) - : prefix_name(prefix_name_in), interface_info(interface_info_in) + : prefix_name(prefix_name_in), + interface_info(interface_info_in), + interface_name(prefix_name + "/" + interface_info.name) { } @@ -208,9 +210,16 @@ struct InterfaceDescription */ InterfaceInfo interface_info; - std::string get_name() const { return prefix_name + "/" + interface_info.name; } + /** + * Name of the interface + */ + std::string interface_name; + + std::string get_prefix_name() const { return prefix_name; } + + std::string get_interface_name() const { return interface_info.name; } - std::string get_interface_type() const { return interface_info.name; } + std::string get_name() const { return interface_name; } }; /// This structure stores information about hardware defined in a robot's URDF. diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index b12af2ac4d..ce558ca567 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -137,7 +137,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto sensor_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); + parse_state_interface_descriptions(hardware_info.sensors); for (const auto & description : sensor_state_interface_descriptions) { sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index c3bc8cef44..1b0a1aaa60 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -141,19 +141,19 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void import_state_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_state_interface_descriptions(hardware_info.joints); for (const auto & description : joint_state_interface_descriptions) { joint_state_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto sensor_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.sensors); + parse_state_interface_descriptions(hardware_info.sensors); for (const auto & description : sensor_state_interface_descriptions) { sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto gpio_state_interface_descriptions = - parse_state_interface_descriptions_from_hardware_info(hardware_info.gpios); + parse_state_interface_descriptions(hardware_info.gpios); for (const auto & description : gpio_state_interface_descriptions) { gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description)); @@ -167,13 +167,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void import_command_interface_descriptions(const HardwareInfo & hardware_info) { auto joint_command_interface_descriptions = - parse_command_interface_descriptions_from_hardware_info(hardware_info.joints); + parse_command_interface_descriptions(hardware_info.joints); for (const auto & description : joint_command_interface_descriptions) { joint_command_interfaces_.insert(std::make_pair(description.get_name(), description)); } auto gpio_command_interface_descriptions = - parse_command_interface_descriptions_from_hardware_info(hardware_info.gpios); + parse_command_interface_descriptions(hardware_info.gpios); for (const auto & description : gpio_command_interface_descriptions) { gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description)); diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 9dac3d9f66..0ef6c084f9 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,7 +905,7 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } -std::vector parse_state_interface_descriptions_from_hardware_info( +std::vector parse_state_interface_descriptions( const std::vector & component_info) { std::vector component_state_interface_descriptions; @@ -922,7 +922,7 @@ std::vector parse_state_interface_descriptions_from_hardwa return component_state_interface_descriptions; } -std::vector parse_command_interface_descriptions_from_hardware_info( +std::vector parse_command_interface_descriptions( const std::vector & component_info) { std::vector component_command_interface_descriptions; diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index cdbff8ca21..6a78b0807d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1414,13 +1414,13 @@ TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardwa const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto joint_state_descriptions = - parse_state_interface_descriptions_from_hardware_info(control_hardware[0].joints); - EXPECT_EQ(joint_state_descriptions[0].prefix_name, "joint1"); - EXPECT_EQ(joint_state_descriptions[0].get_interface_type(), "position"); + parse_state_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_state_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_name(), "position"); EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); - EXPECT_EQ(joint_state_descriptions[1].prefix_name, "joint2"); - EXPECT_EQ(joint_state_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_name(), "position"); EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); } @@ -1433,15 +1433,15 @@ TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hard const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto joint_command_descriptions = - parse_command_interface_descriptions_from_hardware_info(control_hardware[0].joints); - EXPECT_EQ(joint_command_descriptions[0].prefix_name, "joint1"); - EXPECT_EQ(joint_command_descriptions[0].get_interface_type(), "position"); + parse_command_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_command_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_name(), "position"); EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); - EXPECT_EQ(joint_command_descriptions[1].prefix_name, "joint2"); - EXPECT_EQ(joint_command_descriptions[1].get_interface_type(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_name(), "position"); EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); @@ -1455,19 +1455,19 @@ TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardw const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto sensor_state_descriptions = - parse_state_interface_descriptions_from_hardware_info(control_hardware[0].sensors); - EXPECT_EQ(sensor_state_descriptions[0].prefix_name, "sensor1"); - EXPECT_EQ(sensor_state_descriptions[0].get_interface_type(), "roll"); + parse_state_interface_descriptions(control_hardware[0].sensors); + EXPECT_EQ(sensor_state_descriptions[0].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_name(), "roll"); EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); - EXPECT_EQ(sensor_state_descriptions[1].prefix_name, "sensor1"); - EXPECT_EQ(sensor_state_descriptions[1].get_interface_type(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_name(), "pitch"); EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); - EXPECT_EQ(sensor_state_descriptions[2].prefix_name, "sensor1"); - EXPECT_EQ(sensor_state_descriptions[2].get_interface_type(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_name(), "yaw"); EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); - EXPECT_EQ(sensor_state_descriptions[3].prefix_name, "sensor2"); - EXPECT_EQ(sensor_state_descriptions[3].get_interface_type(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_prefix_name(), "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_name(), "image"); EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); } @@ -1480,19 +1480,19 @@ TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardwar const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto gpio_state_descriptions = - parse_state_interface_descriptions_from_hardware_info(control_hardware[0].gpios); - EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); - EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + parse_state_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); - EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_analog_IOs"); - EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "analog_input1"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); - EXPECT_EQ(gpio_state_descriptions[2].prefix_name, "flange_analog_IOs"); - EXPECT_EQ(gpio_state_descriptions[2].get_interface_type(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_name(), "analog_input2"); EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); - EXPECT_EQ(gpio_state_descriptions[3].prefix_name, "flange_vacuum"); - EXPECT_EQ(gpio_state_descriptions[3].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_name(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); } @@ -1505,12 +1505,12 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); const auto gpio_state_descriptions = - parse_command_interface_descriptions_from_hardware_info(control_hardware[0].gpios); - EXPECT_EQ(gpio_state_descriptions[0].prefix_name, "flange_analog_IOs"); - EXPECT_EQ(gpio_state_descriptions[0].get_interface_type(), "analog_output1"); + parse_command_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); - EXPECT_EQ(gpio_state_descriptions[1].prefix_name, "flange_vacuum"); - EXPECT_EQ(gpio_state_descriptions[1].get_interface_type(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); }