From 56f9b1798676982dc8a335b48a247d6121645796 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 06358f205e..84b9992350 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. /** @@ -249,6 +395,23 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI const HardwareInfo & get_hardware_info() const { return info_; } 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 7f97182a1ae6d8822a23a848f41773cb1c6ad76d 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 84b9992350..5b136da6cf 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. /** @@ -395,23 +374,23 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI const HardwareInfo & get_hardware_info() const { return info_; } 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 0a25035b8d03a3b47ae2798f893ea8639c7e5d72 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 168f427cef5b8ba565fcc585f4606e89318cc16d 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 5b136da6cf..c99d3cd918 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()])); @@ -374,13 +347,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI const HardwareInfo & get_hardware_info() const { return info_; } 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 e1ac94c4068b1f89025beea5880b9e60e1af8102 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 1228d88bcd..a83ac3203e 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. /** @@ -248,6 +328,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod const HardwareInfo & get_hardware_info() const { return info_; } 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 6fde3f8891..b134d1a288 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. /** @@ -187,6 +230,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI const HardwareInfo & get_hardware_info() const { return info_; } 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 c99d3cd918..ac354efa78 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 08948836c611407d626a0e0002dfa12e6bfbac93 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 94eb6a8fe1758befb52fcb3cb73510be743156d9 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 b134d1a288..07067d5f25 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. /** @@ -231,10 +254,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 715ccac94dd0f0dc18fd3e6e144e687c6ae128a2 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 a83ac3203e..5a52bbb74e 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. /** @@ -328,8 +357,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod const HardwareInfo & get_hardware_info() const { return info_; } 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 07067d5f25..767107a1f2 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. /** @@ -254,12 +246,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 ac354efa78..f827e9659e 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. /** @@ -387,13 +457,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI const HardwareInfo & get_hardware_info() const { return info_; } 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 468b0c7932909051de72591fb2a2312c85bd8403 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 5a52bbb74e..9e02c45585 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: @@ -361,8 +362,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 767107a1f2..3ca41318af 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: @@ -249,7 +252,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 f827e9659e..f1edb417fb 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: @@ -466,13 +405,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 68691984d14bae0c0abcd26da1eeb97046ef1707 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 239a25b044995d405469adae5c5ed752809e4691 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 9e02c45585..3a4286b629 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 3ca41318af..5a94289f13 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 f1edb417fb..28aefc1943 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 213cbaeaf8d0f735779a6b621be9135786fdf99f 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 295b6206162dc27877cb402d6c0b47d31e376086 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 a133e2b62f8fd2859dfb592f0fe8f8168b819f79 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 3a4286b629..e7673f1632 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 5a94289f13..555c14ed30 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 28aefc1943..3d9b16e663 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 bfab95cd68deda7074c4127fd76a57fc58ec080c 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 e7673f1632..31d98a56f6 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; @@ -361,9 +376,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 555c14ed30..bd6a4aaaa2 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; @@ -251,8 +258,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 3d9b16e663..27df0c15d6 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; } @@ -404,9 +421,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 c2db38f9425d19772c890f73d8da0a7fb5206381 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 31d98a56f6..2ee8fb1ceb 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_) @@ -376,8 +376,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 bd6a4aaaa2..c78d8bd179 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_) @@ -258,7 +258,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 27df0c15d6..c8fdbbf000 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_) @@ -421,8 +421,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 323211422a34adf73e780702ea7722157d0624cc 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 2ee8fb1ceb..c01424844d 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 c78d8bd179..98bbb06b6f 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 c8fdbbf000..ccd61df2e5 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 fb3c074f523b2828d6b27307a358093dace95e8f 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 c01424844d..92272157b0 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 98bbb06b6f..f514aea3ef 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 ccd61df2e5..d5ac5aa3ff 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 3378fcffa94014fe32267c82383ba5af5c3574ad 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 92272157b0..bb26961c22 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 @@ -402,8 +401,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod const HardwareInfo & get_hardware_info() const { return info_; } 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 f514aea3ef..ab755bf595 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 @@ -270,7 +269,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 d5ac5aa3ff..a5a4de7631 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 @@ -442,13 +441,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI const HardwareInfo & get_hardware_info() const { return info_; } 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 6667d401f9a7373b938420dc1901d55c423d53ff 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 bb26961c22..a0ca2ef9ce 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_) { @@ -404,14 +437,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 ab755bf595..17f9e9eace 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_) { @@ -270,14 +287,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 a5a4de7631..02cdcc5382 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_) { @@ -449,14 +482,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 a6ab1f0eee913305dbfd868c8afce7c81439719e 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 1ffdb84e7fb8f3ae27851dadc5152a27ab897439 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 4199be161b2cc4f3bfe960a2d3419c267be9fc70 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 2d6e7ea8bfffb8e711c309d1a92b5b000790d289 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 25add2045e..337b5a619c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -753,14 +753,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 3a53368376a379d5f062b794f2f65c6c8ebb69b8 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 a0ca2ef9ce..659e5b54e8 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 17f9e9eace..f2e452c151 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 02cdcc5382..0bb6a13f59 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 e9c2669b64d17e27b3327a915bb9f04deac462c7 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 337b5a619c..e598a01023 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -752,12 +752,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 659e5b54e8..7b3c4e869b 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 f2e452c151..8b8b5275e1 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 0bb6a13f59..58bbe71a4d 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 1cea7f2a73877388f38b132acabef2a015df7eab 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 84002fdbbe6f297e72b93d6a4fd0e6673801eb45 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 14 Aug 2024 11:51:51 +0200 Subject: [PATCH 28/28] reviwe changes --- .../src/chainable_controller_interface.cpp | 2 +- doc/Iron.rst | 48 ------------------- 2 files changed, 1 insertion(+), 49 deletions(-) diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index d7cf82f32e..0c17b42095 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -52,7 +52,7 @@ ChainableControllerInterface::export_state_interfaces() state_interfaces_ptrs_vec.reserve(state_interfaces.size()); // check if the names of the controller state interfaces begin with the controller's name - for (auto & interface : state_interfaces) + for (const auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { diff --git a/doc/Iron.rst b/doc/Iron.rst index 3a463c736a..323d5ecffe 100644 --- a/doc/Iron.rst +++ b/doc/Iron.rst @@ -1,54 +1,6 @@ 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) `__