From 2a065a35069e04f011a46a38d56fa1483e2a435d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 7 Dec 2023 09:33:53 +0100 Subject: [PATCH 01/25] move creation of stateinterfaces to systeminterface --- .../include/hardware_interface/handle.hpp | 14 ++ .../hardware_interface/hardware_info.hpp | 21 +++ .../hardware_interface/system_interface.hpp | 166 +++++++++++++++++- 3 files changed, 199 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..ec3d6a6d3b 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,6 +18,7 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" @@ -116,6 +117,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; @@ -126,6 +134,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 97abc00438..eb3a02ff7f 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -122,6 +122,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 e5c6f2f542..a814b77791 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -22,6 +22,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" @@ -98,9 +99,101 @@ 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 @@ -110,7 +203,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. /** @@ -121,7 +240,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. /** @@ -205,6 +350,23 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: 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_; }; From 1e35d1f3c903524e5ffa5ba355abe09ca318e9fc Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 8 Dec 2023 10:46:02 +0100 Subject: [PATCH 02/25] store description in state_interface and provide functions for setting/gettting --- .../hardware_interface/hardware_info.hpp | 1 + .../hardware_interface/system_interface.hpp | 224 ++++++++---------- 2 files changed, 102 insertions(+), 123 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eb3a02ff7f..e9f3e2b010 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -141,6 +141,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 a814b77791..532da0634f 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include #include #include #include @@ -100,25 +101,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; }; @@ -128,32 +111,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)); } } } @@ -164,32 +138,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)); } } } @@ -206,26 +162,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; @@ -243,26 +185,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) + for (const auto & descr : joint_commands_descr_) { - 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])); + joint_commands_[descr.get_name()] = std::numeric_limits::quiet_NaN(); + command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()])); } return command_interfaces; @@ -348,25 +276,75 @@ 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: 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_; }; From 8334d9f447ddb5aa181a50f6f0bf8137c34c2d16 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 10:20:10 +0100 Subject: [PATCH 03/25] 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 e9f3e2b010..ef133f1216 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -140,7 +140,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 e3ca9ef806798265ab7b034a9017a237a4f574f0 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 11:40:37 +0100 Subject: [PATCH 04/25] 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 d5d999cca8..db11c6f0a6 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,5 +33,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 ef133f1216..8ad3276661 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -138,10 +138,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 532da0634f..af6f350c72 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" @@ -107,47 +109,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. @@ -162,9 +135,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()])); @@ -185,9 +158,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()])); @@ -328,13 +301,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: 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 0841265e81..add5c2603e 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -722,4 +722,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 112f20f0707e86333c5efbfc0d91b9bf3a98117b Mon Sep 17 00:00:00 2001 From: "Manuel M." Date: Tue, 12 Dec 2023 13:06:58 +0100 Subject: [PATCH 05/25] adjusted actuator- and sensor_interface as well --- .../hardware_interface/actuator_interface.hpp | 89 ++++++++++++++++++- .../hardware_interface/sensor_interface.hpp | 49 +++++++++- .../hardware_interface/system_interface.hpp | 62 ++++++++++--- 3 files changed, 186 insertions(+), 14 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index abfd8eb45a..73e892a703 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,10 +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" @@ -97,11 +100,38 @@ 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. * @@ -109,10 +139,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. * @@ -120,7 +166,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. /** @@ -202,8 +260,35 @@ 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: 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_; }; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 14a59e4588..485aa15d30 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,10 +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" @@ -97,11 +100,27 @@ 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. * @@ -109,7 +128,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. /** @@ -141,8 +172,24 @@ 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: HardwareInfo info_; + + std::vector sensor_states_descriptions_; + +private: + 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 af6f350c72..485cdad00d 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -102,29 +102,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. * @@ -135,7 +150,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_) { @@ -143,11 +160,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. * @@ -158,7 +191,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_) { @@ -166,6 +200,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 e6d49e3b9bb8ab6ce2ebb13781b2129621aa868d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 09:08:23 +0100 Subject: [PATCH 06/25] 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 add5c2603e..867ca590b5 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -722,11 +722,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 968d41ed97..76d83e7242 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -800,3 +800,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 4b43db4d8812f7c76709c056787e8f553280206f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 13:36:06 +0100 Subject: [PATCH 07/25] 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 4ea7fdc2f7..41f7f72aff 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -76,6 +76,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 485aa15d30..7c6ebab0a5 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" @@ -101,6 +102,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; }; @@ -110,7 +118,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); } @@ -131,9 +139,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()])); @@ -182,13 +190,30 @@ 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: 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 6dc1c394b0..2511ebcdc1 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -33,6 +33,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 @@ -1005,3 +1007,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 7b2dd46e7a..191ef07b37 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 @@ -400,6 +400,20 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +// Voltage Sensor only +const auto valid_urdf_ros2_control_voltage_sensor_only = + R"( + + + ros2_control_demo_hardware/CameraWithIMUSensor + 2 + + + + + +)"; + const auto valid_urdf_ros2_control_parameter_empty = R"( From 7f7d106e7a61b29a73fa7ffcacd0a52a87773d33 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 16:45:30 +0100 Subject: [PATCH 08/25] 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 73e892a703..381e97d239 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" @@ -111,8 +112,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)); + } } /** @@ -121,8 +126,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. @@ -142,9 +151,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()])); @@ -169,9 +178,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()])); @@ -280,10 +289,30 @@ 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: 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 7c6ebab0a5..598561567e 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -102,13 +102,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; }; @@ -118,8 +111,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. @@ -139,9 +136,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()])); @@ -200,20 +197,13 @@ 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: 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 485cdad00d..18c888234f 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" @@ -113,12 +114,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)); + } } /** @@ -127,10 +140,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. @@ -151,22 +172,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()])); @@ -191,16 +212,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()])); @@ -339,15 +359,65 @@ 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: 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 2511ebcdc1..f68b79ea7d 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1027,7 +1027,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); } @@ -1168,3 +1168,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 191ef07b37..54cb7efdd3 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 @@ -414,6 +414,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 4f3ce18d37b101f116394c70a6b945ecc3623ac8 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 15:49:03 +0100 Subject: [PATCH 09/25] 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 | 89 +++++----- .../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, 162 insertions(+), 238 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 381e97d239..e0ffafcd8c 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -148,20 +148,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 @@ -175,20 +185,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. @@ -269,44 +290,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: @@ -315,8 +316,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 ec3d6a6d3b..ee02e48330 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,45 +15,60 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#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 { /// 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; } @@ -77,50 +92,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) { } @@ -128,16 +117,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. @@ -150,7 +137,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 598561567e..06306da4c5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -133,15 +133,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; @@ -177,24 +190,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: @@ -203,7 +206,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 18c888234f..8537905823 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -168,31 +168,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; } @@ -209,23 +217,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; } @@ -309,104 +328,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: @@ -420,13 +359,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 f68b79ea7d..5fb024660f 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1029,7 +1029,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; @@ -1047,7 +1047,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; } @@ -1188,12 +1188,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; @@ -1225,17 +1225,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 bc1c0a78d8..7696db03d9 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -22,17 +22,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 dedf0623efb0cd0f1fbe0431690519c307cd9b25 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 16:56:27 +0100 Subject: [PATCH 10/25] add variant support --- .../include/hardware_interface/handle.hpp | 33 +++++++++++-------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index ee02e48330..09b0134b18 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,19 +18,24 @@ #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) @@ -39,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) { } @@ -101,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 5e244a6aff5c998526ff88bcbd55b9567dc7d2ca Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 13:06:25 +0100 Subject: [PATCH 11/25] 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 | 143 ++++++++++++++++-- hardware_interface/src/sensor.cpp | 5 + hardware_interface/src/system.cpp | 10 ++ 10 files changed, 201 insertions(+), 22 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 4082863370..5252bafcab 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -64,11 +64,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 e0ffafcd8c..111cd05f02 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -149,7 +149,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() @@ -186,7 +187,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 5d0677c587..8aaa2cf923 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -65,8 +65,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 06306da4c5..239e1b06c1 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -134,7 +134,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 ece14f814d..5267eaad59 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -65,11 +65,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 8537905823..601b20a893 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -169,7 +169,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() { @@ -218,7 +219,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 6b58e365dc..11ed5f42bd 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -192,12 +192,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 2e8ccc7b1f..b2d5f17ee6 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -430,28 +430,126 @@ 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) { - 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) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + 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 + { + 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()); } + 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) { 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 } /// Adds exported command interfaces into internal storage. @@ -465,6 +563,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; @@ -472,7 +572,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); } @@ -709,9 +828,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_; @@ -810,7 +929,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 @@ -981,7 +1100,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 2e53e447b9..5ccf1e5bbc 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -189,6 +189,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(); } const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ee942d6581..93a78ec093 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -189,11 +189,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 8313284a98ca9b8b8de352efbb21c15c64da4702 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:16:47 +0100 Subject: [PATCH 12/25] cleanup --- .../include/hardware_interface/handle.hpp | 15 +++++--- hardware_interface/src/resource_manager.cpp | 34 +++++++++---------- 2 files changed, 28 insertions(+), 21 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 b2d5f17ee6..bf93d8d2b2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -443,8 +443,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( @@ -457,7 +457,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) @@ -468,7 +468,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(); @@ -479,8 +479,8 @@ class ResourceStorage interface_names.push_back(interface->get_name()); } } - // 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 else { interface_names.reserve(interfaces.size()); @@ -490,7 +490,7 @@ class ResourceStorage interface_names.push_back(interface.get_name()); } } - // TODO(Manuel) END: for backward compatibility + // END: for backward compatibility hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( @@ -510,8 +510,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(); @@ -525,7 +525,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) @@ -535,21 +535,21 @@ 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(); 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 + // BEGIN (Handle export change): 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 + // END: for backward compatibility } /// Adds exported command interfaces into internal storage. @@ -563,8 +563,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; @@ -581,7 +581,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 322412d03379900fe219cc51774d9fa729ca1bdc Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:17:03 +0100 Subject: [PATCH 13/25] 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 ac89dc1553..e5b4100e67 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -511,7 +511,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -542,7 +541,6 @@ class TestGenericSystem : public ::testing::Test 2.78 - @@ -551,7 +549,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 5fb024660f..c067a968b9 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -1089,26 +1089,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) @@ -1124,7 +1124,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(); @@ -1146,7 +1146,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()); @@ -1277,23 +1277,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 @@ -1301,8 +1301,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)); } @@ -1316,8 +1316,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)); } @@ -1331,8 +1331,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)); } @@ -1346,8 +1346,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 5421081fa7c58da6d6a97f4a6c83a459fb45518d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 21 Dec 2023 09:04:32 +0100 Subject: [PATCH 14/25] 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 | 1202 ++++++++++++----- .../components_urdfs.hpp | 35 + 6 files changed, 902 insertions(+), 355 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 111cd05f02..4bd788603c 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -157,8 +157,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() @@ -195,8 +194,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 239e1b06c1..040b459ffe 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -141,8 +141,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 601b20a893..c152f16bf7 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -176,8 +176,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() @@ -227,8 +226,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 c067a968b9..c9ba434a9a 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -51,6 +51,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 @@ -157,7 +158,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 @@ -221,7 +311,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 @@ -355,21 +510,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"; } @@ -422,6 +677,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{}; auto state = sensor_hw.initialize(mock_hw_info); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -542,7 +897,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()); @@ -666,6 +1060,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) { @@ -698,6 +1223,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()); @@ -756,18 +1282,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{}; - auto state = actuator_hw.initialize(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.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()); @@ -779,9 +1314,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()); @@ -789,8 +1324,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()); @@ -802,9 +1337,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()); @@ -816,23 +1351,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{}; - auto state = sensor_hw.initialize(mock_hw_info); + auto state = actuator_hw.initialize(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 = 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 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, 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{}; + auto state = sensor_hw.initialize(mock_hw_info); + + 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)); @@ -879,7 +1542,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()); @@ -943,7 +1666,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()); @@ -1007,353 +1802,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 54cb7efdd3..bc0124d8ae 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 @@ -440,6 +440,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 6b214a2ec2b03edbc2abbf8917eac05e9ef46e45 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 23 Jan 2024 11:09:26 +0100 Subject: [PATCH 15/25] 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 | 92 +----- 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, 673 insertions(+), 351 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 41f7f72aff..11938f0253 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -78,6 +78,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 5252bafcab..b854730c52 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -64,23 +64,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 4bd788603c..c526c0a4b6 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 @@ -136,20 +137,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 @@ -160,33 +159,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 @@ -197,15 +204,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; @@ -315,9 +330,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 db11c6f0a6..6eb7eed8fe 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -78,8 +78,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 8ad3276661..bc54371e0c 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -128,15 +128,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 @@ -148,7 +148,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 8aaa2cf923..8a6111c3cd 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -65,14 +65,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 040b459ffe..b3e30d499c 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 @@ -121,20 +122,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() @@ -144,17 +143,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; @@ -205,8 +212,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 5267eaad59..f5a9759929 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -65,23 +65,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 c152f16bf7..ff3353fb97 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 @@ -156,20 +157,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() @@ -179,46 +178,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 @@ -229,21 +237,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; } @@ -358,9 +375,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 11ed5f42bd..76ac732e34 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -186,26 +186,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 bf93d8d2b2..4453ae22e5 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -430,67 +430,26 @@ 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) { 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()); - } - } - // BEGIN (Handle export change): 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()); } - // END: for backward compatibility hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( @@ -530,26 +489,10 @@ class ResourceStorage template void import_command_interfaces(HardwareT & hardware) { - 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); - } - // BEGIN (Handle export change): 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); - } - // END: for backward compatibility + std::vector> interfaces = + hardware.export_command_interfaces(); + + hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } /// Adds exported command interfaces into internal storage. @@ -563,8 +506,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; @@ -581,7 +522,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 5ccf1e5bbc..e758953369 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -184,14 +184,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 93a78ec093..a64fb78fba 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -184,24 +184,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 c9ba434a9a..7588b594d3 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( @@ -878,24 +878,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 @@ -914,7 +914,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()); @@ -948,41 +948,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 @@ -990,12 +990,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)); } @@ -1009,12 +1009,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)); } @@ -1028,12 +1028,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)); } @@ -1047,12 +1047,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)); } @@ -1077,7 +1077,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()); @@ -1098,7 +1098,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()); @@ -1114,12 +1114,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 @@ -1128,7 +1128,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(); @@ -1176,7 +1176,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 @@ -1185,7 +1185,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({}, {})); @@ -1256,8 +1256,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()); @@ -1301,8 +1301,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()); @@ -1384,8 +1384,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()); @@ -1428,8 +1428,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()); @@ -1514,7 +1514,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()); @@ -1557,7 +1557,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(); @@ -1637,11 +1637,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()); @@ -1683,8 +1683,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()); @@ -1773,11 +1773,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()); @@ -1819,8 +1819,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 76d83e7242..d51cc32dea 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -811,11 +811,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"); } @@ -830,13 +830,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"); @@ -852,17 +852,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"); } @@ -877,17 +877,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"); } @@ -902,11 +902,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 bc0124d8ae..ac5bcd1173 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 @@ -403,9 +403,9 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = // 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 8ea003be044dcd60d67f7a3e07383dd1f6f3e0e2 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 09:56:07 +0100 Subject: [PATCH 16/25] 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 b854730c52..b3c668bf38 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -65,10 +65,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 c526c0a4b6..c09f949c22 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -148,7 +148,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 @@ -166,9 +166,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_) @@ -193,7 +193,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 @@ -211,9 +211,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_) @@ -330,8 +330,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 bc54371e0c..8ad3276661 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -128,15 +128,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 @@ -148,7 +148,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 8a6111c3cd..dd6579f66e 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -66,7 +66,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 b3e30d499c..547b52c47a 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -133,7 +133,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() @@ -150,9 +150,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_) @@ -212,7 +212,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 f5a9759929..f74ff5a7f7 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -66,10 +66,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 ff3353fb97..a9221e6659 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -168,7 +168,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() @@ -185,9 +185,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()); @@ -226,7 +226,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 @@ -244,9 +244,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_) @@ -375,8 +375,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 d51cc32dea..76d83e7242 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -811,11 +811,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"); } @@ -830,13 +830,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"); @@ -852,17 +852,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"); } @@ -877,17 +877,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"); } @@ -902,11 +902,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 373b1824a9fcd7e470757b4ca97c13180bcb8195 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 10:17:11 +0100 Subject: [PATCH 17/25] 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 c09f949c22..91b0adfca6 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -114,7 +114,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)); @@ -128,7 +128,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 6eb7eed8fe..bb68e72b12 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -34,49 +34,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 547b52c47a..193d0fd1c4 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -113,7 +113,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 a9221e6659..c73e4c069c 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -116,19 +116,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)); @@ -142,13 +142,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 867ca590b5..09ff91467a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -722,74 +722,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 76d83e7242..71f428053d 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -810,7 +810,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"); @@ -829,7 +829,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"); @@ -851,7 +851,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"); @@ -876,7 +876,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"); @@ -901,7 +901,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 18301daea6c90c1755e3fd5a2f6beaafc0b62d32 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 09:34:49 +0100 Subject: [PATCH 18/25] 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 91b0adfca6..19af19d1a0 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -159,6 +159,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 @@ -168,7 +182,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_) @@ -204,6 +218,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 @@ -213,7 +241,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 8ad3276661..6322fdc17e 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -38,9 +38,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; }; @@ -138,11 +138,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 193d0fd1c4..db8377f661 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -143,6 +143,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 @@ -152,7 +166,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 c73e4c069c..a34335a3df 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -178,6 +178,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 @@ -185,9 +199,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()); @@ -237,6 +251,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 @@ -244,9 +272,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 d7b2fb1fa903c0e51d0255ccc1aa469b46a29d4f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 10:51:39 +0100 Subject: [PATCH 19/25] 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 | 278 ++++++++++++------ 4 files changed, 200 insertions(+), 97 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 19af19d1a0..46369f9499 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 @@ -356,8 +355,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod protected: 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 db8377f661..c61abf4157 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 @@ -224,7 +223,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: 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 a34335a3df..07135c72a6 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 @@ -396,13 +395,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: 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 7588b594d3..ef856f58f5 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -35,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 +789,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 +845,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 +861,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 +878,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)); } @@ -916,24 +940,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 @@ -1079,54 +1109,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)); } @@ -1140,12 +1230,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)); } @@ -1159,12 +1252,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)); } @@ -1178,12 +1277,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)); } @@ -1323,9 +1422,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()); @@ -1450,9 +1552,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()); @@ -1578,8 +1683,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 e2f15641c13908af141478a6df62161e66a9d2eb Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 1 Feb 2024 10:46:13 +0100 Subject: [PATCH 20/25] make states and commands of component interfaces private --- .../hardware_interface/actuator_interface.hpp | 57 ++++++++++++++---- .../hardware_interface/sensor_interface.hpp | 31 ++++++++-- .../hardware_interface/system_interface.hpp | 59 ++++++++++++++---- hardware_interface/src/component_parser.cpp | 17 ------ ...est_component_interfaces_custom_export.cpp | 60 ++++++++----------- 5 files changed, 145 insertions(+), 79 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 46369f9499..30961316e7 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -160,13 +160,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 {}; @@ -181,8 +182,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_) { @@ -219,13 +236,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 {}; @@ -240,9 +258,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_) { @@ -358,10 +391,14 @@ 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: + 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 c61abf4157..2eeeba3482 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -144,13 +144,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 {}; @@ -165,8 +166,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_) { @@ -224,10 +241,12 @@ 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: + 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 07135c72a6..a5dd048215 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -179,13 +179,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 {}; @@ -200,10 +201,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_) { @@ -252,13 +268,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 {}; @@ -273,9 +290,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_) { @@ -403,10 +436,14 @@ 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: + 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 09ff91467a..40a646dea4 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -756,21 +756,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 a48e726d05d0c8c678f6a442c91cc361d24cfb23 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Jan 2024 11:52:13 +0100 Subject: [PATCH 21/25] add interface for warning, error and report --- hardware_interface/CMakeLists.txt | 4 + .../hardware_interface/actuator_interface.hpp | 94 +++ .../include/hardware_interface/handle.hpp | 4 +- .../hardware_interface/sensor_interface.hpp | 76 ++ .../hardware_interface/system_interface.hpp | 94 +++ ...rdware_interface_emergency_stop_signal.hpp | 29 + .../hardware_interface_error_signals.hpp | 105 +++ .../hardware_interface_warning_signals.hpp | 104 +++ .../test/test_component_interfaces.cpp | 11 +- ...est_component_interfaces_custom_export.cpp | 25 +- .../test/test_error_warning_codes.cpp | 727 ++++++++++++++++++ 11 files changed, 1263 insertions(+), 10 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp create mode 100644 hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp create mode 100644 hardware_interface/test/test_error_warning_codes.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 11938f0253..4367af5d53 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -82,6 +82,10 @@ if(BUILD_TESTING) target_link_libraries(test_component_interfaces_custom_export hardware_interface) ament_target_dependencies(test_component_interfaces_custom_export ros2_control_test_assets) + ament_add_gmock(test_error_warning_codes test/test_error_warning_codes.cpp) + target_link_libraries(test_error_warning_codes hardware_interface) + ament_target_dependencies(test_error_warning_codes ros2_control_test_assets) + ament_add_gmock(test_component_parser test/test_component_parser.cpp) target_link_libraries(test_component_parser hardware_interface) ament_target_dependencies(test_component_parser ros2_control_test_assets) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 30961316e7..fc4328aa0b 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -25,7 +25,10 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -103,6 +106,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod info_ = hardware_info; import_state_interface_descriptions(info_); import_command_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -134,6 +138,52 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod } } + /** + * Creates all interfaces used for reporting emergency stop, warning and error messages. + * The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called joint_1 -> interface for WARNING_SIGNAL is called: joint_1/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // EMERGENCY STOP + InterfaceInfo emergency_interface_info; + emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL; + emergency_interface_info.data_type = "bool"; + InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info); + emergency_stop_ = std::make_shared(emergency_interface_descr); + + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "std::array"; + InterfaceDescription error_interface_descr(info_.name, error_interface_info); + error_signal_ = std::make_shared(error_interface_descr); + // create error signal report message interface + InterfaceInfo error_msg_interface_info; + error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; + error_msg_interface_info.data_type = "std::array"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "std::array"; + InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); + warning_signal_ = std::make_shared(warning_interface_descr); + // create warning signal report message interface + InterfaceInfo warning_msg_interface_info; + warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; + warning_msg_interface_info.data_type = "std::array"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -207,6 +257,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod actuator_states_.insert(std::make_pair(name, state_interface)); state_interfaces.push_back(state_interface); } + + // export warning signal interfaces + state_interfaces.push_back(emergency_stop_); + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -386,6 +444,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod return actuator_commands_.at(interface_name)->get_value(); } + void set_emergency_stop(const double & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + double get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: HardwareInfo info_; std::unordered_map joint_state_interfaces_; @@ -394,6 +481,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; +private: + std::shared_ptr emergency_stop_; + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + rclcpp_lifecycle::State lifecycle_state_; private: diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 72b1d6cf92..16d06c578d 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include #include @@ -23,8 +24,9 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/visibility_control.h" - namespace hardware_interface { diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2eeeba3482..154120edd8 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -25,7 +25,9 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -102,6 +104,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { info_ = hardware_info; import_state_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -119,6 +122,45 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } } + /** + * Creates all interfaces used for reporting warning and error messages. + * The available report interfaces are: ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called sensor_1 -> interface for WARNING_SIGNAL is called: sensor_1/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "std::array"; + InterfaceDescription error_interface_descr(info_.name, error_interface_info); + error_signal_ = std::make_shared(error_interface_descr); + // create error signal report message interface + InterfaceInfo error_msg_interface_info; + error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; + error_msg_interface_info.data_type = "std::array"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "std::array"; + InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); + warning_signal_ = std::make_shared(warning_interface_descr); + // create warning signal report message interface + InterfaceInfo warning_msg_interface_info; + warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; + warning_msg_interface_info.data_type = "std::array"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -194,6 +236,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI state_interfaces.push_back(state_interface); } + // export warning signal interfaces + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -237,12 +285,40 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return sensor_states_.at(interface_name)->get_value(); } + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: HardwareInfo info_; std::unordered_map sensor_state_interfaces_; std::unordered_map unlisted_state_interfaces_; +private: + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + rclcpp_lifecycle::State lifecycle_state_; private: diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index a5dd048215..71ca108e51 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,8 +25,11 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" @@ -105,6 +108,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI info_ = hardware_info; import_state_interface_descriptions(info_); import_command_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; @@ -154,6 +158,52 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } } + /** + * Creates all interfaces used for reporting emergency stop, warning and error messages. + * The available report interfaces are: EMERGENCY_STOP_SIGNAL, ERROR_SIGNAL, ERROR_SIGNAL_MESSAGE, + * WARNING_SIGNAL and WARNING_SIGNAL_MESSAGE. Where the _MESSAGE hold the message for + * the corresponding report signal. + * The interfaces are named like /. E.g. if hardware is + * called rrbot -> interface for WARNING_SIGNAL is called: rrbot/WARNING_SIGNAL + */ + void create_report_interfaces() + { + // EMERGENCY STOP + InterfaceInfo emergency_interface_info; + emergency_interface_info.name = hardware_interface::EMERGENCY_STOP_SIGNAL; + emergency_interface_info.data_type = "bool"; + InterfaceDescription emergency_interface_descr(info_.name, emergency_interface_info); + emergency_stop_ = std::make_shared(emergency_interface_descr); + + // ERROR + // create error signal interface + InterfaceInfo error_interface_info; + error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; + error_interface_info.data_type = "std::array"; + InterfaceDescription error_interface_descr(info_.name, error_interface_info); + error_signal_ = std::make_shared(error_interface_descr); + // create error signal report message interface + InterfaceInfo error_msg_interface_info; + error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; + error_msg_interface_info.data_type = "std::array"; + InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); + error_signal_message_ = std::make_shared(error_msg_interface_descr); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + warning_interface_info.data_type = "std::array"; + InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); + warning_signal_ = std::make_shared(warning_interface_descr); + // create warning signal report message interface + InterfaceInfo warning_msg_interface_info; + warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; + warning_msg_interface_info.data_type = "std::array"; + InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); + warning_signal_message_ = std::make_shared(warning_msg_interface_descr); + } + /// Exports all state interfaces for this hardware interface. /** * Old way of exporting the StateInterfaces. If a empty vector is returned then @@ -239,6 +289,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI system_states_.insert(std::make_pair(name, state_interface)); state_interfaces.push_back(state_interface); } + + // export warning signal interfaces + state_interfaces.push_back(emergency_stop_); + state_interfaces.push_back(error_signal_); + state_interfaces.push_back(error_signal_message_); + state_interfaces.push_back(warning_signal_); + state_interfaces.push_back(warning_signal_message_); + return state_interfaces; } @@ -426,6 +484,35 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return system_commands_.at(interface_name)->get_value(); } + void set_emergency_stop(const double & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + double get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + + double get_error_code() const { return error_signal_->get_value(); } + + void set_error_message(const double & error_message) + { + error_signal_message_->set_value(error_message); + } + + double get_error_message() const { return error_signal_message_->get_value(); } + + void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + + double get_warning_code() const { return warning_signal_->get_value(); } + + void set_warning_message(const double & error_message) + { + warning_signal_message_->set_value(error_message); + } + + double get_warning_message() const { return warning_signal_message_->get_value(); } + protected: HardwareInfo info_; std::unordered_map joint_state_interfaces_; @@ -439,6 +526,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; +private: + std::shared_ptr emergency_stop_; + std::shared_ptr error_signal_; + std::shared_ptr error_signal_message_; + std::shared_ptr warning_signal_; + std::shared_ptr warning_signal_message_; + rclcpp_lifecycle::State lifecycle_state_; private: diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp new file mode 100644 index 0000000000..1478556f5f --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp @@ -0,0 +1,29 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ + +#include +#include + +namespace hardware_interface +{ +// Count of how many different emergency stop signals there are that can be reported. +const size_t emergency_stop_signal_count = 1; + +constexpr char EMERGENCY_STOP_SIGNAL[] = "EMERGENCY_STOP_SIGNAL"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp new file mode 100644 index 0000000000..4b5095ef39 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -0,0 +1,105 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ + +#include +#include + +namespace hardware_interface +{ + +// Count of how many different error signals there are that can be reported. +const size_t error_signal_count = 32; + +constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; +// Available error signal names +enum class error_signal : std::uint8_t +{ + ERROR_SIGNAL_0 = 0, + ERROR_SIGNAL_1 = 1, + ERROR_SIGNAL_2 = 2, + ERROR_SIGNAL_3 = 3, + ERROR_SIGNAL_4 = 4, + ERROR_SIGNAL_5 = 5, + ERROR_SIGNAL_6 = 6, + ERROR_SIGNAL_7 = 7, + ERROR_SIGNAL_8 = 8, + ERROR_SIGNAL_9 = 9, + ERROR_SIGNAL_10 = 10, + ERROR_SIGNAL_11 = 11, + ERROR_SIGNAL_12 = 12, + ERROR_SIGNAL_13 = 13, + ERROR_SIGNAL_14 = 14, + ERROR_SIGNAL_15 = 15, + ERROR_SIGNAL_16 = 16, + ERROR_SIGNAL_17 = 17, + ERROR_SIGNAL_18 = 18, + ERROR_SIGNAL_19 = 19, + ERROR_SIGNAL_20 = 20, + ERROR_SIGNAL_21 = 21, + ERROR_SIGNAL_22 = 22, + ERROR_SIGNAL_23 = 23, + ERROR_SIGNAL_24 = 24, + ERROR_SIGNAL_25 = 25, + ERROR_SIGNAL_26 = 26, + ERROR_SIGNAL_27 = 27, + ERROR_SIGNAL_28 = 28, + ERROR_SIGNAL_29 = 29, + ERROR_SIGNAL_30 = 30, + ERROR_SIGNAL_31 = 31 +}; + +constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; +// Available WARNING signal message names +enum class error_signal_message : std::uint8_t +{ + ERROR_SIGNAL_MESSAGE_0 = 0, + ERROR_SIGNAL_MESSAGE_1 = 1, + ERROR_SIGNAL_MESSAGE_2 = 2, + ERROR_SIGNAL_MESSAGE_3 = 3, + ERROR_SIGNAL_MESSAGE_4 = 4, + ERROR_SIGNAL_MESSAGE_5 = 5, + ERROR_SIGNAL_MESSAGE_6 = 6, + ERROR_SIGNAL_MESSAGE_7 = 7, + ERROR_SIGNAL_MESSAGE_8 = 8, + ERROR_SIGNAL_MESSAGE_9 = 9, + ERROR_SIGNAL_MESSAGE_10 = 10, + ERROR_SIGNAL_MESSAGE_11 = 11, + ERROR_SIGNAL_MESSAGE_12 = 12, + ERROR_SIGNAL_MESSAGE_13 = 13, + ERROR_SIGNAL_MESSAGE_14 = 14, + ERROR_SIGNAL_MESSAGE_15 = 15, + ERROR_SIGNAL_MESSAGE_16 = 16, + ERROR_SIGNAL_MESSAGE_17 = 17, + ERROR_SIGNAL_MESSAGE_18 = 18, + ERROR_SIGNAL_MESSAGE_19 = 19, + ERROR_SIGNAL_MESSAGE_20 = 20, + ERROR_SIGNAL_MESSAGE_21 = 21, + ERROR_SIGNAL_MESSAGE_22 = 22, + ERROR_SIGNAL_MESSAGE_23 = 23, + ERROR_SIGNAL_MESSAGE_24 = 24, + ERROR_SIGNAL_MESSAGE_25 = 25, + ERROR_SIGNAL_MESSAGE_26 = 26, + ERROR_SIGNAL_MESSAGE_27 = 27, + ERROR_SIGNAL_MESSAGE_28 = 28, + ERROR_SIGNAL_MESSAGE_29 = 29, + ERROR_SIGNAL_MESSAGE_30 = 30, + ERROR_SIGNAL_MESSAGE_31 = 31 +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp new file mode 100644 index 0000000000..434f2ec988 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -0,0 +1,104 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ + +#include +#include + +namespace hardware_interface +{ +// Count of how many different warn signals there are that can be reported. +const size_t warning_signal_count = 32; + +constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; +// Available warning signals names mapping to position in the interface +enum class warning_signal : std::uint8_t +{ + WARNING_SIGNAL_0 = 0, + WARNING_SIGNAL_1 = 1, + WARNING_SIGNAL_2 = 2, + WARNING_SIGNAL_3 = 3, + WARNING_SIGNAL_4 = 4, + WARNING_SIGNAL_5 = 5, + WARNING_SIGNAL_6 = 6, + WARNING_SIGNAL_7 = 7, + WARNING_SIGNAL_8 = 8, + WARNING_SIGNAL_9 = 9, + WARNING_SIGNAL_10 = 10, + WARNING_SIGNAL_11 = 11, + WARNING_SIGNAL_12 = 12, + WARNING_SIGNAL_13 = 13, + WARNING_SIGNAL_14 = 14, + WARNING_SIGNAL_15 = 15, + WARNING_SIGNAL_16 = 16, + WARNING_SIGNAL_17 = 17, + WARNING_SIGNAL_18 = 18, + WARNING_SIGNAL_19 = 19, + WARNING_SIGNAL_20 = 20, + WARNING_SIGNAL_21 = 21, + WARNING_SIGNAL_22 = 22, + WARNING_SIGNAL_23 = 23, + WARNING_SIGNAL_24 = 24, + WARNING_SIGNAL_25 = 25, + WARNING_SIGNAL_26 = 26, + WARNING_SIGNAL_27 = 27, + WARNING_SIGNAL_28 = 28, + WARNING_SIGNAL_29 = 29, + WARNING_SIGNAL_30 = 30, + WARNING_SIGNAL_31 = 31 +}; + +constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; +// Available WARNING signal message names +enum class warning_signal_message : std::uint8_t +{ + WARNING_SIGNAL_MESSAGE_0 = 0, + WARNING_SIGNAL_MESSAGE_1 = 1, + WARNING_SIGNAL_MESSAGE_2 = 2, + WARNING_SIGNAL_MESSAGE_3 = 3, + WARNING_SIGNAL_MESSAGE_4 = 4, + WARNING_SIGNAL_MESSAGE_5 = 5, + WARNING_SIGNAL_MESSAGE_6 = 6, + WARNING_SIGNAL_MESSAGE_7 = 7, + WARNING_SIGNAL_MESSAGE_8 = 8, + WARNING_SIGNAL_MESSAGE_9 = 9, + WARNING_SIGNAL_MESSAGE_10 = 10, + WARNING_SIGNAL_MESSAGE_11 = 11, + WARNING_SIGNAL_MESSAGE_12 = 12, + WARNING_SIGNAL_MESSAGE_13 = 13, + WARNING_SIGNAL_MESSAGE_14 = 14, + WARNING_SIGNAL_MESSAGE_15 = 15, + WARNING_SIGNAL_MESSAGE_16 = 16, + WARNING_SIGNAL_MESSAGE_17 = 17, + WARNING_SIGNAL_MESSAGE_18 = 18, + WARNING_SIGNAL_MESSAGE_19 = 19, + WARNING_SIGNAL_MESSAGE_20 = 20, + WARNING_SIGNAL_MESSAGE_21 = 21, + WARNING_SIGNAL_MESSAGE_22 = 22, + WARNING_SIGNAL_MESSAGE_23 = 23, + WARNING_SIGNAL_MESSAGE_24 = 24, + WARNING_SIGNAL_MESSAGE_25 = 25, + WARNING_SIGNAL_MESSAGE_26 = 26, + WARNING_SIGNAL_MESSAGE_27 = 27, + WARNING_SIGNAL_MESSAGE_28 = 28, + WARNING_SIGNAL_MESSAGE_29 = 29, + WARNING_SIGNAL_MESSAGE_30 = 30, + WARNING_SIGNAL_MESSAGE_31 = 31 +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index ef856f58f5..3362d1eb53 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -45,6 +45,11 @@ namespace const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +const auto warnig_signals_size = 2; +const auto error_signals_size = 2; +const auto report_signals_size = + emergency_stop_signal_size + warnig_signals_size + error_signals_size; } // namespace using namespace ::testing; // NOLINT @@ -788,7 +793,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); + ASSERT_EQ(2u + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -939,7 +944,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); + ASSERT_EQ(1u + warnig_signals_size + error_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/voltage"); @@ -1108,7 +1113,7 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(6u, state_interfaces.size()); + ASSERT_EQ(6u + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 7a5011892b..dccd49aed1 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -40,11 +40,15 @@ // Values to send over command interface to trigger error in write and read methods +// Values to send over command interface to trigger error in write and read methods + namespace { -const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); -constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +const auto warnig_signals_size = 2; +const auto error_signals_size = 2; +const auto report_signals_size = + emergency_stop_signal_size + warnig_signals_size + error_signals_size; } // namespace using namespace ::testing; // NOLINT @@ -171,8 +175,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 2u; + const auto interfaces_size = listed_interface_size + report_signals_size; auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(3u, state_interfaces.size()); + // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" + ASSERT_EQ(interfaces_size + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -234,8 +241,11 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 1u; + const auto interfaces_sizeze = listed_interface_size + error_signals_size + warnig_signals_size; auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(2u, state_interfaces.size()); + // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" + ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/voltage"); @@ -270,8 +280,11 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + const auto listed_interface_size = 6u; + const auto interfaces_sizeze = listed_interface_size + report_signals_size; auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(7u, state_interfaces.size()); + // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" + ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp new file mode 100644 index 0000000000..ee847fa1d3 --- /dev/null +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -0,0 +1,727 @@ +// 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 + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" +// Values to send over command interface to trigger error in write and read methods + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +const auto warnig_signals_size = 2; +const auto error_signals_size = 2; +const auto report_signals_size = + emergency_stop_signal_size + warnig_signals_size + error_signals_size; +} // namespace + +using namespace ::testing; // NOLINT + +namespace test_components +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class DummyActuatorDefault : public hardware_interface::ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + // We hardcode the info + if ( + hardware_interface::ActuatorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); + set_emergency_stop(0.0); + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + + if (recoverable_error_happened_) + { + set_command("joint1/velocity", 0.0); + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummyActuatorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + set_state("joint1/position", position_state + get_command("joint1/velocity")); + set_state("joint1/velocity", get_command("joint1/velocity")); + + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + set_state("joint1/velocity", 0.0); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySensorDefault : public hardware_interface::SensorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SensorInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & [name, descr] : sensor_state_interfaces_) + { + set_state(name, 0.0); + } + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + read_calls_ = 0; + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySensorDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + // no-op, static value + set_state("joint1/voltage", voltage_level_hw_value_); + return hardware_interface::return_type::OK; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + double voltage_level_hw_value_ = 0x666; + + // Helper variables to initiate error on read + int read_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +class DummySystemDefault : public hardware_interface::SystemInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + // We hardcode the info + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (auto i = 0ul; i < 3; ++i) + { + set_state(position_states_[i], 0.0); + set_state(velocity_states_[i], 0.0); + } + set_emergency_stop(0.0); + set_error_code(0.0); + set_error_message(0.0); + set_warning_code(0.0); + set_warning_message(0.0); + // reset command only if error is initiated + if (recoverable_error_happened_) + { + for (auto i = 0ul; i < 3; ++i) + { + set_command(velocity_commands_[i], 0.0); + } + } + + read_calls_ = 0; + write_calls_ = 0; + + return CallbackReturn::SUCCESS; + } + + std::string get_name() const override { return "DummySystemDefault"; } + + hardware_interface::return_type read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++read_calls_; + if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + // no-op, state is getting propagated within write. + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + ++write_calls_; + if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) + { + set_emergency_stop(1.0); + set_error_code(1.0); + set_error_message(1.0); + set_warning_code(1.0); + set_warning_message(1.0); + return hardware_interface::return_type::ERROR; + } + + for (auto i = 0; i < 3; ++i) + { + auto current_pos = get_state(position_states_[i]); + set_state(position_states_[i], current_pos + get_command(velocity_commands_[i])); + set_state(velocity_states_[i], get_command(velocity_commands_[i])); + } + return hardware_interface::return_type::OK; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override + { + for (const auto & velocity_state : velocity_states_) + { + set_state(velocity_state, 0.0); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override + { + if (!recoverable_error_happened_) + { + recoverable_error_happened_ = true; + return CallbackReturn::SUCCESS; + } + else + { + return CallbackReturn::ERROR; + } + return CallbackReturn::FAILURE; + } + +private: + std::vector position_states_ = { + "joint1/position", "joint2/position", "joint3/position"}; + std::vector velocity_states_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + std::vector velocity_commands_ = { + "joint1/velocity", "joint2/velocity", "joint3/velocity"}; + + // Helper variables to initiate error on read + unsigned int read_calls_ = 0; + unsigned int write_calls_ = 0; + bool recoverable_error_happened_ = false; +}; + +} // namespace test_components + +TEST(TestComponentInterfaces, dummy_actuator_default) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + // EMERGENCY STOP + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::EMERGENCY_STOP_SIGNAL, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + // ERROR_SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + // WARNING SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("ActuatorModularJoint1", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = actuator_hw.export_command_interfaces(); + ASSERT_EQ(1u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_sensor_default) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 1; + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ( + state_interface_offset + warnig_signals_size + error_signals_size, state_interfaces.size()); + // check that the normal interfaces get exported as expected + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/voltage"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); + EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + } + // ERROR_SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } + // WARNING SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "SingleJointVoltage/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "SingleJointVoltage/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("SingleJointVoltage", state_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_system_default) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + // EMERGENCY STOP + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::EMERGENCY_STOP_SIGNAL, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + // ERROR_SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + // WARNING SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(3u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } +} \ No newline at end of file From 63c523f417e28d9d557b71954c6719deacf7c8f6 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 24 Jan 2024 09:43:53 +0100 Subject: [PATCH 22/25] review suggestions --- .../include/hardware_interface/actuator_interface.hpp | 8 ++++---- .../include/hardware_interface/sensor_interface.hpp | 8 ++++---- .../include/hardware_interface/system_interface.hpp | 8 ++++---- .../types/hardware_interface_emergency_stop_signal.hpp | 2 +- .../types/hardware_interface_error_signals.hpp | 2 +- .../types/hardware_interface_warning_signals.hpp | 2 +- .../test/test_component_interfaces_custom_export.cpp | 2 +- hardware_interface/test/test_error_warning_codes.cpp | 2 +- 8 files changed, 17 insertions(+), 17 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index fc4328aa0b..1bb07ae29e 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -159,13 +159,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "std::array"; + error_interface_info.data_type = "array[32]"; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "std::array"; + error_msg_interface_info.data_type = "array[32]"; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -173,13 +173,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "std::array"; + warning_interface_info.data_type = "array[32]"; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "std::array"; + warning_msg_interface_info.data_type = "array[32]"; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 154120edd8..2718028002 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -136,13 +136,13 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "std::array"; + error_interface_info.data_type = "array[32]"; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "std::array"; + error_msg_interface_info.data_type = "array[32]"; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -150,13 +150,13 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "std::array"; + warning_interface_info.data_type = "array[32]"; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "std::array"; + warning_msg_interface_info.data_type = "array[32]"; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 71ca108e51..c7884835f5 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -179,13 +179,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "std::array"; + error_interface_info.data_type = "array[32]"; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "std::array"; + error_msg_interface_info.data_type = "array[32]"; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -193,13 +193,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "std::array"; + warning_interface_info.data_type = "array[32]"; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "std::array"; + warning_msg_interface_info.data_type = "array[32]"; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp index 1478556f5f..c74941f9df 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp index 4b5095ef39..d5159f29c8 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp index 434f2ec988..58af04b80a 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index dccd49aed1..695ddc001f 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control development team +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index ee847fa1d3..8c3fd3f637 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control development team +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From b692f613c01f1d336b8351c4283051f5e86b2145 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 12:23:49 +0100 Subject: [PATCH 23/25] add basic test for error codes setting --- .../test/test_error_warning_codes.cpp | 529 +++++++++++++++++- 1 file changed, 528 insertions(+), 1 deletion(-) diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index 8c3fd3f637..06e13a9fa3 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -724,4 +724,531 @@ TEST(TestComponentInterfaces, dummy_system_default) hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); } -} \ No newline at end of file +} + +TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + auto si_joint1_pos = test_components::vector_contains(state_interfaces, "joint1/position").second; + auto ci_joint1_vel = + test_components::vector_contains(command_interfaces, "joint1/velocity").second; + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + auto command_interfaces = actuator_hw.export_command_interfaces(); + state = actuator_hw.configure(); + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = actuator_hw.configure(); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + + state = actuator_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "ActuatorModularJoint1/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "ActuatorModularJoint1/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = actuator_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = actuator_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) +{ + hardware_interface::Sensor sensor_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_voltage_sensor_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; + auto state = sensor_hw.initialize(voltage_sensor_res); + + const auto state_interface_offset = 1; + auto state_interfaces = sensor_hw.export_state_interfaces(); + // Updated because is is INACTIVE + state = sensor_hw.configure(); + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + + // Initiate recoverable error - call read 99 times OK and on 100-time will return error + auto error_signal = + test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "SingleJointVoltage/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "SingleJointVoltage/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = sensor_hw.configure(); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + + state = sensor_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); + + state = sensor_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = sensor_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} + +TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) +{ + hardware_interface::System system_hw(std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_system_robot + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_system = control_resources[0]; + auto state = system_hw.initialize(dummy_system); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + const auto state_interface_offset = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + auto command_interfaces = system_hw.export_command_interfaces(); + state = system_hw.configure(); + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is first time therefore recoverable) + auto emergency_stop = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)) + .second; + auto error_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)) + .second; + auto error_signal_msg = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + auto warning_signal = + test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)) + .second; + auto warning_signal_msg = + test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)) + .second; + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + // activate again and expect reset values + state = system_hw.configure(); + for (auto index = 0ul; index < 6; ++index) + { + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + } + for (auto index = 0ul; index < 3; ++index) + { + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + } + state = system_hw.activate(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); + + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + + // Initiate error on write (this is the second time therefore unrecoverable) + for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) + { + ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); + } + ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); + + state = system_hw.get_state(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); + + // can not change state anymore + state = system_hw.configure(); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); +} From ccfe700440f78795a9d1b7c7c62b5da18a80e387 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 1 Feb 2024 10:54:13 +0100 Subject: [PATCH 24/25] remove rebase artefacts --- .../include/hardware_interface/actuator_interface.hpp | 7 +++---- .../include/hardware_interface/sensor_interface.hpp | 5 ++--- .../include/hardware_interface/system_interface.hpp | 7 +++---- 3 files changed, 8 insertions(+), 11 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1bb07ae29e..67731f67d5 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -482,6 +482,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unordered_map unlisted_command_interfaces_; private: + std::unordered_map> actuator_states_; + std::unordered_map> actuator_commands_; + std::shared_ptr emergency_stop_; std::shared_ptr error_signal_; std::shared_ptr error_signal_message_; @@ -489,10 +492,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::shared_ptr warning_signal_message_; rclcpp_lifecycle::State lifecycle_state_; - -private: - 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 2718028002..47355560f5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -314,15 +314,14 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_state_interfaces_; private: + std::unordered_map> sensor_states_; + std::shared_ptr error_signal_; std::shared_ptr error_signal_message_; std::shared_ptr warning_signal_; std::shared_ptr warning_signal_message_; rclcpp_lifecycle::State lifecycle_state_; - -private: - 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 c7884835f5..3b55435ee3 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -527,6 +527,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_command_interfaces_; private: + std::unordered_map> system_states_; + std::unordered_map> system_commands_; + std::shared_ptr emergency_stop_; std::shared_ptr error_signal_; std::shared_ptr error_signal_message_; @@ -534,10 +537,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::shared_ptr warning_signal_message_; rclcpp_lifecycle::State lifecycle_state_; - -private: - std::unordered_map> system_states_; - std::unordered_map> system_commands_; }; } // namespace hardware_interface From 3d017a657a4abb7e6f564673b0ce6fb22bea144d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Apr 2024 14:40:48 +0200 Subject: [PATCH 25/25] docs for error and warning signals --- doc/index.rst | 1 + .../error_and_warning_interfaces_userdoc.rst | 28 +++++++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 hardware_interface/doc/error_and_warning_interfaces_userdoc.rst diff --git a/doc/index.rst b/doc/index.rst index 91f965d601..2e7e121de6 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -32,4 +32,5 @@ Concepts Controller Manager <../controller_manager/doc/userdoc.rst> Controller Chaining / Cascade Control <../controller_manager/doc/controller_chaining.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> + Hardware Components <../hardware_interface/doc/error_and_warning_interfaces_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> diff --git a/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst b/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst new file mode 100644 index 0000000000..3cbf10e0a7 --- /dev/null +++ b/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst @@ -0,0 +1,28 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/error_and_warning_interfaces_userdoc.rst + +.. _error_and_warning_interfaces_userdoc: + +Error and Warning Interfaces +============================ + +By default we now create the following error and warning interfaces: + ++-----------------+--------------+----------------------------------------------------------------------------------------------------------------------+ +| Type | Datatype | Description | ++=================+====================+================================================================================================================+ +| Emergency Stop | Bool | Used for signaling that hardwares emergency stop is active. Only for Actuator and System. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Error Code | array | Used for sending 32 error codes (uint8_t) at the same time. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Error Message | array | Used for sending 32 error messages where the message at position x corresponds to error code at position x. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Warning Code | array | Used for sending 32 Warning codes (int8_t) at the same time. | ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ +| Warning Message | array | Used for sending 32 warning messages where the message at position x corresponds to warning code at position x.| ++-----------------+--------------------+----------------------------------------------------------------------------------------------------------------+ + +The error and warning interfaces are created as ``StateInterfaces`` and are stored inside the Actuator-, Sensor- or SystemInterface. They can be accessed via getter and setter methods. E.g. if you want to get/set the emergency stop signal you can do so with the ``get_emergency_stop()`` or ``set_emergency_stop(const bool & emergency_stop)`` methods. For the error and warning signals similar getters and setters exist. + +Note: The SensorInterface does not have a Emergency Stop interface.