From a94d670db134b28ba196b718419cb264099959af Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 7 Dec 2023 09:33:53 +0100 Subject: [PATCH 01/28] 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 c6002b8e097b06b4fb48debd6ca5cc87095c88f6 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 8 Dec 2023 10:46:02 +0100 Subject: [PATCH 02/28] 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 bb330106438c9b979461d4436f6b2e2744fac828 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 10:20:10 +0100 Subject: [PATCH 03/28] 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 d32972c2ca6725d72256926996c37f8dc9d2b22f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 11:40:37 +0100 Subject: [PATCH 04/28] 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 bda8f5d1e0c4ecf791cbd1c5847e07db1d29aaf5 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 12 Dec 2023 13:06:58 +0100 Subject: [PATCH 05/28] 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 30f9a2464fbe13373c549b452cd3f5d3fa0d83be Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 09:08:23 +0100 Subject: [PATCH 06/28] 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 bbdc3cab327eb41713b98504ddc4c759e08d47fc Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 13:36:06 +0100 Subject: [PATCH 07/28] 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 8189306639464f72a131c112e93e595f433ef239 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 16:45:30 +0100 Subject: [PATCH 08/28] 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 64b5c6b4829566ae50e92d901041c793d92362a0 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 15:49:03 +0100 Subject: [PATCH 09/28] 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 e36f48b8e3fe9463023462b899dc65c15f1ed2ec Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 16:56:27 +0100 Subject: [PATCH 10/28] 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 ee8129154488c8ce4fea0d472c7c822f63c6396f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 13:06:25 +0100 Subject: [PATCH 11/28] 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 54a86cab338fb75109f3b874d1198e8b4ad74253 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:16:47 +0100 Subject: [PATCH 12/28] 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 c645b37d9cca52a06df6f4b4d91b255da9b25ad6 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:17:03 +0100 Subject: [PATCH 13/28] 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 bcf9cf27bf94b888c4f0f3f1d482df821c4ffc88 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 21 Dec 2023 09:04:32 +0100 Subject: [PATCH 14/28] 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 7224ef8aaec23c9fd6854dcfcb0ed5d705594eaf Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 23 Jan 2024 11:09:26 +0100 Subject: [PATCH 15/28] 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 c5e743074a01cee8e9f286a9f1b724b0aa2e67ab Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 09:56:07 +0100 Subject: [PATCH 16/28] 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 3f55818eadad1aa695ad1d548b5306e3a5fce265 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 10:17:11 +0100 Subject: [PATCH 17/28] 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 d4d423af76a83dff79062ac02211c62bfad735bd Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 09:34:49 +0100 Subject: [PATCH 18/28] 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 da354d8a9ec643335ab40eecf6f0601e09da6ccd Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 10:51:39 +0100 Subject: [PATCH 19/28] 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 da326fd18488e8f8a841f5c6a189be27e30b31b2 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 1 Feb 2024 10:46:13 +0100 Subject: [PATCH 20/28] 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 c5001eaaf8b6847d89d1accb93116e53a7377c1e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 10 Apr 2024 17:34:13 +0200 Subject: [PATCH 21/28] 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 c98a1c40f05367569b3a227bf99410ff56980c51 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Apr 2024 11:43:36 +0200 Subject: [PATCH 22/28] 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 3d0b3642a14813eac37bc88573389f6bcf192723 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/28] 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 9710a4c05b6cc8dea31758a8dbdf83516d7e1f68 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 22 Apr 2024 11:16:08 +0200 Subject: [PATCH 24/28] 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 33710bb2e25c8d26baf1a0a8a93b015594d9d755 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 22 May 2024 18:05:45 +0200 Subject: [PATCH 25/28] 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 726870cb3969b8d79ea398c1fd1e1e09e66b5a93 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 25 Jul 2024 16:05:01 +0200 Subject: [PATCH 26/28] 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 265f66de57a1f3632f5f1ff75fe8a64270553ad7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jul 2024 09:57:15 +0200 Subject: [PATCH 27/28] 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 2f45ebf2b16cc483b43a5e46c67d4a2071230038 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Jan 2024 11:52:13 +0100 Subject: [PATCH 28/28] 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()); +}