From d26ea2f2212c88666f111f9dcd9e260a4680ac0e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 7 Dec 2023 09:33:53 +0100 Subject: [PATCH 01/35] 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 85c09e9875eb3f9ed0c99c1adb07f03c63268c40 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 8 Dec 2023 10:46:02 +0100 Subject: [PATCH 02/35] 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 afd8fae1916e279edff2779cd706ac21d0f451df Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 10:20:10 +0100 Subject: [PATCH 03/35] 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 59e9968e78935b5dcca89b0da6346bdb7a48502d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 11 Dec 2023 11:40:37 +0100 Subject: [PATCH 04/35] 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 49d2a60a4366b72e6c7695009bc8607b91312b7a Mon Sep 17 00:00:00 2001 From: "Manuel M." Date: Tue, 12 Dec 2023 13:06:58 +0100 Subject: [PATCH 05/35] 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 9ea64a56e842c5c6ff62fde5a8e637d38c0b1fc7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 09:08:23 +0100 Subject: [PATCH 06/35] 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 55952fb256e9a3208e2cfb5376fea58394cc8758 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 13:36:06 +0100 Subject: [PATCH 07/35] 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 db17e281497e1aa5b6469418924cc44229b38ff4 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 18 Dec 2023 16:45:30 +0100 Subject: [PATCH 08/35] 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 2838fbbbd8e8b257faef2c8fe4a1798b6a1375f1 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 15:49:03 +0100 Subject: [PATCH 09/35] 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 12a910e17fe9768d09ca3abbd78dba87080ea551 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 16:56:27 +0100 Subject: [PATCH 10/35] 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 ec7df0b3a2d0eb067959c18a8da50c2de807bfb4 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 13:06:25 +0100 Subject: [PATCH 11/35] 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 59a0f14d96456f4c79254bd71e4885a59b3d6287 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:16:47 +0100 Subject: [PATCH 12/35] 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 c73d9c7aaaf09e68d9b318bf5cfcfd5eae2229ef Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:17:03 +0100 Subject: [PATCH 13/35] 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 b34c09a2e9ead69a6e649ae520dcff5e257115dd Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 21 Dec 2023 09:04:32 +0100 Subject: [PATCH 14/35] 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 f4fdab7cf76ee129a10e11076ffbcbe0870b548d Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 23 Jan 2024 11:09:26 +0100 Subject: [PATCH 15/35] 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 026f43e0077dc4fd30b5c702b51cd9ff12273ca8 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 09:56:07 +0100 Subject: [PATCH 16/35] 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 6cb2d7cb53d67dd82692074daff1b853102fd669 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 26 Jan 2024 10:17:11 +0100 Subject: [PATCH 17/35] 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 e0341b8ed40973a51501ad3cd656651ed4adb42b Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 09:34:49 +0100 Subject: [PATCH 18/35] 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 c5b8db0e0ec301ec1cdded2943820425dbda274f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 10:51:39 +0100 Subject: [PATCH 19/35] 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 2299d08e66241038c82c74049321667211419aab Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 1 Feb 2024 10:46:13 +0100 Subject: [PATCH 20/35] 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 24e6b6eb4e1fb2a2e5143841df2363193f0b41ae Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Jan 2024 11:52:13 +0100 Subject: [PATCH 21/35] 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 4d4968da1a3c629ae52df24ed4f07305a7c8fe59 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 24 Jan 2024 09:43:53 +0100 Subject: [PATCH 22/35] 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 b54f17662dca43c1dee7b8d5f237db4cde8ad974 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 29 Jan 2024 12:23:49 +0100 Subject: [PATCH 23/35] 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 37aa61edc7903da663b6b78c200bc41a5a360b2c Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 1 Feb 2024 10:54:13 +0100 Subject: [PATCH 24/35] 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 68e0f734739eca4cfb6bc19926e16e0cb00306ba Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 11 Jan 2024 11:52:13 +0100 Subject: [PATCH 25/35] add interface for warning, error and report --- .../test/test_component_interfaces_custom_export.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 695ddc001f..d4f7300ed0 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -42,6 +42,8 @@ // 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 emergency_stop_signal_size = 1; From d9cdb4c95e433a3a2651820e1583e48bb1198c08 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 24 Jan 2024 11:53:23 +0100 Subject: [PATCH 26/35] not finished, showcase for what hase to be changed --- .../test_chainable_controller_interface.cpp | 4 +- .../hardware_interface/actuator_interface.hpp | 49 +++- .../include/hardware_interface/handle.hpp | 40 +-- .../loaned_command_interface.hpp | 2 +- .../loaned_state_interface.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 43 ++- .../hardware_interface/system_interface.hpp | 50 +++- .../hardware_interface_error_signals.hpp | 4 + .../hardware_interface_warning_signals.hpp | 4 + .../test/test_component_interfaces.cpp | 184 ++++++------ ...est_component_interfaces_custom_export.cpp | 2 +- .../test/test_error_warning_codes.cpp | 262 ++++++++++-------- hardware_interface/test/test_handle.cpp | 12 +- .../differential_transmission.hpp | 31 ++- .../four_bar_linkage_transmission.hpp | 28 +- .../simple_transmission.hpp | 12 +- 16 files changed, 429 insertions(+), 300 deletions(-) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 47487e019c..33959e7fd9 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -48,7 +48,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) @@ -103,7 +103,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 67731f67d5..dc04c79cd0 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -431,7 +431,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod double get_state(const std::string & interface_name) const { - return actuator_states_.at(interface_name)->get_value(); + return actuator_states_.at(interface_name)->get_value(); } void set_command(const std::string & interface_name, const double & value) @@ -441,37 +441,60 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod double get_command(const std::string & interface_name) const { - return actuator_commands_.at(interface_name)->get_value(); + return actuator_commands_.at(interface_name)->get_value(); } - void set_emergency_stop(const double & emergency_stop) + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); } - double get_emergency_stop() const { return emergency_stop_->get_value(); } + bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::array error_codes) + { + error_signal_->set_value(error_codes); + } - double get_error_code() const { return error_signal_->get_value(); } + std::array get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message( + std::array error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::array get_error_message() const + { + return error_signal_message_ + ->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::array warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::array get_warning_code() const + { + return warning_signal_ + ->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message( + std::array error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::array get_warning_message() const + { + return warning_signal_message_ + ->get_value>(); + } protected: HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 16d06c578d..b10413bba9 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -30,7 +31,19 @@ namespace hardware_interface { -typedef std::variant HANDLE_DATATYPE; +using HANDLE_DATATYPE = std::variant< + bool, double, hardware_interface::WARNING_SIGNALS, hardware_interface::ERROR_SIGNALS, + hardware_interface::WARNING_MESSAGES>; + +// Define a type trait for allowed types +template +struct HANDLE_DATATYPE_TYPES : std::disjunction< + std::is_same, std::is_same, + std::is_same, + std::is_same, + std::is_same> +{ +}; /// A handle used to get and set a value on a given interface. class Handle @@ -49,10 +62,10 @@ class Handle : prefix_name_(interface_description.prefix_name), interface_name_(interface_description.interface_info.name) { + // TODO(Manuel): implement this. // 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_); + // value_ = std::numeric_limits::quiet_NaN(); } [[deprecated("Use InterfaceDescription for initializing the Interface")]] @@ -95,32 +108,23 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } - double get_value() const + template ::value, int>::type = 0> + T get_value() const { - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) return value_ if old functionality is removed - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; - // END + return std::get(value_); } - void set_value(double value) + template ::value, int>::type = 0> + void set_value(T value) { - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) set value_ directly if old functionality is removed - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; - // END + value_ = value; } protected: std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; - // BEGIN (Handle export change): for backward compatibility - // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; - // END }; class StateInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..6fc445ac29 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -65,7 +65,7 @@ class LoanedCommandInterface void set_value(double val) { command_interface_.set_value(val); } - double get_value() const { return command_interface_.get_value(); } + double get_value() const { return command_interface_.get_value(); } protected: CommandInterface & command_interface_; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..11abae41d0 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -63,7 +63,7 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const { return state_interface_.get_value(); } protected: StateInterface & state_interface_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 47355560f5..bf014b0d09 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -282,30 +282,53 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_state(const std::string & interface_name) const { - return sensor_states_.at(interface_name)->get_value(); + return sensor_states_.at(interface_name)->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::array error_codes) + { + error_signal_->set_value(error_codes); + } - double get_error_code() const { return error_signal_->get_value(); } + std::array get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message( + std::array error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::array get_error_message() const + { + return error_signal_message_ + ->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::array warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::array get_warning_code() const + { + return warning_signal_ + ->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message( + std::array error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::array get_warning_message() const + { + return warning_signal_message_ + ->get_value>(); + } protected: HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 3b55435ee3..fd33af006a 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -463,7 +463,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return state. */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } - void set_state(const std::string & interface_name, const double & value) { system_states_.at(interface_name)->set_value(value); @@ -471,7 +470,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_state(const std::string & interface_name) const { - return system_states_.at(interface_name)->get_value(); + return system_states_.at(interface_name)->get_value(); } void set_command(const std::string & interface_name, const double & value) @@ -481,37 +480,60 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI double get_command(const std::string & interface_name) const { - return system_commands_.at(interface_name)->get_value(); + return system_commands_.at(interface_name)->get_value(); } - void set_emergency_stop(const double & emergency_stop) + void set_emergency_stop(const bool & emergency_stop) { emergency_stop_->set_value(emergency_stop); } - double get_emergency_stop() const { return emergency_stop_->get_value(); } + bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(const double & error_code) { error_signal_->set_value(error_code); } + void set_error_code(std::array error_codes) + { + error_signal_->set_value(error_codes); + } - double get_error_code() const { return error_signal_->get_value(); } + std::array get_error_code() const + { + return error_signal_->get_value>(); + } - void set_error_message(const double & error_message) + void set_error_message( + std::array error_messages) { - error_signal_message_->set_value(error_message); + error_signal_message_->set_value(error_messages); } - double get_error_message() const { return error_signal_message_->get_value(); } + std::array get_error_message() const + { + return error_signal_message_ + ->get_value>(); + } - void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); } + void set_warning_code(std::array warning_codes) + { + warning_signal_->set_value(warning_codes); + } - double get_warning_code() const { return warning_signal_->get_value(); } + std::array get_warning_code() const + { + return warning_signal_ + ->get_value>(); + } - void set_warning_message(const double & error_message) + void set_warning_message( + std::array error_message) { warning_signal_message_->set_value(error_message); } - double get_warning_message() const { return warning_signal_message_->get_value(); } + std::array get_warning_message() const + { + return warning_signal_message_ + ->get_value>(); + } protected: HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp index d5159f29c8..88848a7ca8 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -16,6 +16,7 @@ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ #include +#include #include namespace hardware_interface @@ -23,6 +24,7 @@ namespace hardware_interface // Count of how many different error signals there are that can be reported. const size_t error_signal_count = 32; +using ERROR_SIGNALS = std::array; constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; // Available error signal names @@ -62,6 +64,8 @@ enum class error_signal : std::uint8_t ERROR_SIGNAL_31 = 31 }; +using ERROR_MESSAGES = std::array; + constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; // Available WARNING signal message names enum class error_signal_message : std::uint8_t diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp index 58af04b80a..b9ebcfdeb3 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -16,12 +16,14 @@ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ #include +#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; +using WARNING_SIGNALS = std::array; constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; // Available warning signals names mapping to position in the interface @@ -61,6 +63,8 @@ enum class warning_signal : std::uint8_t WARNING_SIGNAL_31 = 31 }; +using WARNING_MESSAGES = std::array; + constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; // Available WARNING signal message names enum class warning_signal_message : std::uint8_t diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 3362d1eb53..e836ed3427 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -718,8 +718,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)); } @@ -733,8 +733,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)); } @@ -748,8 +748,9 @@ 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)); } @@ -763,8 +764,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)); } @@ -835,8 +836,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -851,8 +852,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[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -868,8 +869,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -883,8 +884,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[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -910,21 +911,21 @@ TEST(TestComponentInterfaces, dummy_sensor) 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_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 @@ -952,23 +953,23 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } // Not updated because is is UNCONFIGURED auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[si_joint1_vol]->get_value())); // Updated because is is INACTIVE state = sensor_hw.configure(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0.0, state_interfaces[si_joint1_vol]->get_value()); // It can read now sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); + EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); } // BEGIN (Handle export change): for backward compatibility @@ -1025,12 +1026,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)); } @@ -1044,12 +1045,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)); } @@ -1063,12 +1064,15 @@ 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)); } @@ -1082,12 +1086,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)); } @@ -1216,12 +1220,12 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint2_vel]->get_value())); // velocity + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_pos]->get_value())); // position value + ASSERT_TRUE(std::isnan(state_interfaces[si_joint3_vel]->get_value())); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1236,14 +1240,14 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(step ? velocity_value : 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 + 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 + 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)); } @@ -1259,16 +1263,16 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint2_vel]->get_value()); // velocity EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1282,12 +1286,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[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1360,8 +1364,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()); @@ -1431,8 +1435,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1491,8 +1495,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()); @@ -1561,8 +1565,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1624,7 +1628,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()); @@ -1690,7 +1694,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) // activate again and expect reset values auto si_joint1_vol = test_components::vector_contains(state_interfaces, "joint1/voltage").second; state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_vol]->get_value(), 0.0); state = sensor_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1748,11 +1752,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()); @@ -1819,11 +1823,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1884,11 +1888,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()); @@ -1955,11 +1959,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index d4f7300ed0..526f12ae5e 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -255,7 +255,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } { auto [contains, position] = diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index 06e13a9fa3..d04bbcbab3 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -78,11 +78,11 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { set_state("joint1/position", 0.0); set_state("joint1/velocity", 0.0); - set_emergency_stop(0.0); - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_emergency_stop(false); + set_error_code(hardware_interface::ERROR_SIGNALS{}); + set_error_message(hardware_interface::WARNING_MESSAGES{}); + set_warning_code(hardware_interface::WARNING_SIGNALS{}); + set_warning_message(hardware_interface::WARNING_MESSAGES{}); if (recoverable_error_happened_) { @@ -103,11 +103,17 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{ + {"Some", "error", "message that is split"}}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + {"Some", "warning", "message that is split"}}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -121,11 +127,17 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface ++write_calls_; if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{ + {"Some", "error", "message that is split"}}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + {"Some", "warning", "message that is split"}}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } auto position_state = get_state("joint1/position"); @@ -183,10 +195,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface { set_state(name, 0.0); } - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_error_code(hardware_interface::ERROR_SIGNALS{}); + set_error_message(hardware_interface::WARNING_MESSAGES{}); + set_warning_code(hardware_interface::WARNING_SIGNALS{}); + set_warning_message(hardware_interface::WARNING_MESSAGES{}); read_calls_ = 0; return CallbackReturn::SUCCESS; } @@ -199,10 +211,16 @@ class DummySensorDefault : public hardware_interface::SensorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{ + {"Some", "error", "message that is split"}}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + {"Some", "warning", "message that is split"}}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -254,11 +272,11 @@ class DummySystemDefault : public hardware_interface::SystemInterface set_state(position_states_[i], 0.0); set_state(velocity_states_[i], 0.0); } - set_emergency_stop(0.0); - set_error_code(0.0); - set_error_message(0.0); - set_warning_code(0.0); - set_warning_message(0.0); + set_emergency_stop(false); + set_error_code(hardware_interface::ERROR_SIGNALS{}); + set_error_message(hardware_interface::WARNING_MESSAGES{}); + set_warning_code(hardware_interface::WARNING_SIGNALS{}); + set_warning_message(hardware_interface::WARNING_MESSAGES{}); // reset command only if error is initiated if (recoverable_error_happened_) { @@ -282,11 +300,17 @@ class DummySystemDefault : public hardware_interface::SystemInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{ + {"Some", "error", "message that is split"}}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + {"Some", "warning", "message that is split"}}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -300,11 +324,17 @@ class DummySystemDefault : public hardware_interface::SystemInterface ++write_calls_; if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - set_emergency_stop(1.0); - set_error_code(1.0); - set_error_message(1.0); - set_warning_code(1.0); - set_warning_message(1.0); + set_emergency_stop(true); + hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; + set_error_code(error_signals); + hardware_interface::WARNING_MESSAGES error_messages{ + {"Some", "error", "message that is split"}}; + set_error_message(error_messages); + hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; + set_warning_code(warning_signals); + hardware_interface::WARNING_MESSAGES warning_messages{ + {"Some", "warning", "message that is split"}}; + set_warning_message(warning_messages); return hardware_interface::return_type::ERROR; } @@ -501,7 +531,7 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ("joint1/voltage", state_interfaces[position]->get_name()); EXPECT_EQ("voltage", state_interfaces[position]->get_interface_name()); EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); + EXPECT_TRUE(std::isnan(state_interfaces[position]->get_value())); } // ERROR_SIGNAL { @@ -783,18 +813,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 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); + 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()); @@ -805,8 +835,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) auto ci_joint1_vel = test_components::vector_contains(command_interfaces, "joint1/velocity").second; state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[si_joint1_pos]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[ci_joint1_vel]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -819,18 +849,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 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); + 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()); @@ -882,8 +912,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) // activate again and expect reset values state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); state = actuator_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -921,18 +951,18 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 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); + 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()); @@ -991,16 +1021,16 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value(), 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); + 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()); @@ -1008,7 +1038,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()); @@ -1086,18 +1116,18 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 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); + 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()); @@ -1107,11 +1137,11 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); @@ -1124,18 +1154,18 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 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); + 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()); @@ -1202,18 +1232,18 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 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); + 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()); @@ -1223,11 +1253,11 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) state = system_hw.configure(); for (auto index = 0ul; index < 6; ++index) { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); } for (auto index = 0ul; index < 3; ++index) { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); + EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); } state = system_hw.activate(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 7d79c032f0..aea7bbb04c 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -31,16 +31,16 @@ TEST(TestHandle, command_interface) { double value = 1.337; CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + EXPECT_DOUBLE_EQ(interface.get_value(), value); EXPECT_NO_THROW(interface.set_value(0.0)); - EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); } TEST(TestHandle, state_interface) { double value = 1.337; StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + EXPECT_DOUBLE_EQ(interface.get_value(), value); // interface.set_value(5); compiler error, no set_value function } @@ -55,7 +55,7 @@ TEST(TestHandle, name_getters_work) TEST(TestHandle, value_methods_throw_for_nullptr) { CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); + EXPECT_ANY_THROW(handle.get_value()); EXPECT_ANY_THROW(handle.set_value(0.0)); } @@ -63,9 +63,9 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) { double value = 1.337; CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); + EXPECT_DOUBLE_EQ(handle.get_value(), value); EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); + EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 2002f6f9d7..53a084fe76 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -264,10 +264,12 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); joint_pos[0].set_value( - (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + + (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / + (2.0 * jr[0]) + joint_offset_[0]); joint_pos[1].set_value( - (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) + + (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / + (2.0 * jr[1]) + joint_offset_[1]); } @@ -278,9 +280,11 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); joint_vel[0].set_value( - (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0])); + (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / + (2.0 * jr[0])); joint_vel[1].set_value( - (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / (2.0 * jr[1])); + (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / + (2.0 * jr[1])); } auto & act_eff = actuator_effort_; @@ -290,9 +294,9 @@ inline void DifferentialTransmission::actuator_to_joint() assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); joint_eff[0].set_value( - jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); + jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); joint_eff[1].set_value( - jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); + jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1])); } } @@ -308,7 +312,8 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; + joint_pos[0].get_value() - joint_offset_[0], + joint_pos[1].get_value() - joint_offset_[1]}; act_pos[0].set_value( (joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1]) * ar[0]); act_pos[1].set_value( @@ -322,9 +327,11 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); act_vel[0].set_value( - (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]); + (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * + ar[0]); act_vel[1].set_value( - (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * ar[1]); + (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * + ar[1]); } auto & act_eff = actuator_effort_; @@ -334,9 +341,11 @@ inline void DifferentialTransmission::joint_to_actuator() assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); act_eff[0].set_value( - (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0])); + (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / + (2.0 * ar[0])); act_eff[1].set_value( - (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1])); + (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / + (2.0 * ar[1])); } } diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 39f5df6f61..8b5fdd5433 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -261,9 +261,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); + joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); joint_pos[1].set_value( - (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / jr[1] + + (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / + jr[1] + joint_offset_[1]); } @@ -274,9 +275,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); + joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); joint_vel[1].set_value( - (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / jr[1]); + (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / + jr[1]); } // effort @@ -286,9 +288,10 @@ inline void FourBarLinkageTransmission::actuator_to_joint() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); + joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); + jr[1] * + (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); } } @@ -305,7 +308,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); double joints_offset_applied[2] = { - joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; + joint_pos[0].get_value() - joint_offset_[0], + joint_pos[1].get_value() - joint_offset_[1]}; act_pos[0].set_value(joints_offset_applied[0] * jr[0] * ar[0]); act_pos[1].set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]); } @@ -317,8 +321,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); - act_vel[1].set_value((joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); + act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); + act_vel[1].set_value( + (joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); } // effort @@ -328,8 +333,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator() { assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); - act_eff[1].set_value((joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); + act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); + act_eff[1].set_value( + (joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); } } diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 809472ab75..b240c1e489 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -218,17 +218,17 @@ inline void SimpleTransmission::actuator_to_joint() { if (joint_effort_ && actuator_effort_) { - joint_effort_.set_value(actuator_effort_.get_value() * reduction_); + joint_effort_.set_value(actuator_effort_.get_value() * reduction_); } if (joint_velocity_ && actuator_velocity_) { - joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); + joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); } if (joint_position_ && actuator_position_) { - joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); + joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); } } @@ -236,17 +236,17 @@ inline void SimpleTransmission::joint_to_actuator() { if (joint_effort_ && actuator_effort_) { - actuator_effort_.set_value(joint_effort_.get_value() / reduction_); + actuator_effort_.set_value(joint_effort_.get_value() / reduction_); } if (joint_velocity_ && actuator_velocity_) { - actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); + actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); } if (joint_position_ && actuator_position_) { - actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); + actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); } } From 0e24e3af12fefbeb9627f53dd2d4e4d32f77dd49 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 31 Jan 2024 15:52:22 +0100 Subject: [PATCH 27/35] add parsing of handle type for initialization of variant and adjust most tests --- .../hardware_interface/actuator_interface.hpp | 44 +- .../include/hardware_interface/handle.hpp | 105 +- .../hardware_interface/hardware_info.hpp | 11 + .../hardware_interface/sensor_interface.hpp | 44 +- .../hardware_interface/system_interface.hpp | 44 +- .../types/handle_datatype.hpp | 43 + .../hardware_interface_error_signals.hpp | 77 +- .../hardware_interface_warning_signals.hpp | 77 +- .../test/test_component_interfaces.cpp | 1008 ++--------------- .../test/test_error_warning_codes.cpp | 297 ++--- hardware_interface/test/test_handle.cpp | 535 ++++++++- 11 files changed, 951 insertions(+), 1334 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/handle_datatype.hpp diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index dc04c79cd0..47281f659b 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -159,13 +159,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -173,13 +175,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -451,49 +455,41 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(std::array error_codes) - { - error_signal_->set_value(error_codes); - } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - std::array get_error_code() const + std::vector get_error_code() const { - return error_signal_->get_value>(); + return error_signal_->get_value>(); } - void set_error_message( - std::array error_messages) + void set_error_message(std::vector error_messages) { error_signal_message_->set_value(error_messages); } - std::array get_error_message() const + std::vector get_error_message() const { - return error_signal_message_ - ->get_value>(); + return error_signal_message_->get_value>(); } - void set_warning_code(std::array warning_codes) + void set_warning_code(std::vector warning_codes) { warning_signal_->set_value(warning_codes); } - std::array get_warning_code() const + std::vector get_warning_code() const { - return warning_signal_ - ->get_value>(); + return warning_signal_->get_value>(); } - void set_warning_message( - std::array error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - std::array get_warning_message() const + std::vector get_warning_message() const { - return warning_signal_message_ - ->get_value>(); + return warning_signal_message_->get_value>(); } protected: diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index b10413bba9..31b459177a 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,35 +15,20 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ -#include #include #include #include #include #include #include +#include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" -#include "hardware_interface/types/hardware_interface_error_signals.hpp" -#include "hardware_interface/types/hardware_interface_warning_signals.hpp" -#include "hardware_interface/visibility_control.h" -namespace hardware_interface -{ +#include "hardware_interface/types/handle_datatype.hpp" -using HANDLE_DATATYPE = std::variant< - bool, double, hardware_interface::WARNING_SIGNALS, hardware_interface::ERROR_SIGNALS, - hardware_interface::WARNING_MESSAGES>; - -// Define a type trait for allowed types -template -struct HANDLE_DATATYPE_TYPES : std::disjunction< - std::is_same, std::is_same, - std::is_same, - std::is_same, - std::is_same> +namespace hardware_interface { -}; /// A handle used to get and set a value on a given interface. class Handle @@ -56,16 +41,15 @@ class Handle double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { + // default to double + value_ = std::numeric_limits::quiet_NaN(); } explicit Handle(const InterfaceDescription & interface_description) : prefix_name_(interface_description.prefix_name), interface_name_(interface_description.interface_info.name) { - // TODO(Manuel): implement this. - // 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(); + init_handle_value(interface_description.interface_info); } [[deprecated("Use InterfaceDescription for initializing the Interface")]] @@ -73,13 +57,16 @@ class Handle explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { + // default to double + value_ = std::numeric_limits::quiet_NaN(); } [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) - { + { // default to double + value_ = std::numeric_limits::quiet_NaN(); } Handle(const Handle & other) = default; @@ -121,6 +108,78 @@ class Handle } protected: + // used for the + bool correct_vector_size(const size_t & expected, const size_t & actual) + { + return expected == actual; + } + + void init_handle_value(const InterfaceInfo & interface_info) + { + if (interface_info.data_type == "bool") + { + value_ = interface_info.initial_value.empty() ? false + : (interface_info.initial_value == "true" || + interface_info.initial_value == "True"); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + value_ = std::vector(hardware_interface::warning_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if (interface_info.size != 0 && hardware_interface::error_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::error_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::error_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::warning_signal_count, ""); + } + // Default for empty is double + else if (interface_info.data_type.empty() || interface_info.data_type == "double") + { + value_ = interface_info.initial_value.empty() ? std::numeric_limits::quiet_NaN() + : std::stod(interface_info.initial_value); + } + // If not empty and it belongs to none of the above types, we still want to throw as there might + // be a typo in the data_type like "bol" or user wants some unsupported type + else + { + throw std::runtime_error( + "The data_type:{" + interface_info.data_type + "} for the InterfaceInfo with name:{" + + interface_info.name + + "} is not supported for Handles. Supported data_types are: bool, double, vector, " + "vector and vector."); + } + } + std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 6322fdc17e..dd757ffaeb 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -27,6 +27,17 @@ namespace hardware_interface */ struct InterfaceInfo { + // Add default constructor, so that e.g. size is initialized to sensible value + InterfaceInfo() + { + // cpp_lint complains about min and max include otherwise + name = ""; + min = ""; + max = ""; + initial_value = ""; + data_type = ""; + size = 0; + } /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index bf014b0d09..545548104e 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -136,13 +136,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -150,13 +152,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -285,49 +289,41 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return sensor_states_.at(interface_name)->get_value(); } - void set_error_code(std::array error_codes) - { - error_signal_->set_value(error_codes); - } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - std::array get_error_code() const + std::vector get_error_code() const { - return error_signal_->get_value>(); + return error_signal_->get_value>(); } - void set_error_message( - std::array error_messages) + void set_error_message(std::vector error_messages) { error_signal_message_->set_value(error_messages); } - std::array get_error_message() const + std::vector get_error_message() const { - return error_signal_message_ - ->get_value>(); + return error_signal_message_->get_value>(); } - void set_warning_code(std::array warning_codes) + void set_warning_code(std::vector warning_codes) { warning_signal_->set_value(warning_codes); } - std::array get_warning_code() const + std::vector get_warning_code() const { - return warning_signal_ - ->get_value>(); + return warning_signal_->get_value>(); } - void set_warning_message( - std::array error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - std::array get_warning_message() const + std::vector get_warning_message() const { - return warning_signal_message_ - ->get_value>(); + return warning_signal_message_->get_value>(); } protected: diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index fd33af006a..399e59f3fb 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -179,13 +179,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create error signal interface InterfaceInfo error_interface_info; error_interface_info.name = hardware_interface::ERROR_SIGNAL_INTERFACE_NAME; - error_interface_info.data_type = "array[32]"; + error_interface_info.data_type = "vector"; + error_interface_info.size = 32; InterfaceDescription error_interface_descr(info_.name, error_interface_info); error_signal_ = std::make_shared(error_interface_descr); // create error signal report message interface InterfaceInfo error_msg_interface_info; error_msg_interface_info.name = hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME; - error_msg_interface_info.data_type = "array[32]"; + error_msg_interface_info.data_type = "vector"; + error_msg_interface_info.size = 32; InterfaceDescription error_msg_interface_descr(info_.name, error_msg_interface_info); error_signal_message_ = std::make_shared(error_msg_interface_descr); @@ -193,13 +195,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // create warning signal interface InterfaceInfo warning_interface_info; warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; - warning_interface_info.data_type = "array[32]"; + warning_interface_info.data_type = "vector"; + warning_interface_info.size = 32; InterfaceDescription warning_interface_descr(info_.name, warning_interface_info); warning_signal_ = std::make_shared(warning_interface_descr); // create warning signal report message interface InterfaceInfo warning_msg_interface_info; warning_msg_interface_info.name = hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME; - warning_msg_interface_info.data_type = "array[32]"; + warning_msg_interface_info.data_type = "vector"; + warning_msg_interface_info.size = 32; InterfaceDescription warning_msg_interface_descr(info_.name, warning_msg_interface_info); warning_signal_message_ = std::make_shared(warning_msg_interface_descr); } @@ -490,49 +494,41 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI bool get_emergency_stop() const { return emergency_stop_->get_value(); } - void set_error_code(std::array error_codes) - { - error_signal_->set_value(error_codes); - } + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } - std::array get_error_code() const + std::vector get_error_code() const { - return error_signal_->get_value>(); + return error_signal_->get_value>(); } - void set_error_message( - std::array error_messages) + void set_error_message(std::vector error_messages) { error_signal_message_->set_value(error_messages); } - std::array get_error_message() const + std::vector get_error_message() const { - return error_signal_message_ - ->get_value>(); + return error_signal_message_->get_value>(); } - void set_warning_code(std::array warning_codes) + void set_warning_code(std::vector warning_codes) { warning_signal_->set_value(warning_codes); } - std::array get_warning_code() const + std::vector get_warning_code() const { - return warning_signal_ - ->get_value>(); + return warning_signal_->get_value>(); } - void set_warning_message( - std::array error_message) + void set_warning_message(std::vector error_message) { warning_signal_message_->set_value(error_message); } - std::array get_warning_message() const + std::vector get_warning_message() const { - return warning_signal_message_ - ->get_value>(); + return warning_signal_message_->get_value>(); } protected: diff --git a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp new file mode 100644 index 0000000000..6a4d32c3c1 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ +#define HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" + +namespace hardware_interface +{ + +using HANDLE_DATATYPE = + std::variant, std::vector, std::vector>; + +// Define a type trait for allowed types +template +struct HANDLE_DATATYPE_TYPES +: std::disjunction< + std::is_same, std::is_same, std::is_same>, + std::is_same>, std::is_same>> +{ +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp index 88848a7ca8..b40011eb3a 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -24,85 +24,12 @@ namespace hardware_interface // Count of how many different error signals there are that can be reported. const size_t error_signal_count = 32; -using ERROR_SIGNALS = std::array; +using ERROR_SIGNALS = std::vector; constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; -// Available error signal names -enum class error_signal : std::uint8_t -{ - ERROR_SIGNAL_0 = 0, - ERROR_SIGNAL_1 = 1, - ERROR_SIGNAL_2 = 2, - ERROR_SIGNAL_3 = 3, - ERROR_SIGNAL_4 = 4, - ERROR_SIGNAL_5 = 5, - ERROR_SIGNAL_6 = 6, - ERROR_SIGNAL_7 = 7, - ERROR_SIGNAL_8 = 8, - ERROR_SIGNAL_9 = 9, - ERROR_SIGNAL_10 = 10, - ERROR_SIGNAL_11 = 11, - ERROR_SIGNAL_12 = 12, - ERROR_SIGNAL_13 = 13, - ERROR_SIGNAL_14 = 14, - ERROR_SIGNAL_15 = 15, - ERROR_SIGNAL_16 = 16, - ERROR_SIGNAL_17 = 17, - ERROR_SIGNAL_18 = 18, - ERROR_SIGNAL_19 = 19, - ERROR_SIGNAL_20 = 20, - ERROR_SIGNAL_21 = 21, - ERROR_SIGNAL_22 = 22, - ERROR_SIGNAL_23 = 23, - ERROR_SIGNAL_24 = 24, - ERROR_SIGNAL_25 = 25, - ERROR_SIGNAL_26 = 26, - ERROR_SIGNAL_27 = 27, - ERROR_SIGNAL_28 = 28, - ERROR_SIGNAL_29 = 29, - ERROR_SIGNAL_30 = 30, - ERROR_SIGNAL_31 = 31 -}; - -using ERROR_MESSAGES = std::array; +using ERROR_MESSAGES = std::vector; constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; -// Available WARNING signal message names -enum class error_signal_message : std::uint8_t -{ - ERROR_SIGNAL_MESSAGE_0 = 0, - ERROR_SIGNAL_MESSAGE_1 = 1, - ERROR_SIGNAL_MESSAGE_2 = 2, - ERROR_SIGNAL_MESSAGE_3 = 3, - ERROR_SIGNAL_MESSAGE_4 = 4, - ERROR_SIGNAL_MESSAGE_5 = 5, - ERROR_SIGNAL_MESSAGE_6 = 6, - ERROR_SIGNAL_MESSAGE_7 = 7, - ERROR_SIGNAL_MESSAGE_8 = 8, - ERROR_SIGNAL_MESSAGE_9 = 9, - ERROR_SIGNAL_MESSAGE_10 = 10, - ERROR_SIGNAL_MESSAGE_11 = 11, - ERROR_SIGNAL_MESSAGE_12 = 12, - ERROR_SIGNAL_MESSAGE_13 = 13, - ERROR_SIGNAL_MESSAGE_14 = 14, - ERROR_SIGNAL_MESSAGE_15 = 15, - ERROR_SIGNAL_MESSAGE_16 = 16, - ERROR_SIGNAL_MESSAGE_17 = 17, - ERROR_SIGNAL_MESSAGE_18 = 18, - ERROR_SIGNAL_MESSAGE_19 = 19, - ERROR_SIGNAL_MESSAGE_20 = 20, - ERROR_SIGNAL_MESSAGE_21 = 21, - ERROR_SIGNAL_MESSAGE_22 = 22, - ERROR_SIGNAL_MESSAGE_23 = 23, - ERROR_SIGNAL_MESSAGE_24 = 24, - ERROR_SIGNAL_MESSAGE_25 = 25, - ERROR_SIGNAL_MESSAGE_26 = 26, - ERROR_SIGNAL_MESSAGE_27 = 27, - ERROR_SIGNAL_MESSAGE_28 = 28, - ERROR_SIGNAL_MESSAGE_29 = 29, - ERROR_SIGNAL_MESSAGE_30 = 30, - ERROR_SIGNAL_MESSAGE_31 = 31 -}; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp index b9ebcfdeb3..cf4b5a8c0e 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -23,85 +23,12 @@ namespace hardware_interface { // Count of how many different warn signals there are that can be reported. const size_t warning_signal_count = 32; -using WARNING_SIGNALS = std::array; +using WARNING_SIGNALS = std::vector; constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; -// Available warning signals names mapping to position in the interface -enum class warning_signal : std::uint8_t -{ - WARNING_SIGNAL_0 = 0, - WARNING_SIGNAL_1 = 1, - WARNING_SIGNAL_2 = 2, - WARNING_SIGNAL_3 = 3, - WARNING_SIGNAL_4 = 4, - WARNING_SIGNAL_5 = 5, - WARNING_SIGNAL_6 = 6, - WARNING_SIGNAL_7 = 7, - WARNING_SIGNAL_8 = 8, - WARNING_SIGNAL_9 = 9, - WARNING_SIGNAL_10 = 10, - WARNING_SIGNAL_11 = 11, - WARNING_SIGNAL_12 = 12, - WARNING_SIGNAL_13 = 13, - WARNING_SIGNAL_14 = 14, - WARNING_SIGNAL_15 = 15, - WARNING_SIGNAL_16 = 16, - WARNING_SIGNAL_17 = 17, - WARNING_SIGNAL_18 = 18, - WARNING_SIGNAL_19 = 19, - WARNING_SIGNAL_20 = 20, - WARNING_SIGNAL_21 = 21, - WARNING_SIGNAL_22 = 22, - WARNING_SIGNAL_23 = 23, - WARNING_SIGNAL_24 = 24, - WARNING_SIGNAL_25 = 25, - WARNING_SIGNAL_26 = 26, - WARNING_SIGNAL_27 = 27, - WARNING_SIGNAL_28 = 28, - WARNING_SIGNAL_29 = 29, - WARNING_SIGNAL_30 = 30, - WARNING_SIGNAL_31 = 31 -}; - -using WARNING_MESSAGES = std::array; +using WARNING_MESSAGES = std::vector; constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; -// Available WARNING signal message names -enum class warning_signal_message : std::uint8_t -{ - WARNING_SIGNAL_MESSAGE_0 = 0, - WARNING_SIGNAL_MESSAGE_1 = 1, - WARNING_SIGNAL_MESSAGE_2 = 2, - WARNING_SIGNAL_MESSAGE_3 = 3, - WARNING_SIGNAL_MESSAGE_4 = 4, - WARNING_SIGNAL_MESSAGE_5 = 5, - WARNING_SIGNAL_MESSAGE_6 = 6, - WARNING_SIGNAL_MESSAGE_7 = 7, - WARNING_SIGNAL_MESSAGE_8 = 8, - WARNING_SIGNAL_MESSAGE_9 = 9, - WARNING_SIGNAL_MESSAGE_10 = 10, - WARNING_SIGNAL_MESSAGE_11 = 11, - WARNING_SIGNAL_MESSAGE_12 = 12, - WARNING_SIGNAL_MESSAGE_13 = 13, - WARNING_SIGNAL_MESSAGE_14 = 14, - WARNING_SIGNAL_MESSAGE_15 = 15, - WARNING_SIGNAL_MESSAGE_16 = 16, - WARNING_SIGNAL_MESSAGE_17 = 17, - WARNING_SIGNAL_MESSAGE_18 = 18, - WARNING_SIGNAL_MESSAGE_19 = 19, - WARNING_SIGNAL_MESSAGE_20 = 20, - WARNING_SIGNAL_MESSAGE_21 = 21, - WARNING_SIGNAL_MESSAGE_22 = 22, - WARNING_SIGNAL_MESSAGE_23 = 23, - WARNING_SIGNAL_MESSAGE_24 = 24, - WARNING_SIGNAL_MESSAGE_25 = 25, - WARNING_SIGNAL_MESSAGE_26 = 26, - WARNING_SIGNAL_MESSAGE_27 = 27, - WARNING_SIGNAL_MESSAGE_28 = 28, - WARNING_SIGNAL_MESSAGE_29 = 29, - WARNING_SIGNAL_MESSAGE_30 = 30, - WARNING_SIGNAL_MESSAGE_31 = 31 -}; } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index e836ed3427..f995d5d4d2 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -58,115 +58,6 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -// BEGIN (Handle export change): for backward compatibility -class DummyActuator : public hardware_interface::ActuatorInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - position_state_ = 0.0; - velocity_state_ = 0.0; - - if (recoverable_error_happened_) - { - velocity_command_ = 0.0; - } - - read_calls_ = 0; - write_calls_ = 0; - - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read a position and a velocity - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_)); - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - // We can command in velocity - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_)); - - return command_interfaces; - } - - std::string get_name() const override { return "DummyActuator"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, state is getting propagated within write. - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++write_calls_; - if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - position_state_ += velocity_command_; - velocity_state_ = velocity_command_; - - return hardware_interface::return_type::OK; - } - - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override - { - velocity_state_ = 0; - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - double position_state_ = std::numeric_limits::quiet_NaN(); - double velocity_state_ = std::numeric_limits::quiet_NaN(); - double velocity_command_ = 0.0; - - // Helper variables to initiate error on read - unsigned int read_calls_ = 0; - unsigned int write_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummyActuatorDefault : public hardware_interface::ActuatorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -254,72 +145,6 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface bool recoverable_error_happened_ = false; }; -// BEGIN (Handle export change): for backward compatibility -class DummySensor : public hardware_interface::SensorInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - voltage_level_ = 0.0; - read_calls_ = 0; - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read some voltage level - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface("joint1", "voltage", &voltage_level_)); - - return state_interfaces; - } - - std::string get_name() const override { return "DummySensor"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, static value - voltage_level_ = voltage_level_hw_value_; - return hardware_interface::return_type::OK; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - double voltage_level_ = std::numeric_limits::quiet_NaN(); - double voltage_level_hw_value_ = 0x666; - - // Helper variables to initiate error on read - int read_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummySensorDefault : public hardware_interface::SensorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -382,143 +207,6 @@ class DummySensorDefault : public hardware_interface::SensorInterface int read_calls_ = 0; bool recoverable_error_happened_ = false; }; - -// BEGIN (Handle export change): for backward compatibility -class DummySystem : public hardware_interface::SystemInterface -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & /* info */) override - { - // We hardcode the info - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override - { - for (auto i = 0ul; i < 3; ++i) - { - position_state_[i] = 0.0; - velocity_state_[i] = 0.0; - } - // reset command only if error is initiated - if (recoverable_error_happened_) - { - for (auto i = 0ul; i < 3; ++i) - { - velocity_command_[i] = 0.0; - } - } - - read_calls_ = 0; - write_calls_ = 0; - - return CallbackReturn::SUCCESS; - } - - std::vector export_state_interfaces() override - { - // We can read a position and a velocity - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_POSITION, &position_state_[0])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_[0])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_POSITION, &position_state_[1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_state_[1])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_POSITION, &position_state_[2])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_state_[2])); - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - // We can command in velocity - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_[0])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint2", hardware_interface::HW_IF_VELOCITY, &velocity_command_[1])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "joint3", hardware_interface::HW_IF_VELOCITY, &velocity_command_[2])); - - return command_interfaces; - } - - std::string get_name() const override { return "DummySystem"; } - - hardware_interface::return_type read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++read_calls_; - if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - // no-op, state is getting propagated within write. - return hardware_interface::return_type::OK; - } - - hardware_interface::return_type write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override - { - ++write_calls_; - if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) - { - return hardware_interface::return_type::ERROR; - } - - for (auto i = 0; i < 3; ++i) - { - position_state_[i] += velocity_command_[0]; - velocity_state_[i] = velocity_command_[0]; - } - return hardware_interface::return_type::OK; - } - - CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) override - { - for (auto i = 0ul; i < 3; ++i) - { - velocity_state_[i] = 0.0; - } - return CallbackReturn::SUCCESS; - } - - CallbackReturn on_error(const rclcpp_lifecycle::State & /*previous_state*/) override - { - if (!recoverable_error_happened_) - { - recoverable_error_happened_ = true; - return CallbackReturn::SUCCESS; - } - else - { - return CallbackReturn::ERROR; - } - return CallbackReturn::FAILURE; - } - -private: - std::array position_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_state_ = { - {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN()}}; - std::array velocity_command_ = {{0.0, 0.0, 0.0}}; - - // Helper variables to initiate error on read - unsigned int read_calls_ = 0; - unsigned int write_calls_ = 0; - bool recoverable_error_happened_ = false; -}; -// END - class DummySystemDefault : public hardware_interface::SystemInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override @@ -683,134 +371,40 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface }; } // namespace test_components - -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator) +TEST(TestComponentInterfaces, dummy_actuator_default) { - hardware_interface::Actuator actuator_hw(std::make_unique()); + hardware_interface::Actuator actuator_hw( + std::make_unique()); + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + 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(); - ASSERT_EQ(2u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - - auto command_interfaces = actuator_hw.export_command_interfaces(); - ASSERT_EQ(1u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - - double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - - // Read and Write are working because it is INACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Read and Write are working because it is ACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ( - (10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - state = actuator_hw.shutdown(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[1]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch({""}, {""})); - EXPECT_EQ( - hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); -} -// END - -TEST(TestComponentInterfaces, dummy_actuator_default) -{ - hardware_interface::Actuator actuator_hw( - std::make_unique()); - const std::string urdf_to_test = - std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + - ros2_control_test_assets::urdf_tail; - const std::vector control_resources = - hardware_interface::parse_control_resources_from_urdf(urdf_to_test); - const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; - 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 + report_signals_size, state_interfaces.size()); - { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/position"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - } - { - auto [contains, position] = - test_components::vector_contains(state_interfaces, "joint1/velocity"); - EXPECT_TRUE(contains); - EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); - } + ASSERT_EQ(2u + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } auto command_interfaces = actuator_hw.export_command_interfaces(); ASSERT_EQ(1u, command_interfaces.size()); @@ -836,7 +430,8 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_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)); @@ -852,8 +447,10 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -869,7 +466,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position 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)); @@ -884,8 +481,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -896,39 +494,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default) hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_sensor) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - 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()); - - auto state_interfaces = sensor_hw.export_state_interfaces(); - ASSERT_EQ(1u, state_interfaces.size()); - EXPECT_EQ("joint1/voltage", state_interfaces[0]->get_name()); - EXPECT_EQ("voltage", state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); - - // Not updated because is is UNCONFIGURED - sensor_hw.read(TIME, PERIOD); - EXPECT_TRUE(std::isnan(state_interfaces[0]->get_value())); - - // Updated because is is INACTIVE - state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - EXPECT_EQ(0.0, state_interfaces[0]->get_value()); - - // It can read now - sensor_hw.read(TIME, PERIOD); - EXPECT_EQ(0x666, state_interfaces[0]->get_value()); -} -// END - TEST(TestComponentInterfaces, dummy_sensor_default) { hardware_interface::Sensor sensor_hw(std::make_unique()); @@ -972,135 +537,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(0x666, state_interfaces[si_joint1_vol]->get_value()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_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 = system_hw.export_state_interfaces(); - ASSERT_EQ(6u, state_interfaces.size()); - EXPECT_EQ("joint1/position", state_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint1/velocity", state_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint1", state_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint2/position", state_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[2]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", state_interfaces[3]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[3]->get_interface_name()); - EXPECT_EQ("joint2", state_interfaces[3]->get_prefix_name()); - EXPECT_EQ("joint3/position", state_interfaces[4]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[4]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[4]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", state_interfaces[5]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[5]->get_interface_name()); - EXPECT_EQ("joint3", state_interfaces[5]->get_prefix_name()); - - auto command_interfaces = system_hw.export_command_interfaces(); - ASSERT_EQ(3u, command_interfaces.size()); - EXPECT_EQ("joint1/velocity", command_interfaces[0]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[0]->get_interface_name()); - EXPECT_EQ("joint1", command_interfaces[0]->get_prefix_name()); - EXPECT_EQ("joint2/velocity", command_interfaces[1]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[1]->get_interface_name()); - EXPECT_EQ("joint2", command_interfaces[1]->get_prefix_name()); - EXPECT_EQ("joint3/velocity", command_interfaces[2]->get_name()); - EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, command_interfaces[2]->get_interface_name()); - EXPECT_EQ("joint3", command_interfaces[2]->get_prefix_name()); - - double velocity_value = 1.0; - command_interfaces[0]->set_value(velocity_value); // velocity - command_interfaces[1]->set_value(velocity_value); // velocity - command_interfaces[2]->set_value(velocity_value); // velocity - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - ASSERT_TRUE(std::isnan(state_interfaces[0]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[1]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[2]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[3]->get_value())); // velocity - ASSERT_TRUE(std::isnan(state_interfaces[4]->get_value())); // position value - ASSERT_TRUE(std::isnan(state_interfaces[5]->get_value())); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - - // Read and Write are working because it is INACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - EXPECT_EQ(step * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(step * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[5]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Read and Write are working because it is ACTIVE - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - EXPECT_EQ( - (10 + step) * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ( - (10 + step) * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ( - (10 + step) * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[5]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - state = system_hw.shutdown(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[1]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[2]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[3]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[4]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[5]->get_value()); // velocity - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch({}, {})); - EXPECT_EQ(hardware_interface::return_type::OK, system_hw.perform_command_mode_switch({}, {})); -} -// END - TEST(TestComponentInterfaces, dummy_system_default) { hardware_interface::System system_hw(std::make_unique()); @@ -1220,11 +656,14 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_TRUE(std::isnan(state_interfaces[si_joint1_pos]->get_value())); // position value + ASSERT_TRUE( + std::isnan(state_interfaces[si_joint1_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_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_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)); @@ -1240,14 +679,20 @@ TEST(TestComponentInterfaces, dummy_system_default) ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint1_pos]->get_value()); // position value EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + step ? velocity_value : 0, state_interfaces[si_joint1_vel]->get_value()); // velocity EXPECT_EQ( - step * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity + step * velocity_value, + state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + step * velocity_value, + state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1263,15 +708,15 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ( (10 + step) * velocity_value, - state_interfaces[si_joint1_pos]->get_value()); // position 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 + 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 + 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)); @@ -1286,12 +731,15 @@ TEST(TestComponentInterfaces, dummy_system_default) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity - EXPECT_EQ(20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint1_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint2_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint2_vel]->get_value()); // velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[si_joint3_pos]->get_value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint3_vel]->get_value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); } @@ -1331,67 +779,6 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) system_hw.perform_command_mode_switch(two_keys, one_key)); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) -{ - hardware_interface::Actuator actuator_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - 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(); - state = actuator_hw.configure(); - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) { hardware_interface::Actuator actuator_hw( @@ -1462,67 +849,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) -{ - hardware_interface::Actuator actuator_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - 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(); - state = actuator_hw.configure(); - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = actuator_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - EXPECT_EQ(command_interfaces[0]->get_value(), 0.0); - - state = actuator_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - - state = actuator_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = actuator_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) { hardware_interface::Actuator actuator_hw( @@ -1592,72 +918,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) -{ - hardware_interface::Sensor sensor_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - 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)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - - state = sensor_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // Noting should change because it is UNCONFIGURED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - - // activate again and expect reset values - state = sensor_hw.configure(); - EXPECT_EQ(state_interfaces[0]->get_value(), 0.0); - - state = sensor_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - // Initiate unrecoverable error - call read 99 times OK and on 100-time will return error - for (auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - - state = sensor_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // Noting should change because it is FINALIZED - for (auto step = 0u; step < 10; ++step) - { - ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - } - - // can not change state anymore - state = sensor_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) { hardware_interface::Sensor sensor_hw(std::make_unique()); @@ -1717,72 +977,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system_read_error_behavior) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_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 = system_hw.export_state_interfaces(); - auto command_interfaces = system_hw.export_command_interfaces(); - state = system_hw.configure(); - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = system_hw.configure(); - for (auto index = 0ul; index < 6; ++index) - { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); - } - for (auto index = 0ul; index < 3; ++index) - { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); - } - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) { hardware_interface::System system_hw(std::make_unique()); @@ -1853,72 +1047,6 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -// BEGIN (Handle export change): for backward compatibility -TEST(TestComponentInterfaces, dummy_system_write_error_behavior) -{ - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_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 = system_hw.export_state_interfaces(); - auto command_interfaces = system_hw.export_command_interfaces(); - state = system_hw.configure(); - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is first time therefore recoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - - // activate again and expect reset values - state = system_hw.configure(); - for (auto index = 0ul; index < 6; ++index) - { - EXPECT_EQ(state_interfaces[index]->get_value(), 0.0); - } - for (auto index = 0ul; index < 3; ++index) - { - EXPECT_EQ(command_interfaces[index]->get_value(), 0.0); - } - state = system_hw.activate(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::ACTIVE, state.label()); - - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - - // Initiate error on write (this is the second time therefore unrecoverable) - for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) - { - ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - } - ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - - state = system_hw.get_state(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); - - // can not change state anymore - state = system_hw.configure(); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); - EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); -} -// END - TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) { hardware_interface::System system_hw(std::make_unique()); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index d04bbcbab3..3b7a66f9ac 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -52,6 +52,17 @@ const auto warnig_signals_size = 2; const auto error_signals_size = 2; const auto report_signals_size = emergency_stop_signal_size + warnig_signals_size + error_signals_size; + +const hardware_interface::WARNING_MESSAGES empty_error_msg_vec(32, ""); +const hardware_interface::ERROR_MESSAGES empty_warn_msg_vec(32, ""); +const hardware_interface::ERROR_SIGNALS empty_error_code_vec(32, 0); +const hardware_interface::WARNING_SIGNALS empty_warn_code_vec(32, 0); +const hardware_interface::WARNING_MESSAGES error_msg_vec{ + "Some", "error", "msgs", "so vector is", "not empty."}; +const hardware_interface::ERROR_MESSAGES warn_msg_vec{"Some", "warning", "warn", + "msgs", "so vector is", "not empty."}; +const hardware_interface::ERROR_SIGNALS error_code_vec{1, 2, 3, 4, 5, 6, 7}; +const hardware_interface::WARNING_SIGNALS warn_code_vec{1, 2, 3, 4, 5, 6, 7}; } // namespace using namespace ::testing; // NOLINT @@ -79,10 +90,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface set_state("joint1/position", 0.0); set_state("joint1/velocity", 0.0); set_emergency_stop(false); - set_error_code(hardware_interface::ERROR_SIGNALS{}); - set_error_message(hardware_interface::WARNING_MESSAGES{}); - set_warning_code(hardware_interface::WARNING_SIGNALS{}); - set_warning_message(hardware_interface::WARNING_MESSAGES{}); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); if (recoverable_error_happened_) { @@ -104,16 +115,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { set_emergency_stop(true); - hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; - set_error_code(error_signals); - hardware_interface::WARNING_MESSAGES error_messages{ - {"Some", "error", "message that is split"}}; - set_error_message(error_messages); - hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; - set_warning_code(warning_signals); - hardware_interface::WARNING_MESSAGES warning_messages{ - {"Some", "warning", "message that is split"}}; - set_warning_message(warning_messages); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -128,16 +133,10 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { set_emergency_stop(true); - hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; - set_error_code(error_signals); - hardware_interface::WARNING_MESSAGES error_messages{ - {"Some", "error", "message that is split"}}; - set_error_message(error_messages); - hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; - set_warning_code(warning_signals); - hardware_interface::WARNING_MESSAGES warning_messages{ - {"Some", "warning", "message that is split"}}; - set_warning_message(warning_messages); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } auto position_state = get_state("joint1/position"); @@ -195,10 +194,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface { set_state(name, 0.0); } - set_error_code(hardware_interface::ERROR_SIGNALS{}); - set_error_message(hardware_interface::WARNING_MESSAGES{}); - set_warning_code(hardware_interface::WARNING_SIGNALS{}); - set_warning_message(hardware_interface::WARNING_MESSAGES{}); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); read_calls_ = 0; return CallbackReturn::SUCCESS; } @@ -211,16 +210,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface ++read_calls_; if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { - hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; - set_error_code(error_signals); - hardware_interface::WARNING_MESSAGES error_messages{ - {"Some", "error", "message that is split"}}; - set_error_message(error_messages); - hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; - set_warning_code(warning_signals); - hardware_interface::WARNING_MESSAGES warning_messages{ - {"Some", "warning", "message that is split"}}; - set_warning_message(warning_messages); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -273,10 +266,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface set_state(velocity_states_[i], 0.0); } set_emergency_stop(false); - set_error_code(hardware_interface::ERROR_SIGNALS{}); - set_error_message(hardware_interface::WARNING_MESSAGES{}); - set_warning_code(hardware_interface::WARNING_SIGNALS{}); - set_warning_message(hardware_interface::WARNING_MESSAGES{}); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); // reset command only if error is initiated if (recoverable_error_happened_) { @@ -301,16 +294,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface if (read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { set_emergency_stop(true); - hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; - set_error_code(error_signals); - hardware_interface::WARNING_MESSAGES error_messages{ - {"Some", "error", "message that is split"}}; - set_error_message(error_messages); - hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; - set_warning_code(warning_signals); - hardware_interface::WARNING_MESSAGES warning_messages{ - {"Some", "warning", "message that is split"}}; - set_warning_message(warning_messages); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -325,16 +312,10 @@ class DummySystemDefault : public hardware_interface::SystemInterface if (write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS) { set_emergency_stop(true); - hardware_interface::ERROR_SIGNALS error_signals{{1, 2, 3}}; - set_error_code(error_signals); - hardware_interface::WARNING_MESSAGES error_messages{ - {"Some", "error", "message that is split"}}; - set_error_message(error_messages); - hardware_interface::WARNING_SIGNALS warning_signals{{1, 2, 3}}; - set_warning_code(warning_signals); - hardware_interface::WARNING_MESSAGES warning_messages{ - {"Some", "warning", "message that is split"}}; - set_warning_message(warning_messages); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); return hardware_interface::return_type::ERROR; } @@ -813,18 +794,26 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -849,18 +838,26 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -951,18 +948,26 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = actuator_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -1021,16 +1026,24 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = sensor_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -1116,18 +1129,26 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); @@ -1154,18 +1175,26 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); @@ -1232,18 +1261,26 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) for (auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i) { ASSERT_EQ(hardware_interface::return_type::OK, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 0.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 0.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), false); + ASSERT_EQ( + state_interfaces[error_signal]->get_value>(), empty_error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), + empty_error_msg_vec); + ASSERT_EQ( + state_interfaces[warning_signal]->get_value>(), empty_warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), + empty_warn_msg_vec); } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[error_signal_msg]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal]->get_value(), 1.0); - ASSERT_EQ(state_interfaces[warning_signal_msg]->get_value(), 1.0); + ASSERT_EQ(state_interfaces[emergency_stop]->get_value(), true); + ASSERT_EQ(state_interfaces[error_signal]->get_value>(), error_code_vec); + ASSERT_EQ( + state_interfaces[error_signal_msg]->get_value>(), error_msg_vec); + ASSERT_EQ(state_interfaces[warning_signal]->get_value>(), warn_code_vec); + ASSERT_EQ( + state_interfaces[warning_signal_msg]->get_value>(), warn_msg_vec); state = system_hw.get_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index aea7bbb04c..52007fde5a 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -13,8 +13,12 @@ // limitations under the License. #include +#include + #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" using hardware_interface::CommandInterface; using hardware_interface::InterfaceDescription; @@ -27,21 +31,510 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace -TEST(TestHandle, command_interface) +TEST(TestHandle, ci_empty_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, ci_true_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, ci_empty_double_initialization) { - double value = 1.337; - CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, ci_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), 1.5); EXPECT_NO_THROW(interface.set_value(0.0)); - EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + EXPECT_EQ(interface.get_value(), 0.0); } -TEST(TestHandle, state_interface) +TEST(TestHandle, ci_negative_double_initialization) { - double value = 1.337; - StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); - // interface.set_value(5); compiler error, no set_value function + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, ci_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::invalid_argument); +} + +TEST(TestHandle, ci_int8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_int8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_int8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, ci_throw_on_get_wrong_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, ci_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, ci_throw_on_empty_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); +} + +TEST(TestHandle, si_empty_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, si_true_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, si_empty_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, si_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), 1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_negative_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::invalid_argument); +} + +TEST(TestHandle, si_int8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_int8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_int8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, si_throw_on_get_wrong_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, si_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, si_throw_on_empty_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); } TEST(TestHandle, name_getters_work) @@ -52,20 +545,22 @@ TEST(TestHandle, name_getters_work) EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME); } -TEST(TestHandle, value_methods_throw_for_nullptr) +TEST(TestHandle, ci_value_methods) { CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); - EXPECT_ANY_THROW(handle.set_value(0.0)); + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); + EXPECT_NO_THROW(handle.set_value(0.0)); + EXPECT_EQ(handle.get_value(), 0.0); } -TEST(TestHandle, value_methods_work_on_non_nullptr) +TEST(TestHandle, si_value_methods) { - double value = 1.337; - CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); + StateInterface handle{JOINT_NAME, FOO_INTERFACE}; + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); + EXPECT_EQ(handle.get_value(), 0.0); } TEST(TestHandle, interface_description_state_interface_name_getters_work) @@ -74,12 +569,13 @@ TEST(TestHandle, interface_description_state_interface_name_getters_work) const std::string JOINT_NAME_1 = "joint1"; InterfaceInfo info; info.name = POSITION_INTERFACE; + info.data_type = "double"; InterfaceDescription interface_descr(JOINT_NAME_1, info); StateInterface handle{interface_descr}; EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); - EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); + (handle.get_prefix_name(), JOINT_NAME_1); } TEST(TestHandle, interface_description_command_interface_name_getters_work) @@ -87,6 +583,7 @@ TEST(TestHandle, interface_description_command_interface_name_getters_work) const std::string POSITION_INTERFACE = "position"; const std::string JOINT_NAME_1 = "joint1"; InterfaceInfo info; + info.data_type = "double"; info.name = POSITION_INTERFACE; InterfaceDescription interface_descr(JOINT_NAME_1, info); CommandInterface handle{interface_descr}; From c3c87187311769785d8fe9bccb2ea165e57b9bff Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 31 Jan 2024 16:40:15 +0100 Subject: [PATCH 28/35] rename unclear variables --- .../test_component_interfaces_custom_export.cpp | 8 ++++---- .../test/test_error_warning_codes.cpp | 17 ++++++----------- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index 526f12ae5e..757bbb68dd 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -244,10 +244,10 @@ TEST(TestComponentInterfaces, dummy_sensor_default_custom_export) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); const auto listed_interface_size = 1u; - const auto interfaces_sizeze = listed_interface_size + error_signals_size + warnig_signals_size; + const auto interfaces_sizes = listed_interface_size + error_signals_size + warnig_signals_size; auto state_interfaces = sensor_hw.export_state_interfaces(); // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" - ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); + ASSERT_EQ(interfaces_sizes + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/voltage"); @@ -283,10 +283,10 @@ TEST(TestComponentInterfaces, dummy_system_default_custom_export) EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); const auto listed_interface_size = 6u; - const auto interfaces_sizeze = listed_interface_size + report_signals_size; + const auto interfaces_sizes = listed_interface_size + report_signals_size; auto state_interfaces = system_hw.export_state_interfaces(); // interfaces size + the one unlisted interface "joint1/some_unlisted_interface" - ASSERT_EQ(interfaces_sizeze + 1u, state_interfaces.size()); + ASSERT_EQ(interfaces_sizes + 1u, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); diff --git a/hardware_interface/test/test_error_warning_codes.cpp b/hardware_interface/test/test_error_warning_codes.cpp index 3b7a66f9ac..d90b616a4b 100644 --- a/hardware_interface/test/test_error_warning_codes.cpp +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -383,9 +383,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 2; + const auto listed_interface_size = 2; auto state_interfaces = actuator_hw.export_state_interfaces(); - ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + ASSERT_EQ(listed_interface_size + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -500,10 +500,10 @@ TEST(TestComponentInterfaces, dummy_sensor_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 1; + const auto listed_interface_size = 1; auto state_interfaces = sensor_hw.export_state_interfaces(); ASSERT_EQ( - state_interface_offset + warnig_signals_size + error_signals_size, state_interfaces.size()); + listed_interface_size + warnig_signals_size + error_signals_size, state_interfaces.size()); // check that the normal interfaces get exported as expected { auto [contains, position] = @@ -586,9 +586,9 @@ TEST(TestComponentInterfaces, dummy_system_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 6; + const auto listed_interface_size = 6; auto state_interfaces = system_hw.export_state_interfaces(); - ASSERT_EQ(state_interface_offset + report_signals_size, state_interfaces.size()); + ASSERT_EQ(listed_interface_size + report_signals_size, state_interfaces.size()); { auto [contains, position] = test_components::vector_contains(state_interfaces, "joint1/position"); @@ -754,7 +754,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 2; auto state_interfaces = actuator_hw.export_state_interfaces(); auto command_interfaces = actuator_hw.export_command_interfaces(); state = actuator_hw.configure(); @@ -885,7 +884,6 @@ TEST(TestComponentInterfaces, dummy_actuator_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 2; auto state_interfaces = actuator_hw.export_state_interfaces(); auto command_interfaces = actuator_hw.export_command_interfaces(); state = actuator_hw.configure(); @@ -992,7 +990,6 @@ TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) const hardware_interface::HardwareInfo voltage_sensor_res = control_resources[0]; auto state = sensor_hw.initialize(voltage_sensor_res); - const auto state_interface_offset = 1; auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE state = sensor_hw.configure(); @@ -1089,7 +1086,6 @@ TEST(TestComponentInterfaces, dummy_system_default_read_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 6; auto state_interfaces = system_hw.export_state_interfaces(); auto command_interfaces = system_hw.export_command_interfaces(); state = system_hw.configure(); @@ -1221,7 +1217,6 @@ TEST(TestComponentInterfaces, dummy_system_default_write_error_behavior) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); - const auto state_interface_offset = 6; auto state_interfaces = system_hw.export_state_interfaces(); auto command_interfaces = system_hw.export_command_interfaces(); state = system_hw.configure(); From ad416cea004b99db04525030e19dade41fc5e289 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 2 Feb 2024 13:07:35 +0100 Subject: [PATCH 29/35] tmp commit! remove deprecated handle constructor -> adjust generic system ! not compiling not all tests have been adjusted still wip! --- .../include/hardware_interface/handle.hpp | 32 +- .../mock_components/generic_system.hpp | 67 +-- .../src/mock_components/generic_system.cpp | 528 ++++++++---------- .../test_force_torque_sensor.cpp | 53 +- .../test_single_joint_actuator.cpp | 39 +- .../test_system_with_command_modes.cpp | 55 +- .../test_two_joint_system.cpp | 28 +- .../test/test_components/test_actuator.cpp | 60 +- .../test/test_components/test_sensor.cpp | 12 - .../test/test_components/test_system.cpp | 62 +- .../test/test_resource_manager.cpp | 31 +- .../transmission_interface/accessor.hpp | 5 +- .../differential_transmission.hpp | 12 - .../four_bar_linkage_transmission.hpp | 12 - .../simple_transmission.hpp | 12 +- transmission_interface/test/utils_test.cpp | 7 +- 16 files changed, 406 insertions(+), 609 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 31b459177a..471258b759 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -36,13 +36,12 @@ class Handle public: [[deprecated("Use InterfaceDescription for initializing the Interface")]] - Handle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) + Handle(const std::string & prefix_name, const std::string & interface_name) + : prefix_name_(prefix_name), interface_name_(interface_name) { - // default to double - value_ = std::numeric_limits::quiet_NaN(); + // init to default value defined by init_handle_value() + InterfaceInfo info; + init_handle_value(info); } explicit Handle(const InterfaceDescription & interface_description) @@ -52,23 +51,6 @@ class Handle init_handle_value(interface_description.interface_info); } - [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - explicit Handle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) - { - // default to double - value_ = std::numeric_limits::quiet_NaN(); - } - - [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - explicit Handle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) - { // default to double - value_ = std::numeric_limits::quiet_NaN(); - } - Handle(const Handle & other) = default; Handle(Handle && other) = default; @@ -79,9 +61,6 @@ class Handle virtual ~Handle() = default; - /// Returns true if handle references a value. - inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } const std::string & get_interface_name() const { return interface_name_; } @@ -183,7 +162,6 @@ class Handle std::string prefix_name_; std::string interface_name_; HANDLE_DATATYPE value_; - double * value_ptr_; }; class StateInterface : public Handle diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index fbb8547ab1..addc8f21f5 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -18,6 +18,8 @@ #define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ #include +#include +#include #include #include "hardware_interface/handle.hpp" @@ -41,9 +43,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste public: CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + std::vector export_command_interfaces_2() override; return_type prepare_command_mode_switch( const std::vector & start_interfaces, @@ -71,54 +71,43 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste const std::vector standard_interfaces_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; + // added dynamically during on_init + std::vector non_standard_interfaces_; - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_commands_; - std::vector> joint_states_; - - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_commands_; - std::vector> other_states_; - - std::vector sensor_interfaces_; - /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_mock_commands_; - std::vector> sensor_states_; - - std::vector gpio_interfaces_; - /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_mock_commands_; - std::vector> gpio_commands_; - std::vector> gpio_states_; + struct MimicJoint + { + std::string joint_name; + std::string mimic_joint_name; + double multiplier = 1.0; + }; + std::vector mimic_joints_; + + // All the joints that are of type defined by standard_interfaces_ vector -> In {pos, vel, acc, + // effort} + std::vector std_joint_names_; + std::unordered_set std_joint_command_interface_names_; + std::unordered_set std_joint_state_interface_names_; + + // All the joints that are of not of a type defined by standard_interfaces_ vector -> Not in {pos, + // vel, acc, effort} + std::vector other_joint_names_; + std::unordered_set other_joint_command_interface_names_; + std::unordered_set other_joint_state_interface_names_; private: - template - bool get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces); - - void initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos); - - template - bool populate_interfaces( + void search_and_add_interface_names( const std::vector & components, - std::vector & interfaces, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces); + const std::vector & interface_list, std::vector & vector_to_add); bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; std::string custom_interface_with_following_offset_; - size_t index_custom_interface_with_following_offset_; + std::string custom_interface_name_with_following_offset_; bool calculate_dynamics_; - std::vector joint_control_mode_; + std::unordered_map joint_control_mode_; bool command_propagation_disabled_; }; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 2d8a01a34f..9d8ab9e582 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -59,7 +59,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } }; - // check if to create mock command interface for sensor auto it = info_.hardware_parameters.find("mock_sensor_commands"); if (it != info_.hardware_parameters.end()) @@ -119,49 +118,90 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i custom_interface_with_following_offset_ = it->second; } } - // it's extremely improbable that std::distance results int this value - therefore default - index_custom_interface_with_following_offset_ = std::numeric_limits::max(); - - // Initialize storage for standard interfaces - initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints); - // set all values without initial values to 0 - for (auto i = 0u; i < info_.joints.size(); i++) - { - for (auto j = 0u; j < standard_interfaces_.size(); j++) - { - if (std::isnan(joint_states_[j][i])) - { - joint_states_[j][i] = 0.0; - } - } - } // search for non-standard joint interfaces for (const auto & joint : info_.joints) { // populate non-standard command interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); + populate_non_standard_interfaces(joint.command_interfaces, non_standard_interfaces_); // populate non-standard state interfaces to other_interfaces_ - populate_non_standard_interfaces(joint.state_interfaces, other_interfaces_); + populate_non_standard_interfaces(joint.state_interfaces, non_standard_interfaces_); } - // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); + // search for standard joint interfaces and add them to std_joint_names_ and + // search for non-standard joint interfaces and add them to other_joint_names_ + for (const auto & joint : info.joints) + { + for (const auto & state_inteface : joint.state_interfaces) + { + if ( + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), state_inteface.name) == + standard_interfaces_.end()) + { + std_joint_names_.push_back(joint.name); + std_joint_state_interface_names_.insert(joint.name + "/" + state_inteface.name); + } + else if ( + std::find( + non_standard_interfaces_.begin(), non_standard_interfaces_.end(), state_inteface.name) == + non_standard_interfaces_.end()) + { + other_joint_names_.push_back(joint.name); + other_joint_state_interface_names_.insert(joint.name + "/" + state_inteface.name); + } + else + { + RCUTILS_LOG_WARN_NAMED( + "The state_interface: '%s' of the joint: '%s' is neither part of the std_interfaces " + "(pos, vel, acc, eff), nor of any other use " + "defined.", + state_inteface.name.c_str(), joint.name.c_str()); + } + } + + // Check that for all the available state_interfaces a command_interface exists + // We don't need to add name of the joint again since it has already been added for the + // state_interface + for (const auto & command_interface : joint.command_interfaces) + { + if ( + std::find( + std_joint_state_interface_names_.begin(), std_joint_state_interface_names_.end(), + command_interface.name) == std_joint_state_interface_names_.end()) + { + std_joint_command_interface_names_.insert(joint.name + "/" + command_interface.name); + } + else if ( + std::find( + other_joint_state_interface_names_.begin(), other_joint_state_interface_names_.end(), + command_interface.name) == other_joint_state_interface_names_.end()) + { + other_joint_command_interface_names_.insert(joint.name + "/" + command_interface.name); + } + else + { + throw std::runtime_error( + std::string("For command_interface: '") + command_interface.name + "' of the joint: '" + + joint.name + "' exists no state_interface"); + } + } + } // when following offset is used on custom interface then find its index + custom_interface_name_with_following_offset_ = ""; if (!custom_interface_with_following_offset_.empty()) { auto if_it = std::find( - other_interfaces_.begin(), other_interfaces_.end(), custom_interface_with_following_offset_); - if (if_it != other_interfaces_.end()) + non_standard_interfaces_.begin(), non_standard_interfaces_.end(), + custom_interface_with_following_offset_); + if (if_it != non_standard_interfaces_.end()) { - index_custom_interface_with_following_offset_ = - std::distance(other_interfaces_.begin(), if_it); + custom_interface_name_with_following_offset_ = *if_it; RCUTILS_LOG_INFO_NAMED( - "mock_generic_system", "Custom interface with following offset '%s' found at index: %zu.", + "mock_generic_system", "Custom interface with following offset '%s' found at index: %s.", custom_interface_with_following_offset_.c_str(), - index_custom_interface_with_following_offset_); + custom_interface_name_with_following_offset_.c_str()); } else { @@ -172,149 +212,79 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - for (const auto & sensor : info_.sensors) + // Search for mimic joints + for (auto i = 0u; i < info_.joints.size(); ++i) { - for (const auto & interface : sensor.state_interfaces) + const auto & joint = info_.joints.at(i); + if (joint.parameters.find("mimic") != joint.parameters.cend()) { - if ( - std::find(sensor_interfaces_.begin(), sensor_interfaces_.end(), interface.name) == - sensor_interfaces_.end()) + const auto mimicked_joint_it = std::find_if( + info_.joints.begin(), info_.joints.end(), + [&mimicked_joint = + joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) + { return joint_info.name == mimicked_joint; }); + if (mimicked_joint_it == info_.joints.cend()) { - sensor_interfaces_.emplace_back(interface.name); + throw std::runtime_error( + std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); } - } - } - initialize_storage_vectors( - sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); - - // search for gpio interfaces - for (const auto & gpio : info_.gpios) - { - // populate non-standard command interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); - - // populate non-standard state interfaces to gpio_interfaces_ - populate_non_standard_interfaces(gpio.state_interfaces, gpio_interfaces_); - } - - // Mock gpio command interfaces - if (use_mock_gpio_command_interfaces_) - { - initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); - } - // Real gpio command interfaces - else - { - initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); - } - - return CallbackReturn::SUCCESS; -} - -std::vector GenericSystem::export_state_interfaces() -{ - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.state_interfaces) - { - // Add interface: if not in the standard list then use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces)) + MimicJoint mimic_joint; + mimic_joint.joint_name = joint.name; + mimic_joint.mimic_joint_name = mimicked_joint_it->name; + auto param_it = joint.parameters.find("multiplier"); + if (param_it != joint.parameters.end()) { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); } + mimic_joints_.push_back(mimic_joint); } } - // Sensor state interfaces - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); - }; - - // GPIO state interfaces - if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) + // set all values without initial values to 0 + for (auto [name, descr] : joint_state_interfaces_) { - throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); + if (std::isnan(get_state(name))) + { + set_state(name, 0.0); + } } - return state_interfaces; + return CallbackReturn::SUCCESS; } -std::vector GenericSystem::export_command_interfaces() +std::vector GenericSystem::export_command_interfaces_2() { - std::vector command_interfaces; - - // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) - { - const auto & joint = info_.joints[i]; - for (const auto & interface : joint.command_interfaces) - { - // Add interface: if not in the standard list than use "other" interface list - if (!get_interface( - joint.name, standard_interfaces_, interface.name, i, joint_commands_, - command_interfaces)) - { - if (!get_interface( - joint.name, other_interfaces_, interface.name, i, other_commands_, - command_interfaces)) - { - throw std::runtime_error( - "Interface is not found in the standard nor other list. " - "This should never happen!"); - } - } - } - } - // Set position control mode per default - joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); - + // we check if we should mock command interfaces or not. After they have been exported we can then + // use them as we would normally via (set/get)_(state/command) + std::vector descriptions; // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { - if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) + for (const auto & sensor : info_.sensors) { - throw std::runtime_error( - "Interface is not found in the standard nor other list. This should never happen!"); + for (const auto & state_interface : sensor.state_interfaces) + { + hardware_interface::InterfaceInfo info = state_interface; + hardware_interface::InterfaceDescription descr(sensor.name, info); + descriptions.push_back(descr); + } } } // Mock gpio command interfaces (consider all state interfaces for command interfaces) if (use_mock_gpio_command_interfaces_) { - if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) + for (const auto & gpio : info_.gpios) { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); - } - } - // GPIO command interfaces (real command interfaces) - else - { - if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) - { - throw std::runtime_error( - "Interface is not found in the gpio list. This should never happen!"); + for (const auto & state_interface : gpio.state_interfaces) + { + hardware_interface::InterfaceInfo info = state_interface; + hardware_interface::InterfaceDescription descr(gpio.name, info); + descriptions.push_back(descr); + } } } - - return command_interfaces; + return descriptions; } return_type GenericSystem::prepare_command_mode_switch( @@ -330,8 +300,8 @@ return_type GenericSystem::prepare_command_mode_switch( const size_t FOUND_ONCE_FLAG = 1000000; - std::vector joint_found_in_x_requests_; - joint_found_in_x_requests_.resize(info_.joints.size(), 0); + std::vector joint_found_in_x_requests; + joint_found_in_x_requests.resize(info_.joints.size(), 0); for (const auto & key : start_interfaces) { @@ -343,14 +313,14 @@ return_type GenericSystem::prepare_command_mode_switch( if (joint_it_found != info_.joints.end()) { const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); - if (joint_found_in_x_requests_[joint_index] == 0) + if (joint_found_in_x_requests[joint_index] == 0) { - joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; + joint_found_in_x_requests[joint_index] = FOUND_ONCE_FLAG; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { @@ -362,7 +332,7 @@ return_type GenericSystem::prepare_command_mode_switch( "might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); } - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { @@ -374,7 +344,7 @@ return_type GenericSystem::prepare_command_mode_switch( "this might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); } - joint_found_in_x_requests_[joint_index] += 1; + joint_found_in_x_requests[joint_index] += 1; } } else @@ -388,7 +358,7 @@ return_type GenericSystem::prepare_command_mode_switch( for (size_t i = 0; i < info_.joints.size(); ++i) { // There has to always be at least one control mode from the above three set - if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) + if (joint_found_in_x_requests[i] == FOUND_ONCE_FLAG) { RCUTILS_LOG_ERROR_NAMED( "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", @@ -398,13 +368,13 @@ return_type GenericSystem::prepare_command_mode_switch( } // Currently we don't support multiple interface request - if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + if (joint_found_in_x_requests[i] > (FOUND_ONCE_FLAG + 1)) { RCUTILS_LOG_ERROR_NAMED( "mock_generic_system", "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", - joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + joint_found_in_x_requests[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); ret_val = hardware_interface::return_type::ERROR; } } @@ -434,15 +404,15 @@ return_type GenericSystem::perform_command_mode_switch( if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { - joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + joint_control_mode_[key] = POSITION_INTERFACE_INDEX; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { - joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + joint_control_mode_[key] = VELOCITY_INTERFACE_INDEX; } if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { - joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + joint_control_mode_[key] = ACCELERATION_INTERFACE_INDEX; } } } @@ -459,97 +429,120 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::OK; } - auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0) + auto mirror_command_to_state = [this](const auto & joint_names, const auto & interface_names) { - for (size_t i = start_index; i < states_.size(); ++i) + for (const auto & joint_name : joint_names) { - for (size_t j = 0; j < states_[i].size(); ++j) + for (const auto & interface_name : interface_names) { - if (!std::isnan(commands_[i][j])) + const auto & joint_state_name = joint_name + "/" + interface_name; + if ( + this->std_joint_command_interface_names_.find(joint_state_name) != + this->std_joint_command_interface_names_.end()) { - states_[i][j] = commands_[i][j]; + this->set_state(joint_state_name, this->get_command(joint_state_name)); } } } }; - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + auto mirror_all_available_commands_to_states = [this](const auto & interfaces) + { + for (const auto & [name, descr] : interfaces) + { + // TODO(Manuel) should this be checked if interface exists??? + this->set_state(name, this->get_command(name)); + } + }; + + if (calculate_dynamics_) { - if (calculate_dynamics_) + for (const auto joint_name : std_joint_names_) { - switch (joint_control_mode_[j]) + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; + const auto joint_vel = joint_name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto joint_acc = joint_name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto joint_eff = joint_name + "/" + hardware_interface::HW_IF_EFFORT; + + switch (joint_control_mode_.at(joint_name)) { case ACCELERATION_INTERFACE_INDEX: { // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + // apply offset to positions only + const auto old_pos = get_state(joint_pos); + const auto new_pos = + get_state(joint_vel) * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + set_state(joint_pos, old_pos + new_pos); - joint_states_[VELOCITY_INTERFACE_INDEX][j] += - joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + const auto old_vel = get_state(joint_vel); + const auto new_vel = get_state(joint_acc) * period.seconds(); + set_state(joint_vel, old_vel + new_vel); - if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_acc) != + std_joint_command_interface_names_.end()) { - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + set_state(joint_acc, get_command(joint_acc)); } break; } case VELOCITY_INTERFACE_INDEX: { // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + const auto old_pos = get_state(joint_pos); + const auto new_pos = + get_state(joint_vel) * period.seconds() + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + set_state(joint_pos, old_pos + new_pos); - if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_vel) != + std_joint_command_interface_names_.end()) { - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - joint_commands_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + const auto old_vel = get_state(joint_vel); + set_state(joint_vel, get_command(joint_vel)); + set_state(joint_acc, (get_state(joint_vel) - old_vel) / period.seconds()); } break; } case POSITION_INTERFACE_INDEX: { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if ( + std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end()) { - const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + const double old_pos = get_state(joint_pos); + const double old_vel = get_state(joint_vel); + + set_state( + joint_pos, get_command(joint_pos) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); + set_state(joint_vel, (get_state(joint_pos) - old_pos) / period.seconds()); + set_state(joint_acc, (get_state(joint_vel) - old_vel) / period.seconds()); } break; } } } - else + } + else + { + for (const auto joint_name : std_joint_names_) { - for (size_t k = 0; k < joint_states_[POSITION_INTERFACE_INDEX].size(); ++k) + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; + if ( + std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end()) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][k])) - { - joint_states_[POSITION_INTERFACE_INDEX][k] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][k] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - } + set_state( + joint_pos, get_command(joint_pos) + (custom_interface_with_following_offset_.empty() + ? position_state_following_offset_ + : 0.0)); } } } @@ -558,136 +551,57 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur // velocity, and acceleration interface if (calculate_dynamics_) { - mirror_command_to_state(joint_states_, joint_commands_, 3); + std::vector interfaces( + standard_interfaces_.begin() + 3, standard_interfaces_.end()); + mirror_command_to_state(std_joint_names_, interfaces); } else { - mirror_command_to_state(joint_states_, joint_commands_, 1); + std::vector interfaces( + standard_interfaces_.begin() + 1, standard_interfaces_.end()); + mirror_command_to_state(std_joint_names_, interfaces); } - for (const auto & mimic_joint : info_.mimic_joints) + for (const auto & mimic_joint : mimic_joints_) { - for (auto i = 0u; i < joint_states_.size(); ++i) - { - joint_states_[i][mimic_joint.joint_index] = - mimic_joint.offset + - mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index]; - } + set_state( + mimic_joint.joint_name, get_state(mimic_joint.mimic_joint_name) * mimic_joint.multiplier); } - for (size_t i = 0; i < other_states_.size(); ++i) + for (const auto joint_name : other_joint_names_) { - for (size_t j = 0; j < other_states_[i].size(); ++j) + for (const auto interface_name : non_standard_interfaces_) { + const auto joint_inteface = joint_name + "/" + interface_name; + const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; if ( - i == index_custom_interface_with_following_offset_ && - !std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + interface_name == custom_interface_name_with_following_offset_ && + (std_joint_command_interface_names_.find(joint_pos) != + std_joint_command_interface_names_.end())) { - other_states_[i][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_; + set_state(joint_inteface, get_command(joint_pos) + position_state_following_offset_); } - else if (!std::isnan(other_commands_[i][j])) + else if ( + other_joint_command_interface_names_.find(joint_inteface) != + other_joint_command_interface_names_.end()) { - other_states_[i][j] = other_commands_[i][j]; + set_state(joint_inteface, get_command(joint_inteface)); } } } + // do loopback on all sensor interfaces if (use_mock_sensor_command_interfaces_) { - mirror_command_to_state(sensor_states_, sensor_mock_commands_); + mirror_all_available_commands_to_states(sensor_state_interfaces_); } - if (use_mock_gpio_command_interfaces_) - { - mirror_command_to_state(gpio_states_, gpio_mock_commands_); - } - else - { - // do loopback on all gpio interfaces - mirror_command_to_state(gpio_states_, gpio_commands_); - } + // do loopback on all gpio interfaces + mirror_all_available_commands_to_states(gpio_state_interfaces_); return return_type::OK; } -// Private methods -template -bool GenericSystem::get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces) -{ - auto it = std::find(interface_list.begin(), interface_list.end(), interface_name); - if (it != interface_list.end()) - { - auto j = std::distance(interface_list.begin(), it); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; - } - return false; -} - -void GenericSystem::initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces, - const std::vector & component_infos) -{ - // Initialize storage for all joints, regardless of their existence - commands.resize(interfaces.size()); - states.resize(interfaces.size()); - for (auto i = 0u; i < interfaces.size(); i++) - { - commands[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - states[i].resize(component_infos.size(), std::numeric_limits::quiet_NaN()); - } - - // Initialize with values from URDF - for (auto i = 0u; i < component_infos.size(); i++) - { - const auto & component = component_infos[i]; - for (const auto & interface : component.state_interfaces) - { - auto it = std::find(interfaces.begin(), interfaces.end(), interface.name); - - // If interface name is found in the interfaces list - if (it != interfaces.end()) - { - auto index = std::distance(interfaces.begin(), it); - - // Check the initial_value param is used - if (!interface.initial_value.empty()) - { - states[index][i] = hardware_interface::stod(interface.initial_value); - } - } - } - } -} - -template -bool GenericSystem::populate_interfaces( - const std::vector & components, - std::vector & interface_names, std::vector> & storage, - std::vector & target_interfaces, bool using_state_interfaces) -{ - for (auto i = 0u; i < components.size(); i++) - { - const auto & component = components[i]; - const auto interfaces = - (using_state_interfaces) ? component.state_interfaces : component.command_interfaces; - for (const auto & interface : interfaces) - { - if (!get_interface( - component.name, interface_names, interface.name, i, storage, target_interfaces)) - { - return false; - } - } - } - - return true; -} } // namespace mock_components #include "pluginlib/class_list_macros.hpp" diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index 47b19f9769..16ecb7649d 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -19,9 +19,9 @@ #include "hardware_interface/sensor_interface.hpp" +using hardware_interface::InterfaceDescription; using hardware_interface::return_type; using hardware_interface::SensorInterface; -using hardware_interface::StateInterface; namespace test_hardware_components { @@ -50,54 +50,41 @@ class TestForceTorqueSensor : public SensorInterface } } + sensor_name_ = info_.sensors[0].name; fprintf(stderr, "TestForceTorqueSensor configured successfully.\n"); return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; - const auto & sensor_name = info_.sensors[0].name; - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fy", &values_.fy)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "fz", &values_.fz)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tx", &values_.tx)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "ty", &values_.ty)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(sensor_name, "tz", &values_.tz)); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + for (const auto & interface_name : inteface_names_) + { + info.name = interface_name; + state_interfaces.push_back(hardware_interface::InterfaceDescription(sensor_name_, info)); + } return state_interfaces; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - values_.fx = fmod((values_.fx + 1.0), 10); - values_.fy = fmod((values_.fy + 1.0), 10); - values_.fz = fmod((values_.fz + 1.0), 10); - values_.tx = fmod((values_.tx + 1.0), 10); - values_.ty = fmod((values_.ty + 1.0), 10); - values_.tz = fmod((values_.tz + 1.0), 10); + for (const auto & interface_name : inteface_names_) + { + const auto name = sensor_name_ + "/" + interface_name; + + set_state(name, fmod((get_state(name) + 1.0), 10)); + } return return_type::OK; } private: - struct FTValues - { - double fx = 0.0; - double fy = 0.0; - double fz = 0.0; - double tx = 0.0; - double ty = 0.0; - double tz = 0.0; - }; - - FTValues values_; + std::vector inteface_names_{"fx", "fy", "fz", "tx", "ty", "tz"}; + std::string sensor_name_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index 390056d990..c4b64f9a3f 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -64,30 +64,35 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::ERROR; } } + joint_name_ = info_.joints[0].name; + joint_pos_ = joint_name_ + "/" + hardware_interface::HW_IF_POSITION; + joint_vel_ = joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY; fprintf(stderr, "TestSingleJointActuator configured successfully.\n"); return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; - const auto & joint_name = info_.joints[0].name; - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint_name, hardware_interface::HW_IF_VELOCITY, &velocity_state_)); + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + state_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; - const auto & joint_name = info_.joints[0].name; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back(hardware_interface::InterfaceDescription(joint_name_, info)); return command_interfaces; } @@ -99,15 +104,15 @@ class TestSingleJointActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - velocity_state_ = position_command_ - position_state_; - position_state_ = position_command_; + set_state(joint_vel_, get_command(joint_pos_) - get_state(joint_pos_)); + set_state(joint_pos_, get_command(joint_pos_)); return return_type::OK; } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double position_command_ = 0.0; + std::string joint_name_; + std::string joint_pos_; + std::string joint_vel_; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index aba2f86fe5..f9cba77394 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -74,31 +74,42 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_ACCELERATION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); + info.name = hardware_interface::HW_IF_VELOCITY; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return command_interfaces; @@ -120,7 +131,10 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & stop_interfaces) override { - acceleration_state_[0] += 1.0; + const auto first_joint_acc = + info_.joints[0].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto old_acc = get_state(first_joint_acc); + set_state(first_joint_acc, old_acc + 1.0); // Starting interfaces start_modes_.clear(); @@ -168,7 +182,10 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface const std::vector & start_interfaces, const std::vector & /*stop_interfaces*/) override { - acceleration_state_[0] += 100.0; + const auto first_joint_acc = + info_.joints[0].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto old_acc = get_state(first_joint_acc); + set_state(first_joint_acc, old_acc + 100.0); // Test of failure in perform command mode switch // Fail if given an empty list. // This should never occur in a real system as the same start_interfaces list is sent to both @@ -183,12 +200,6 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface private: std::vector start_modes_ = {"position", "position"}; std::vector stop_modes_ = {false, false}; - - std::array position_command_ = {{0.0, 0.0}}; - std::array velocity_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; - std::array velocity_state_ = {{0.0, 0.0}}; - std::array acceleration_state_ = {{0.0, 0.0}}; }; } // namespace test_hardware_components diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index fe06a64223..5e612abe39 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -68,25 +68,33 @@ class TestTwoJointSystem : public SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; + std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + state_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return state_interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; + std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + hardware_interface::InterfaceInfo info; + info.initial_value = "0.0"; + + info.name = hardware_interface::HW_IF_POSITION; + command_interfaces.push_back( + hardware_interface::InterfaceDescription(info_.joints[i].name, info)); } return command_interfaces; @@ -101,10 +109,6 @@ class TestTwoJointSystem : public SystemInterface { return return_type::OK; } - -private: - std::array position_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; }; } // namespace test_hardware_components diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index a01ccd879f..c020f57e54 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -45,42 +45,29 @@ class TestActuator : public ActuatorInterface * CallbackReturn::ERROR;} */ - return CallbackReturn::SUCCESS; - } + pos_state_ = info_.joints[0].name + "/position"; + vel_state_ = info_.joints[0].name + "/velocity"; + vel_command_ = info_.joints[0].name + "/velocity"; - std::vector export_state_interfaces() override - { - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[0].name, "some_unlisted_interface", nullptr)); - - return state_interfaces; + return CallbackReturn::SUCCESS; } - std::vector export_command_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_)); - - if (info_.joints[0].command_interfaces.size() > 1) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_)); - } + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); + interfaces.push_back(unlisted_state_interface); - return command_interfaces; + return interfaces; } hardware_interface::return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { - position_state_ += 1.0; + set_state(pos_state_, get_state(pos_state_) + 1.0); return hardware_interface::return_type::OK; } @@ -88,22 +75,22 @@ class TestActuator : public ActuatorInterface const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) override { - position_state_ += 100.0; + set_state(pos_state_, get_state(pos_state_) + 100.0); return hardware_interface::return_type::OK; } return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_ == test_constants::READ_FAIL_VALUE) + if (get_command(vel_command_) == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + set_command(vel_command_, 0.0); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + if (get_command(vel_command_) == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -112,22 +99,22 @@ class TestActuator : public ActuatorInterface // working as it should. This makes value checks clearer and confirms there // is no "state = command" line or some other mixture of interfaces // somewhere in the test stack. - velocity_state_ = velocity_command_ / 2; + set_state(vel_state_, get_command(vel_command_) / 2); return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) + if (get_command(vel_command_) == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_ = 0.0; + set_command(vel_command_, 0.0); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + if (get_command(vel_command_) == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -135,10 +122,9 @@ class TestActuator : public ActuatorInterface } private: - double position_state_ = 0.0; - double velocity_state_ = 0.0; - double velocity_command_ = 0.0; - double max_velocity_command_ = 0.0; + std::string pos_state_; + std::string vel_state_; + std::string vel_command_; }; class TestUninitializableActuator : public TestActuator diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 2ea36ac5c1..254d2cec11 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -37,22 +37,10 @@ class TestSensor : public SensorInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override - { - std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_)); - - return state_interfaces; - } - return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { return return_type::OK; } - -private: - double velocity_state_ = 0.0; }; class TestUninitializableSensor : public TestSensor diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 20e606ee6d..d6d0cad190 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -27,64 +27,18 @@ using hardware_interface::SystemInterface; class TestSystem : public SystemInterface { - std::vector export_state_interfaces() override - { - std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); - } - - if (info_.gpios.size() > 0) - { - // Add configuration/max_tcp_jerk interface - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); - } - - return state_interfaces; - } - - std::vector export_command_interfaces() override - { - std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); - } - // Add max_acceleration command interface - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, - &max_acceleration_command_)); - - if (info_.gpios.size() > 0) - { - // Add configuration/max_tcp_jerk interface - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); - } - - return command_interfaces; - } - return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + set_command(info_.joints[0].name + "/velocity", 0.0); return return_type::ERROR; } // simulate deactivate on read - if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::READ_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -94,15 +48,15 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM // tests - velocity_command_[0] = 0.0; + set_command(info_.joints[0].name + "/velocity", 0.0); return return_type::ERROR; } // simulate deactivate on write - if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + if (get_command(info_.joints[0].name + "/velocity") == test_constants::WRITE_DEACTIVATE_VALUE) { return return_type::DEACTIVATE; } @@ -110,10 +64,6 @@ class TestSystem : public SystemInterface } private: - std::array velocity_command_ = {{0.0, 0.0}}; - std::array position_state_ = {{0.0, 0.0}}; - std::array velocity_state_ = {{0.0, 0.0}}; - std::array acceleration_state_ = {{0.0, 0.0}}; double max_acceleration_command_ = 0.0; double configuration_state_ = 0.0; double configuration_command_ = 0.0; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index c8b077229e..5bafdb7864 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -309,22 +309,26 @@ TEST_F(ResourceManagerTest, resource_claiming) class ExternalComponent : public hardware_interface::ActuatorInterface { - std::vector export_state_interfaces() override + std::vector export_state_interfaces_2() override { - std::vector state_interfaces; - state_interfaces.emplace_back( - hardware_interface::StateInterface("external_joint", "external_state_interface", nullptr)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "external_state_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface("external_joint", info); + interfaces.push_back(unlisted_state_interface); - return state_interfaces; + return interfaces; } - std::vector export_command_interfaces() override + std::vector export_command_interfaces_2() override { - std::vector command_interfaces; - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "external_joint", "external_command_interface", nullptr)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "external_command_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface("external_joint", info); + interfaces.push_back(unlisted_state_interface); - return command_interfaces; + return interfaces; } std::string get_name() const override { return "ExternalComponent"; } @@ -1191,8 +1195,11 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( - CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); + hardware_interface::InterfaceInfo info; + info.name = REFERENCE_INTERFACE_NAMES[i]; + info.initial_value = std::to_string(reference_interface_values[i]); + hardware_interface::InterfaceDescription ref_interface(CONTROLLER_NAME, info); + reference_interfaces.push_back(hardware_interface::CommandInterface(ref_interface)); } rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces); diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index dd58e55744..64652f7802 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -60,10 +60,9 @@ std::vector get_ordered_handles( { std::copy_if( unordered_handles.cbegin(), unordered_handles.cend(), std::back_inserter(result), - [&](const auto & handle) - { + [&](const auto & handle) { return (handle.get_prefix_name() == name) && - (handle.get_interface_name() == interface_type) && handle; + (handle.get_interface_name() == interface_type); }); } return result; diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 53a084fe76..5df12b8352 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -261,8 +261,6 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value( (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + @@ -277,8 +275,6 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value( (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0])); @@ -291,8 +287,6 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value( jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); joint_eff[1].set_value( @@ -309,8 +303,6 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - double joints_offset_applied[2] = { joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; @@ -324,8 +316,6 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value( (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]); @@ -338,8 +328,6 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value( (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0])); diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 8b5fdd5433..e2ebcbeab6 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -259,8 +259,6 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); joint_pos[1].set_value( (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / @@ -273,8 +271,6 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); joint_vel[1].set_value( (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / @@ -286,8 +282,6 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( jr[1] * @@ -305,8 +299,6 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { - assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); - double joints_offset_applied[2] = { joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; @@ -319,8 +311,6 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { - assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); - act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); act_vel[1].set_value( (joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); @@ -331,8 +321,6 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { - assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); - act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); act_eff[1].set_value( (joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index b240c1e489..91638060a2 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -128,13 +128,13 @@ class SimpleTransmission : public Transmission double reduction_; double jnt_offset_; - JointHandle joint_position_ = {"", "", nullptr}; - JointHandle joint_velocity_ = {"", "", nullptr}; - JointHandle joint_effort_ = {"", "", nullptr}; + JointHandle joint_position_(std::string(), std::string()); + JointHandle joint_velocity_(std::string(), std::string()); + JointHandle joint_effort_(std::string(), std::string()); - ActuatorHandle actuator_position_ = {"", "", nullptr}; - ActuatorHandle actuator_velocity_ = {"", "", nullptr}; - ActuatorHandle actuator_effort_ = {"", "", nullptr}; + ActuatorHandle actuator_position_(std::string(), std::string()); + ActuatorHandle actuator_velocity_(std::string(), std::string()); + ActuatorHandle actuator_effort_(std::string(), std::string()); }; inline SimpleTransmission::SimpleTransmission( diff --git a/transmission_interface/test/utils_test.cpp b/transmission_interface/test/utils_test.cpp index 9afaccaa47..808faa20d7 100644 --- a/transmission_interface/test/utils_test.cpp +++ b/transmission_interface/test/utils_test.cpp @@ -25,8 +25,11 @@ using transmission_interface::JointHandle; TEST(UtilsTest, AccessorTest) { const std::string NAME = "joint"; - double joint_value = 0.0; - const JointHandle joint_handle(NAME, HW_IF_POSITION, &joint_value); + hardware_interface::InterfaceInfo info; + info.name = HW_IF_POSITION; + info.initial_value = "0.0"; + hardware_interface::InterfaceDescription joint_handle_desc(NAME, info); + const JointHandle joint_handle(joint_handle_desc); const std::vector joint_handles = {joint_handle}; ASSERT_EQ(transmission_interface::get_names(joint_handles), std::vector{NAME}); From d552d3648fe2c76c4656b59bbdc6cafc43eaf6a2 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 15 Apr 2024 15:03:16 +0200 Subject: [PATCH 30/35] continuer updaeting tests --- .../chainable_controller_interface.hpp | 4 +- .../test/test_force_torque_sensor.cpp | 122 ++++++--- controller_interface/test/test_imu_sensor.cpp | 77 ++++-- .../test_semantic_component_interface.cpp | 20 +- .../include/hardware_interface/handle.hpp | 30 ++- .../hardware_interface/hardware_info.hpp | 19 ++ .../types/handle_datatype.hpp | 8 +- .../src/mock_components/generic_system.cpp | 8 +- hardware_interface/test/test_handle.cpp | 237 +++++++++++++++++- .../transmission_interface/accessor.hpp | 5 +- .../differential_transmission.hpp | 12 + .../four_bar_linkage_transmission.hpp | 12 + .../simple_transmission.hpp | 17 +- .../test/differential_transmission_test.cpp | 193 +++++++++----- .../four_bar_linkage_transmission_test.cpp | 125 ++++----- 15 files changed, 675 insertions(+), 214 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2bdccefdc5..ea5d062fb7 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -114,8 +114,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for reference interfaces - std::vector reference_interfaces_; + /// Storage of reference Interface + std::unordered_map reference_interfaces_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/test/test_force_torque_sensor.cpp b/controller_interface/test/test_force_torque_sensor.cpp index dd1e55e126..e60ec5c987 100644 --- a/controller_interface/test/test_force_torque_sensor.cpp +++ b/controller_interface/test/test_force_torque_sensor.cpp @@ -47,20 +47,42 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_y; + fts_info_y.name = fts_interface_names_[1]; + fts_info_y.initial_value = std::to_string(force_values_[1]); + hardware_interface::InterfaceDescription fts_descr_y(sensor_name_, fts_info_y); + hardware_interface::StateInterface force_y{fts_descr_y}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_x; + trq_info_x.name = fts_interface_names_[3]; + trq_info_x.initial_value = std::to_string(torque_values_[0]); + hardware_interface::InterfaceDescription torque_descr_x(sensor_name_, trq_info_x); + hardware_interface::StateInterface torque_x{torque_descr_x}; + + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; @@ -131,16 +153,30 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force.x and force.z - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque.y and torque.z - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; @@ -213,20 +249,42 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names) ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z"); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_y; + fts_info_y.name = fts_interface_names_[1]; + fts_info_y.initial_value = std::to_string(force_values_[1]); + hardware_interface::InterfaceDescription fts_descr_y(sensor_name_, fts_info_y); + hardware_interface::StateInterface force_y{fts_descr_y}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_x; + trq_info_x.name = fts_interface_names_[3]; + trq_info_x.initial_value = std::to_string(torque_values_[0]); + hardware_interface::InterfaceDescription torque_descr_x(sensor_name_, trq_info_x); + hardware_interface::StateInterface torque_x{torque_descr_x}; + + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_imu_sensor.cpp b/controller_interface/test/test_imu_sensor.cpp index 1fbf205238..ca0b10b251 100644 --- a/controller_interface/test/test_imu_sensor.cpp +++ b/controller_interface/test/test_imu_sensor.cpp @@ -46,30 +46,67 @@ TEST_F(IMUSensorTest, validate_all) std::vector interface_names = imu_sensor_->get_state_interface_names(); // assign values to orientation - hardware_interface::StateInterface orientation_x{ - sensor_name_, imu_interface_names_[0], &orientation_values_[0]}; - hardware_interface::StateInterface orientation_y{ - sensor_name_, imu_interface_names_[1], &orientation_values_[1]}; - hardware_interface::StateInterface orientation_z{ - sensor_name_, imu_interface_names_[2], &orientation_values_[2]}; - hardware_interface::StateInterface orientation_w{ - sensor_name_, imu_interface_names_[3], &orientation_values_[4]}; + hardware_interface::InterfaceInfo orientation_x_info; + orientation_x_info.name = imu_interface_names_[0]; + orientation_x_info.initial_value = std::to_string(orientation_values_[0]); + hardware_interface::InterfaceDescription orientation_x_descr(sensor_name_, orientation_x_info); + hardware_interface::StateInterface orientation_x{orientation_x_descr}; + + hardware_interface::InterfaceInfo orientation_y_info; + orientation_y_info.name = imu_interface_names_[1]; + orientation_y_info.initial_value = std::to_string(orientation_values_[1]); + hardware_interface::InterfaceDescription orientation_y_descr(sensor_name_, orientation_y_info); + hardware_interface::StateInterface orientation_y{orientation_y_descr}; + + hardware_interface::InterfaceInfo orientation_z_info; + orientation_z_info.name = imu_interface_names_[2]; + orientation_z_info.initial_value = std::to_string(orientation_values_[2]); + hardware_interface::InterfaceDescription orientation_z_descr(sensor_name_, orientation_z_info); + hardware_interface::StateInterface orientation_z{orientation_z_descr}; + + hardware_interface::InterfaceInfo orientation_w_info; + orientation_w_info.name = imu_interface_names_[3]; + orientation_w_info.initial_value = std::to_string(orientation_values_[3]); + hardware_interface::InterfaceDescription orientation_w_descr(sensor_name_, orientation_w_info); + hardware_interface::StateInterface orientation_w{orientation_w_descr}; // assign values to angular velocity - hardware_interface::StateInterface angular_velocity_x{ - sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]}; - hardware_interface::StateInterface angular_velocity_y{ - sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]}; - hardware_interface::StateInterface angular_velocity_z{ - sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]}; + hardware_interface::InterfaceInfo angular_x_info; + angular_x_info.name = imu_interface_names_[4]; + angular_x_info.initial_value = std::to_string(angular_velocity_values_[0]); + hardware_interface::InterfaceDescription angular_x_descr(sensor_name_, angular_x_info); + hardware_interface::StateInterface angular_velocity_x{angular_x_descr}; + + hardware_interface::InterfaceInfo angular_y_info; + angular_y_info.name = imu_interface_names_[5]; + angular_y_info.initial_value = std::to_string(angular_velocity_values_[1]); + hardware_interface::InterfaceDescription angular_y_descr(sensor_name_, angular_y_info); + hardware_interface::StateInterface angular_velocity_y{angular_y_descr}; + + hardware_interface::InterfaceInfo angular_z_info; + angular_z_info.name = imu_interface_names_[6]; + angular_z_info.initial_value = std::to_string(angular_velocity_values_[2]); + hardware_interface::InterfaceDescription angular_z_descr(sensor_name_, angular_z_info); + hardware_interface::StateInterface angular_velocity_z{angular_z_descr}; // assign values to linear acceleration - hardware_interface::StateInterface linear_acceleration_x{ - sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]}; - hardware_interface::StateInterface linear_acceleration_y{ - sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]}; - hardware_interface::StateInterface linear_acceleration_z{ - sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]}; + hardware_interface::InterfaceInfo linear_x_info; + linear_x_info.name = imu_interface_names_[7]; + linear_x_info.initial_value = std::to_string(linear_acceleration_values_[0]); + hardware_interface::InterfaceDescription linear_x_descr(sensor_name_, linear_x_info); + hardware_interface::StateInterface linear_acceleration_x{linear_x_descr}; + + hardware_interface::InterfaceInfo linear_y_info; + linear_y_info.name = imu_interface_names_[8]; + linear_y_info.initial_value = std::to_string(linear_acceleration_values_[1]); + hardware_interface::InterfaceDescription linear_y_descr(sensor_name_, linear_y_info); + hardware_interface::StateInterface linear_acceleration_y{linear_y_descr}; + + hardware_interface::InterfaceInfo linear_z_info; + linear_z_info.name = imu_interface_names_[9]; + linear_z_info.initial_value = std::to_string(linear_acceleration_values_[2]); + hardware_interface::InterfaceDescription linear_z_descr(sensor_name_, linear_z_info); + hardware_interface::StateInterface linear_acceleration_z{linear_z_descr}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_semantic_component_interface.cpp b/controller_interface/test/test_semantic_component_interface.cpp index f81b4f64fe..a1b0e90cdf 100644 --- a/controller_interface/test/test_semantic_component_interface.cpp +++ b/controller_interface/test/test_semantic_component_interface.cpp @@ -94,9 +94,23 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces) std::vector interface_values = {1.1, 3.3, 5.5}; // assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5 - hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]}; - hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]}; - hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]}; + hardware_interface::InterfaceInfo info_1; + info_1.name = "1"; + info_1.initial_value = std::to_string(interface_values[0]); + hardware_interface::InterfaceDescription interface_1_decr(component_name_, info_1); + hardware_interface::StateInterface interface_1{interface_1_decr}; + + hardware_interface::InterfaceInfo info_3; + info_3.name = "3"; + info_3.initial_value = std::to_string(interface_values[1]); + hardware_interface::InterfaceDescription interface_3_decr(component_name_, info_3); + hardware_interface::StateInterface interface_3{interface_3_decr}; + + hardware_interface::InterfaceInfo info_5; + info_5.name = "5"; + info_5.initial_value = std::to_string(interface_values[2]); + hardware_interface::InterfaceDescription interface_5_decr(component_name_, info_5); + hardware_interface::StateInterface interface_5{interface_5_decr}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 471258b759..68e991e94a 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -34,16 +34,6 @@ namespace hardware_interface class Handle { public: - [[deprecated("Use InterfaceDescription for initializing the Interface")]] - - Handle(const std::string & prefix_name, const std::string & interface_name) - : prefix_name_(prefix_name), interface_name_(interface_name) - { - // init to default value defined by init_handle_value() - InterfaceInfo info; - init_handle_value(info); - } - explicit Handle(const InterfaceDescription & interface_description) : prefix_name_(interface_description.prefix_name), interface_name_(interface_description.interface_info.name) @@ -51,10 +41,22 @@ class Handle init_handle_value(interface_description.interface_info); } + Handle() = delete; + Handle(const Handle & other) = default; Handle(Handle && other) = default; + /// Returns true if handle is valid. We define valid if the handle has a non empty prefix name and + /// a non empty interface name. This allows us to construct empty non valid default Handles which + /// can later be assigned e.g. in Transmissions + inline operator bool() const + { + return !( + prefix_name_.empty() || interface_name_.empty() || + std::holds_alternative(value_)); + } + Handle & operator=(const Handle & other) = default; Handle & operator=(Handle && other) = default; @@ -141,12 +143,16 @@ class Handle value_ = std::vector(hardware_interface::warning_signal_count, ""); } - // Default for empty is double - else if (interface_info.data_type.empty() || interface_info.data_type == "double") + else if (interface_info.data_type == "double") { value_ = interface_info.initial_value.empty() ? std::numeric_limits::quiet_NaN() : std::stod(interface_info.initial_value); } + // Default for empty is std::monostate + else if (interface_info.data_type.empty()) + { + value_ = {std::monostate()}; + } // If not empty and it belongs to none of the above types, we still want to throw as there might // be a typo in the data_type like "bol" or user wants some unsupported type else diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index dd757ffaeb..ac735d0f97 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -38,6 +38,19 @@ struct InterfaceInfo data_type = ""; size = 0; } + + explicit InterfaceInfo(const std::string & name_in) : InterfaceInfo() { name = name_in; } + + explicit InterfaceInfo( + const std::string & name_in, const std::string & initial_value_in, + const std::string & data_type_in) + : InterfaceInfo() + { + name = name_in; + initial_value = initial_value_in; + data_type = data_type_in; + } + /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -144,6 +157,12 @@ struct InterfaceDescription { } + explicit InterfaceDescription( + const std::string & prefix_name_in, const std::string & interface_info_name) + : prefix_name(prefix_name_in), interface_info(interface_info_name) + { + } + /** * Name of the interface defined by the user. */ diff --git a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp index 6a4d32c3c1..bcfa774f66 100644 --- a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp +++ b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp @@ -26,8 +26,12 @@ namespace hardware_interface { -using HANDLE_DATATYPE = - std::variant, std::vector, std::vector>; +// std::monostate is that we have a well defined alternative empty default initialization +// !!! IF YOU ADD TYPES TO HANDLE_DATATYPE, std::monostate MUST ALWAYS REMAIN AT THE FIRST POSITION +// This i needed so that e.g.: HANDLE_DATATYPE our_variant = {}; -> defaults to std::monostate +using HANDLE_DATATYPE = std::variant< + std::monostate, bool, double, std::vector, std::vector, + std::vector>; // Define a type trait for allowed types template diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 9d8ab9e582..a3fc0f97a3 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -457,7 +457,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur if (calculate_dynamics_) { - for (const auto joint_name : std_joint_names_) + for (const auto & joint_name : std_joint_names_) { const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; const auto joint_vel = joint_name + "/" + hardware_interface::HW_IF_VELOCITY; @@ -532,7 +532,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } else { - for (const auto joint_name : std_joint_names_) + for (const auto & joint_name : std_joint_names_) { const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; if ( @@ -568,9 +568,9 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur mimic_joint.joint_name, get_state(mimic_joint.mimic_joint_name) * mimic_joint.multiplier); } - for (const auto joint_name : other_joint_names_) + for (const auto & joint_name : other_joint_names_) { - for (const auto interface_name : non_standard_interfaces_) + for (const auto & interface_name : non_standard_interfaces_) { const auto joint_inteface = joint_name + "/" + interface_name; const auto joint_pos = joint_name + "/" + hardware_interface::HW_IF_POSITION; diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 52007fde5a..8184a6210f 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -31,6 +31,194 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace +TEST(TestHandle, ci_empty_handle_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, true); +} + +TEST(TestHandle, si_empty_handle_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, true); +} + +TEST(TestHandle, ci_empty_handle_prefix_empty_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_empty_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_inteface_name_empty_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_inteface_name_empty_value) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, ci_empty_handle_prefix_and_inteface_name) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_and_inteface_name) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_prefix_and_inteface_name_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_and_inteface_name_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_prefix) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_interface_name) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_interface_name) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + TEST(TestHandle, ci_empty_bool_initialization) { InterfaceInfo info; @@ -39,6 +227,7 @@ TEST(TestHandle, ci_empty_bool_initialization) InterfaceDescription descr(JOINT_NAME, info); CommandInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), false); EXPECT_NO_THROW(interface.set_value(true)); EXPECT_EQ(interface.get_value(), true); @@ -53,6 +242,7 @@ TEST(TestHandle, ci_true_bool_initialization) InterfaceDescription descr(JOINT_NAME, info); CommandInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), true); EXPECT_NO_THROW(interface.set_value(false)); EXPECT_EQ(interface.get_value(), false); @@ -66,6 +256,7 @@ TEST(TestHandle, ci_empty_double_initialization) InterfaceDescription descr(JOINT_NAME, info); CommandInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_TRUE(std::isnan(interface.get_value())); EXPECT_NO_THROW(interface.set_value(1.5)); EXPECT_EQ(interface.get_value(), 1.5); @@ -80,6 +271,7 @@ TEST(TestHandle, ci_pos_double_initialization) InterfaceDescription descr(JOINT_NAME, info); CommandInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), 1.5); EXPECT_NO_THROW(interface.set_value(0.0)); EXPECT_EQ(interface.get_value(), 0.0); @@ -94,6 +286,7 @@ TEST(TestHandle, ci_negative_double_initialization) InterfaceDescription descr(JOINT_NAME, info); CommandInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), -1.5); EXPECT_NO_THROW(interface.set_value(0.0)); EXPECT_EQ(interface.get_value(), 0.0); @@ -147,6 +340,7 @@ TEST(TestHandle, ci_int8_t_vector_correct_size_initialization) std::vector zero_vector(hardware_interface::warning_signal_count, 0); + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value>(), zero_vector); zero_vector[1] = 1; zero_vector[2] = 2; @@ -193,6 +387,7 @@ TEST(TestHandle, ci_uint8_t_vector_correct_size_initialization) std::vector zero_vector(hardware_interface::warning_signal_count, 0); + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value>(), zero_vector); zero_vector[1] = 1; zero_vector[2] = 2; @@ -239,6 +434,7 @@ TEST(TestHandle, ci_string_vector_correct_size_initialization) std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value>(), empty_str_vector); empty_str_vector[1] = "Warning message number 1"; empty_str_vector[2] = "Warn msg no. 2"; @@ -246,10 +442,11 @@ TEST(TestHandle, ci_string_vector_correct_size_initialization) empty_str_vector[8] = "Warning message no. 4"; EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value>(), empty_str_vector); } -TEST(TestHandle, ci_throw_on_get_wrong_type) +TEST(TestHandle, ci_throw_on_get_wrong_type_bool_from_int8_vec) { InterfaceInfo info; info.name = FOO_INTERFACE; @@ -273,7 +470,7 @@ TEST(TestHandle, ci_throw_on_not_know_type) EXPECT_THROW(CommandInterface interface{descr};, std::runtime_error); } -TEST(TestHandle, ci_throw_on_empty_type) +TEST(TestHandle, ci_throw_on_get_double_from_monostate) { InterfaceInfo info; info.name = FOO_INTERFACE; @@ -281,7 +478,8 @@ TEST(TestHandle, ci_throw_on_empty_type) InterfaceDescription descr(JOINT_NAME, info); CommandInterface interface{descr}; - EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_EQ(interface, false); + EXPECT_THROW(interface.get_value(), std::bad_variant_access); } TEST(TestHandle, si_empty_bool_initialization) @@ -292,6 +490,7 @@ TEST(TestHandle, si_empty_bool_initialization) InterfaceDescription descr(JOINT_NAME, info); StateInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), false); EXPECT_NO_THROW(interface.set_value(true)); EXPECT_EQ(interface.get_value(), true); @@ -306,6 +505,7 @@ TEST(TestHandle, si_true_bool_initialization) InterfaceDescription descr(JOINT_NAME, info); StateInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), true); EXPECT_NO_THROW(interface.set_value(false)); EXPECT_EQ(interface.get_value(), false); @@ -319,6 +519,7 @@ TEST(TestHandle, si_empty_double_initialization) InterfaceDescription descr(JOINT_NAME, info); StateInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_TRUE(std::isnan(interface.get_value())); EXPECT_NO_THROW(interface.set_value(1.5)); EXPECT_EQ(interface.get_value(), 1.5); @@ -333,6 +534,7 @@ TEST(TestHandle, si_pos_double_initialization) InterfaceDescription descr(JOINT_NAME, info); StateInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), 1.5); EXPECT_NO_THROW(interface.set_value(0.0)); EXPECT_EQ(interface.get_value(), 0.0); @@ -347,6 +549,7 @@ TEST(TestHandle, si_negative_double_initialization) InterfaceDescription descr(JOINT_NAME, info); StateInterface interface{descr}; + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value(), -1.5); EXPECT_NO_THROW(interface.set_value(0.0)); EXPECT_EQ(interface.get_value(), 0.0); @@ -400,6 +603,7 @@ TEST(TestHandle, si_int8_t_vector_correct_size_initialization) std::vector zero_vector(hardware_interface::warning_signal_count, 0); + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value>(), zero_vector); zero_vector[1] = 1; zero_vector[2] = 2; @@ -446,6 +650,7 @@ TEST(TestHandle, si_uint8_t_vector_correct_size_initialization) std::vector zero_vector(hardware_interface::warning_signal_count, 0); + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value>(), zero_vector); zero_vector[1] = 1; zero_vector[2] = 2; @@ -492,6 +697,7 @@ TEST(TestHandle, si_string_vector_correct_size_initialization) std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + EXPECT_EQ(interface, true); EXPECT_EQ(interface.get_value>(), empty_str_vector); empty_str_vector[1] = "Warning message number 1"; empty_str_vector[2] = "Warn msg no. 2"; @@ -539,7 +745,13 @@ TEST(TestHandle, si_throw_on_empty_type) TEST(TestHandle, name_getters_work) { - StateInterface handle{JOINT_NAME, FOO_INTERFACE}; + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + StateInterface handle{descr}; + + EXPECT_EQ(handle, false); EXPECT_EQ(handle.get_name(), std::string(JOINT_NAME) + "/" + std::string(FOO_INTERFACE)); EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME); @@ -547,7 +759,13 @@ TEST(TestHandle, name_getters_work) TEST(TestHandle, ci_value_methods) { - CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + CommandInterface handle{descr}; + + EXPECT_EQ(handle, true); EXPECT_NO_THROW(handle.get_value()); EXPECT_TRUE(std::isnan(handle.get_value())); EXPECT_NO_THROW(handle.set_value(0.0)); @@ -556,7 +774,12 @@ TEST(TestHandle, ci_value_methods) TEST(TestHandle, si_value_methods) { - StateInterface handle{JOINT_NAME, FOO_INTERFACE}; + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + StateInterface handle{descr}; + + EXPECT_EQ(handle, true); EXPECT_NO_THROW(handle.get_value()); EXPECT_TRUE(std::isnan(handle.get_value())); EXPECT_NO_THROW(handle.set_value(0.0)); @@ -573,6 +796,7 @@ TEST(TestHandle, interface_description_state_interface_name_getters_work) InterfaceDescription interface_descr(JOINT_NAME_1, info); StateInterface handle{interface_descr}; + EXPECT_EQ(handle, true); EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); (handle.get_prefix_name(), JOINT_NAME_1); @@ -588,6 +812,7 @@ TEST(TestHandle, interface_description_command_interface_name_getters_work) InterfaceDescription interface_descr(JOINT_NAME_1, info); CommandInterface handle{interface_descr}; + EXPECT_EQ(handle, true); EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index 64652f7802..dd58e55744 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -60,9 +60,10 @@ std::vector get_ordered_handles( { std::copy_if( unordered_handles.cbegin(), unordered_handles.cend(), std::back_inserter(result), - [&](const auto & handle) { + [&](const auto & handle) + { return (handle.get_prefix_name() == name) && - (handle.get_interface_name() == interface_type); + (handle.get_interface_name() == interface_type) && handle; }); } return result; diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 5df12b8352..53a084fe76 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -261,6 +261,8 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { + assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + joint_pos[0].set_value( (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) + @@ -275,6 +277,8 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { + assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + joint_vel[0].set_value( (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0])); @@ -287,6 +291,8 @@ inline void DifferentialTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { + assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + joint_eff[0].set_value( jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1])); joint_eff[1].set_value( @@ -303,6 +309,8 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { + assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + double joints_offset_applied[2] = { joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; @@ -316,6 +324,8 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { + assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + act_vel[0].set_value( (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]); @@ -328,6 +338,8 @@ inline void DifferentialTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { + assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + act_eff[0].set_value( (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0])); diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index e2ebcbeab6..8b5fdd5433 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -259,6 +259,8 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { + assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]); joint_pos[1].set_value( (act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / @@ -271,6 +273,8 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { + assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0])); joint_vel[1].set_value( (act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / @@ -282,6 +286,8 @@ inline void FourBarLinkageTransmission::actuator_to_joint() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { + assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( jr[1] * @@ -299,6 +305,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_pos = joint_position_; if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints()) { + assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]); + double joints_offset_applied[2] = { joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]}; @@ -311,6 +319,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_vel = joint_velocity_; if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints()) { + assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]); + act_vel[0].set_value(joint_vel[0].get_value() * jr[0] * ar[0]); act_vel[1].set_value( (joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]); @@ -321,6 +331,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator() auto & joint_eff = joint_effort_; if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints()) { + assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]); + act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0])); act_eff[1].set_value( (joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]); diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 91638060a2..229a2bbdb3 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -20,6 +20,7 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "transmission_interface/exception.hpp" #include "transmission_interface/transmission.hpp" @@ -128,13 +129,13 @@ class SimpleTransmission : public Transmission double reduction_; double jnt_offset_; - JointHandle joint_position_(std::string(), std::string()); - JointHandle joint_velocity_(std::string(), std::string()); - JointHandle joint_effort_(std::string(), std::string()); + JointHandle joint_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + JointHandle joint_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + JointHandle joint_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; - ActuatorHandle actuator_position_(std::string(), std::string()); - ActuatorHandle actuator_velocity_(std::string(), std::string()); - ActuatorHandle actuator_effort_(std::string(), std::string()); + ActuatorHandle actuator_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + ActuatorHandle actuator_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + ActuatorHandle actuator_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; }; inline SimpleTransmission::SimpleTransmission( @@ -156,7 +157,9 @@ HandleType get_by_interface( [&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; }); if (result == handles.cend()) { - return HandleType(handles.cbegin()->get_prefix_name(), interface_name, nullptr); + // return new Handle where data is set to std::monotype by default + return HandleType(hardware_interface::InterfaceDescription( + handles.cbegin()->get_prefix_name(), interface_name)); } return *result; } diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 14ffe968bc..fb0d06b9a3 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -23,6 +23,8 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using testing::DoubleNear; using transmission_interface::ActuatorHandle; using transmission_interface::DifferentialTransmission; @@ -92,14 +94,22 @@ void testConfigureWithBadHandles(std::string interface_name) DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0}); double dummy; - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + auto a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a2_handle = + ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a3_handle = + ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j3_handle = + JointHandle(InterfaceDescription("joint3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -133,8 +143,6 @@ class TransmissionSetup : public ::testing::Test // Input/output transmission data double a_val[2]; double j_val[2]; - std::vector a_vec = {&a_val[0], &a_val[1]}; - std::vector j_vec = {&j_val[0], &j_val[1]}; }; /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -153,11 +161,16 @@ class BlackBoxTest : public TransmissionSetup // set actuator values to reference a_val[0] = ref_val[0]; a_val[1] = ref_val[1]; + // create handles and configure - auto a1_handle = ActuatorHandle("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(interface_name, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(interface_name, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(interface_name, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(interface_name, std::to_string(j_val[1]), "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip @@ -237,15 +250,19 @@ TEST_F(WhiteBoxTest, DontMoveJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 0.0; - *a_vec[1] = 0.0; + a_val[0] = 0.0; + a_val[1] = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -254,10 +271,18 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -266,10 +291,18 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); @@ -290,10 +323,18 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); @@ -302,10 +343,18 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); @@ -314,10 +363,18 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); @@ -338,10 +395,18 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); + auto joint1_handle = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); + auto joint2_handle = JointHandle(InterfaceDescription( + "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -350,10 +415,10 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -362,10 +427,10 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -391,10 +456,10 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); @@ -403,10 +468,10 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(-0.01250, DoubleNear(j_val[0], EPS)); @@ -415,10 +480,10 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS)); diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index f74e4def6a..cc76f656a5 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -25,6 +25,8 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using testing::DoubleNear; using testing::Not; using transmission_interface::ActuatorHandle; @@ -96,14 +98,16 @@ void testConfigureWithBadHandles(std::string interface_name) FourBarLinkageTransmission trans({1.0, 1.0}, {1.0, 1.0}); double dummy; - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(interface_name))); + auto a3_handle = ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(interface_name))); + auto j1_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); + auto j2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name))); + auto j3_handle = JointHandle(InterfaceDescription("joint3", InterfaceInfo(interface_name))); + auto invalid_a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); + auto invalid_j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -158,10 +162,11 @@ class BlackBoxTest : public TransmissionSetup a_val[0] = ref_val[0]; a_val[1] = ref_val[1]; // create handles and configure - auto a1_handle = ActuatorHandle("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(interface_name, std::to_string(a_val[0]), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(interface_name a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip @@ -246,10 +251,10 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -258,10 +263,10 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -270,10 +275,10 @@ TEST_F(WhiteBoxTest, DontMoveJoints) // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); @@ -292,10 +297,10 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) { a_val[0] = 5.0; a_val[1] = 10.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS)); @@ -306,10 +311,10 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) { a_val[0] = 10.0; a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); @@ -320,10 +325,10 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) { a_val[0] = 10.0; a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); @@ -344,10 +349,10 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -356,10 +361,10 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -368,10 +373,10 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); @@ -397,10 +402,10 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(-60.0, DoubleNear(j_val[0], EPS)); @@ -409,10 +414,10 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(-0.15, DoubleNear(j_val[0], EPS)); @@ -421,10 +426,10 @@ TEST_F(WhiteBoxTest, MoveBothJoints) // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); + auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); EXPECT_THAT(-2.15, DoubleNear(j_val[0], EPS)); From 87b4ae3d13505a0c0a0cefde1dbb42bf213e4758 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 22 Apr 2024 11:16:08 +0200 Subject: [PATCH 31/35] add changes for chainable controllers (epxport of rerference interfaces) --- .../chainable_controller_interface.hpp | 14 ++++- .../controller_interface.hpp | 3 +- .../controller_interface_base.hpp | 3 +- .../src/chainable_controller_interface.cpp | 59 ++++++++++++------- .../src/controller_interface.cpp | 3 +- .../test_chainable_controller_interface.cpp | 10 ++-- controller_manager/src/controller_manager.cpp | 26 ++++++-- .../hardware_interface/resource_manager.hpp | 3 +- hardware_interface/src/resource_manager.cpp | 9 +-- .../test/test_resource_manager.cpp | 5 +- 10 files changed, 91 insertions(+), 44 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index ea5d062fb7..a40a7a4088 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,9 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include +#include +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -56,7 +59,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector> export_reference_interfaces() + final; CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; @@ -114,8 +118,12 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of reference Interface - std::unordered_map reference_interfaces_; + /// Storage of values for reference interfaces + // BEGIN (Handle export change): for backward compatibility + std::vector reference_interfaces_; + // END + std::unordered_map> + reference_interfaces_ptrs_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index a3d006755f..9614ab46a5 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -48,7 +48,8 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_reference_interfaces() final; + std::vector> export_reference_interfaces() + final; /** * Controller is not chainable, therefore no chained mode can be set. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 182ffa7fec..3f75d7e3c0 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -222,7 +222,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of command interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_reference_interfaces() = 0; + virtual std::vector> + export_reference_interfaces() = 0; /** * Set chained mode of a chainable controller. This method triggers internal processes to switch diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 2f7c67741e..7497f6d5a6 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,44 +44,63 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector +std::vector> ChainableControllerInterface::export_reference_interfaces() { auto reference_interfaces = on_export_reference_interfaces(); + std::vector> reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + // BEGIN (Handle export change): for backward compatibility // check if the "reference_interfaces_" variable is resized to number of interfaces if (reference_interfaces_.size() != reference_interfaces.size()) { // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the // framework - RCLCPP_FATAL( - get_node()->get_logger(), - "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', " - "but it is expected to have the size '%zu' equal to the number of exported reference " - "interfaces. No reference interface will be exported. Please correct and recompile " - "the controller with name '%s' and try again.", - reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name()); - reference_interfaces.clear(); + std::string error_msg = + "The internal storage for reference values 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); } + // END // check if the names of the reference interfaces begin with the controller's name - for (const auto & interface : reference_interfaces) + const auto ref_interface_size = reference_interfaces.size(); + for (auto & interface : reference_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory " - " for reference interfaces. No reference interface will be exported. Please correct and " - "recompile the controller with name '%s' and try again.", - interface.get_name().c_str(), get_node()->get_name()); - reference_interfaces.clear(); - break; + std::string error_msg = "The name of the interface " + interface.get_name() + + " does not begin with the controller's name. This is mandatory for " + "reference interfaces. Please " + "correct and recompile the controller with name " + + get_node()->get_name() + " and try again."; + throw std::runtime_error(error_msg); } + + std::shared_ptr interface_ptr = + std::make_shared(std::move(interface)); + reference_interfaces_ptrs_vec.push_back(interface_ptr); + reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr)); + } + + if (reference_interfaces_ptrs_.size() != ref_interface_size) + { + std::string error_msg = + "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + + std::to_string(reference_interfaces_ptrs_.size()) + + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); } - return reference_interfaces; + return reference_interfaces_ptrs_vec; } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 392a48ffa4..7e7f563bd4 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -28,7 +28,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_reference_interfaces() +std::vector> +ControllerInterface::export_reference_interfaces() { return {}; } diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 33959e7fd9..ef951f376c 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -45,10 +45,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), 1u); - EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); + EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) @@ -103,7 +103,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); @@ -112,7 +112,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Success setting chained mode - reference_interfaces[0].set_value(0.0); + reference_interfaces[0]->set_value(0.0); EXPECT_TRUE(controller.set_chained_mode(true)); EXPECT_TRUE(controller.is_in_chained_mode()); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index eadd1b0d78..c48b0e53ac 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -686,13 +686,27 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - auto interfaces = controller->export_reference_interfaces(); - if (interfaces.empty()) + + std::vector> interfaces; + try { - // TODO(destogl): Add test for this! - RCLCPP_ERROR( - get_logger(), "Controller '%s' is chainable, but does not export any reference interfaces.", - controller_name.c_str()); + interfaces = controller->export_reference_interfaces(); + if (interfaces.empty()) + { + // TODO(destogl): Add test for this! + RCLCPP_ERROR( + get_logger(), + "Controller '%s' is chainable, but does not export any reference interfaces. Did you " + "override the on_export_method() correctly?", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + } + catch (const std::runtime_error & e) + { + RCLCPP_FATAL( + get_logger(), "Creation of the reference interfaces failed with following error: %s", + e.what()); return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 743e548a4c..a113d2db17 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -145,7 +145,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces. */ void import_controller_reference_interfaces( - const std::string & controller_name, std::vector & interfaces); + const std::string & controller_name, + const std::vector> & interfaces); /// Get list of reference interface of a controller. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4453ae22e5..0fa1c15e5d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -456,7 +456,7 @@ class ResourceStorage available_state_interfaces_.capacity() + interface_names.size()); } - void insert_command_interface(std::shared_ptr command_interface) + void insert_command_interface(const std::shared_ptr command_interface) { const auto [it, success] = command_interface_map_.insert( std::make_pair(command_interface->get_name(), command_interface)); @@ -524,11 +524,11 @@ class ResourceStorage } std::vector add_command_interfaces( - std::vector> & interfaces) + const std::vector> & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + for (const auto & interface : interfaces) { auto key = interface->get_name(); insert_command_interface(interface); @@ -903,7 +903,8 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, + const std::vector> & interfaces) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); auto interface_names = resource_storage_->add_command_interfaces(interfaces); diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5bafdb7864..3f1bfcbb24 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1190,7 +1190,7 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; - std::vector reference_interfaces; + std::vector> reference_interfaces; std::vector reference_interface_values = {1.0, 2.0, 3.0}; for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) @@ -1199,7 +1199,8 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) info.name = REFERENCE_INTERFACE_NAMES[i]; info.initial_value = std::to_string(reference_interface_values[i]); hardware_interface::InterfaceDescription ref_interface(CONTROLLER_NAME, info); - reference_interfaces.push_back(hardware_interface::CommandInterface(ref_interface)); + reference_interfaces.push_back( + std::make_shared(ref_interface)); } rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces); From 1324028e06ca843acef73bcfc5fd731034df1843 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 22 May 2024 09:51:20 +0200 Subject: [PATCH 32/35] continue adapting tests --- .../chainable_controller_interface.hpp | 2 + .../src/chainable_controller_interface.cpp | 18 +- .../test_chainable_controller_interface.cpp | 4 +- .../test_chainable_controller_interface.hpp | 38 ++- .../test_chainable_controller.cpp | 10 +- .../include/hardware_interface/handle.hpp | 26 ++ .../hardware_interface/hardware_info.hpp | 7 + .../types/handle_datatype.hpp | 7 +- .../test/differential_transmission_test.cpp | 295 +++++++++--------- .../four_bar_linkage_transmission_test.cpp | 246 +++++++++------ .../test/simple_transmission_test.cpp | 91 +++--- 11 files changed, 430 insertions(+), 314 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index a40a7a4088..131307c324 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,7 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include #include #include #include @@ -121,6 +122,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// Storage of values for reference interfaces // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; + std::unordered_map> ref_interface_to_value_; // END std::unordered_map> reference_interfaces_ptrs_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 7497f6d5a6..de9db31443 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -70,8 +70,9 @@ ChainableControllerInterface::export_reference_interfaces() // check if the names of the reference interfaces begin with the controller's name const auto ref_interface_size = reference_interfaces.size(); - for (auto & interface : reference_interfaces) + for (size_t i = 0; i < reference_interfaces.size(); ++i) { + auto & interface = reference_interfaces[i]; if (interface.get_prefix_name() != get_node()->get_name()) { std::string error_msg = "The name of the interface " + interface.get_name() + @@ -84,8 +85,21 @@ ChainableControllerInterface::export_reference_interfaces() std::shared_ptr interface_ptr = std::make_shared(std::move(interface)); - reference_interfaces_ptrs_vec.push_back(interface_ptr); + if ( + reference_interfaces_ptrs_.find(interface_ptr->get_name()) != + reference_interfaces_ptrs_.end()) + { + std::string error_msg = "The controller " + std::string(get_node()->get_name()) + + "exports reference the reference interface with the name:'" + + interface_ptr->get_name() + + "' twice. Names of interfaces have to be unique"; + throw std::runtime_error(error_msg); + } reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr)); + // BEGIN (Handle export change): for backward compatibility + ref_interface_to_value_.insert({interface_ptr->get_name(), std::ref(reference_interfaces_[i])}); + // END + reference_interfaces_ptrs_vec.push_back(interface_ptr); } if (reference_interfaces_ptrs_.size() != ref_interface_size) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index ef951f376c..fd8bb12cdd 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -48,7 +48,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf"); - EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) @@ -103,7 +103,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) EXPECT_FALSE(controller.is_in_chained_mode()); // Fail setting chained mode - EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); + EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE); EXPECT_FALSE(controller.set_chained_mode(true)); EXPECT_FALSE(controller.is_in_chained_mode()); diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp index 28401f13d5..c004751746 100644 --- a/controller_interface/test/test_chainable_controller_interface.hpp +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -22,6 +22,7 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller"; @@ -30,6 +31,9 @@ constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; + class TestableChainableControllerInterface : public controller_interface::ChainableControllerInterface { @@ -37,11 +41,7 @@ class TestableChainableControllerInterface FRIEND_TEST(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size); FRIEND_TEST(ChainableControllerInterfaceTest, test_update_logic); - TestableChainableControllerInterface() - { - reference_interfaces_.reserve(1); - reference_interfaces_.push_back(INTERFACE_VALUE); - } + TestableChainableControllerInterface() { update_ref_itf_full_name(); } controller_interface::CallbackReturn on_init() override { @@ -68,15 +68,16 @@ class TestableChainableControllerInterface { std::vector command_interfaces; - command_interfaces.push_back(hardware_interface::CommandInterface( - name_prefix_of_reference_interfaces_, "test_itf", &reference_interfaces_[0])); + command_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription( + name_prefix_of_reference_interfaces_, + InterfaceInfo(ref_itf_name_, std::to_string(INTERFACE_VALUE), "double")))); return command_interfaces; } bool on_set_chained_mode(bool /*chained_mode*/) override { - if (reference_interfaces_[0] == 0.0) + if (reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == 0.0) { return true; } @@ -89,24 +90,28 @@ class TestableChainableControllerInterface controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (reference_interface_value_ == INTERFACE_VALUE_SUBSCRIBER_ERROR) + if ( + reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == + INTERFACE_VALUE_SUBSCRIBER_ERROR) { return controller_interface::return_type::ERROR; } - reference_interfaces_[0] = reference_interface_value_; + reference_interfaces_ptrs_[ref_itf_full_name_]->set_value(reference_interface_value_); return controller_interface::return_type::OK; } controller_interface::return_type update_and_write_commands( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { - if (reference_interfaces_[0] == INTERFACE_VALUE_UPDATE_ERROR) + if ( + reference_interfaces_ptrs_[ref_itf_full_name_]->get_value() == + INTERFACE_VALUE_UPDATE_ERROR) { return controller_interface::return_type::ERROR; } - reference_interfaces_[0] -= 1; + reference_interfaces_ptrs_[ref_itf_full_name_]->operator-=(1); return controller_interface::return_type::OK; } @@ -114,14 +119,23 @@ class TestableChainableControllerInterface void set_name_prefix_of_reference_interfaces(const std::string & prefix) { name_prefix_of_reference_interfaces_ = prefix; + update_ref_itf_full_name(); } void set_new_reference_interface_value(const double ref_itf_value) { reference_interface_value_ = ref_itf_value; + update_ref_itf_full_name(); + } + + void update_ref_itf_full_name() + { + ref_itf_full_name_ = name_prefix_of_reference_interfaces_ + "/" + ref_itf_name_; } std::string name_prefix_of_reference_interfaces_; + std::string ref_itf_name_ = "test_itf"; + std::string ref_itf_full_name_; double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF; }; diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index d21957a0b4..529ffe618a 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -18,10 +18,16 @@ #include #include +#include "hardware_interface/hardware_info.hpp" + #include "lifecycle_msgs/msg/state.hpp" namespace test_chainable_controller { + +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; + TestChainableController::TestChainableController() : controller_interface::ChainableControllerInterface(), cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE}, @@ -157,8 +163,8 @@ TestChainableController::on_export_reference_interfaces() for (size_t i = 0; i < reference_interface_names_.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); + reference_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription( + get_node()->get_name(), InterfaceInfo(reference_interface_names_[i], "double")))); } return reference_interfaces; diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 68e991e94a..97f4c3f167 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -57,6 +57,28 @@ class Handle std::holds_alternative(value_)); } + template < + typename T, typename std::enable_if< + std::is_integral::value || std::is_floating_point::value, int>::type = 0> + void operator+=(const T & value) + { + value_ = std::get(value_) + value; + } + + template < + typename T, typename std::enable_if< + std::is_integral::value || std::is_floating_point::value, int>::type = 0> + void operator-=(const T & value) + { + value_ = std::get(value_) - value; + } + + template ::value, int>::type = 0> + void operator=(const T & value) + { + value_ = value; + } + Handle & operator=(const Handle & other) = default; Handle & operator=(Handle && other) = default; @@ -103,6 +125,10 @@ class Handle : (interface_info.initial_value == "true" || interface_info.initial_value == "True"); } + else if (interface_info.data_type == "int") + { + value_ = interface_info.initial_value.empty() ? 0 : std::stoi(interface_info.initial_value); + } else if (interface_info.data_type == "vector") { if ( diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index ac735d0f97..83a9ac6dff 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -41,6 +41,13 @@ struct InterfaceInfo explicit InterfaceInfo(const std::string & name_in) : InterfaceInfo() { name = name_in; } + explicit InterfaceInfo(const std::string & name_in, const std::string & data_type_in) + : InterfaceInfo() + { + name = name_in; + data_type = data_type_in; + } + explicit InterfaceInfo( const std::string & name_in, const std::string & initial_value_in, const std::string & data_type_in) diff --git a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp index bcfa774f66..0b84f6262f 100644 --- a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp +++ b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp @@ -30,15 +30,16 @@ namespace hardware_interface // !!! IF YOU ADD TYPES TO HANDLE_DATATYPE, std::monostate MUST ALWAYS REMAIN AT THE FIRST POSITION // This i needed so that e.g.: HANDLE_DATATYPE our_variant = {}; -> defaults to std::monostate using HANDLE_DATATYPE = std::variant< - std::monostate, bool, double, std::vector, std::vector, + std::monostate, bool, int, double, std::vector, std::vector, std::vector>; // Define a type trait for allowed types template struct HANDLE_DATATYPE_TYPES : std::disjunction< - std::is_same, std::is_same, std::is_same>, - std::is_same>, std::is_same>> + std::is_same, std::is_same, std::is_same, + std::is_same>, std::is_same>, + std::is_same>> { }; diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index fb0d06b9a3..8c3c01e30c 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -139,10 +139,6 @@ TEST(ConfigureTest, FailsWithBadHandles) class TransmissionSetup : public ::testing::Test { -protected: - // Input/output transmission data - double a_val[2]; - double j_val[2]; }; /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -159,28 +155,29 @@ class BlackBoxTest : public TransmissionSetup const std::string & interface_name) { // set actuator values to reference - a_val[0] = ref_val[0]; - a_val[1] = ref_val[1]; + auto a_val = ref_val[0]; + auto a_val_1 = ref_val[1]; // create handles and configure - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(interface_name, std::to_string(a_val[0]), "double"))); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(interface_name, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(interface_name, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(interface_name, std::to_string(j_val[1]), "double"))); + "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle.set_value(EXTREMAL_VALUE); + a2_handle.set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(ref_val[0], DoubleNear(a1_handle.get_value(), EPS)); + EXPECT_THAT(ref_val[1], DoubleNear(a2_handle.get_value(), EPS)); } // Generate a set of transmission instances @@ -250,63 +247,57 @@ TEST_F(WhiteBoxTest, DontMoveJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - a_val[0] = 0.0; - a_val[1] = 0.0; + auto a_val = 0.0; + auto a_val_1 = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -318,67 +309,58 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = 10.0; + auto a_val = 10.0; + auto a_val_1 = 10.0; // Effort interface { - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(400.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -390,51 +372,56 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = -10.0; + auto a_val = 10.0; + auto a_val_1 = -10.0; // Effort interface { - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription( - "act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val[1]), "double"))); - auto joint1_handle = JointHandle(InterfaceDescription( - "joint1", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[0]), "double"))); - auto joint2_handle = JointHandle(InterfaceDescription( - "joint2", InterfaceInfo(HW_IF_EFFORT, std::to_string(j_val[1]), "double"))); - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(400.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -451,42 +438,54 @@ TEST_F(WhiteBoxTest, MoveBothJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 3.0; - *a_vec[1] = 5.0; + auto a_val = 3.0; + auto a_val_1 = 5.0; // Effort interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(140.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(520.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.01250, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.06875, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.01250, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(4.06875, DoubleNear(joint2_handle.get_value(), EPS)); } } diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index cc76f656a5..b0d45d42df 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/transmission_interface/test/four_bar_linkage_transmission_test.cpp @@ -96,7 +96,6 @@ TEST(PreconditionsTest, AccessorValidation) void testConfigureWithBadHandles(std::string interface_name) { FourBarLinkageTransmission trans({1.0, 1.0}, {1.0, 1.0}); - double dummy; auto a1_handle = ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); auto a2_handle = ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(interface_name))); @@ -138,11 +137,6 @@ TEST(ConfigureTest, FailsWithBadHandles) class TransmissionSetup : public ::testing::Test { protected: - // Input/output transmission data - double a_val[2]; - double j_val[2]; - std::vector a_vec = {&a_val[0], &a_val[1]}; - std::vector j_vec = {&j_val[0], &j_val[1]}; }; /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -159,24 +153,28 @@ class BlackBoxTest : public TransmissionSetup const std::string & interface_name) { // set actuator values to reference - a_val[0] = ref_val[0]; - a_val[1] = ref_val[1]; + auto a_val = ref_val[0]; + auto a_val_1 = ref_val[1]; // create handles and configure - auto a1_handle = ActuatorHandle(InterfaceDescription( - "act1", InterfaceInfo(interface_name, std::to_string(a_val[0]), "double"))); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(interface_name a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle.set_value(EXTREMAL_VALUE); + a2_handle.set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(a_val, DoubleNear(a1_handle.get_value(), EPS)); + EXPECT_THAT(a_val_1, DoubleNear(a2_handle.get_value(), EPS)); } // Generate a set of transmission instances @@ -246,43 +244,55 @@ TEST_F(WhiteBoxTest, DontMoveJoints) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - a_val[0] = 0.0; - a_val[1] = 0.0; + auto a_val = 0.0; + auto a_val_1 = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -295,44 +305,56 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Effort interface { - a_val[0] = 5.0; - a_val[1] = 10.0; - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); + auto a_val = 5.0; + auto a_val_1 = 10.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(100.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a_val = 10.0; + auto a_val_1 = 5.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a_val = 10.0; + auto a_val_1 = 5.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -344,43 +366,55 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - a_val[0] = 0.0; - a_val[1] = 10.0; + auto a_val = 0.0; + auto a_val_1 = 10.0; // Effort interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(200.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -397,42 +431,54 @@ TEST_F(WhiteBoxTest, MoveBothJoints) FourBarLinkageTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - a_val[0] = 3.0; - a_val[1] = 5.0; + auto a_val = 3.0; + auto a_val_1 = 5.0; // Effort interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_EFFORT, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_EFFORT, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_EFFORT, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_EFFORT, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-60.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-160.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-60.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(-160.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_VELOCITY, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_VELOCITY, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_VELOCITY, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_VELOCITY, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-0.025, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.15, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(-0.025, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle(InterfaceDescription("act1", HW_IF_POSITION, a_vec[0])); - auto a2_handle = ActuatorHandle(InterfaceDescription("act2", HW_IF_POSITION, a_vec[1])); - auto joint1_handle = JointHandle(InterfaceDescription("joint1", HW_IF_POSITION, j_vec[0])); - auto joint2_handle = JointHandle(InterfaceDescription("joint2", HW_IF_POSITION, j_vec[1])); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(3.975, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.15, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(3.975, DoubleNear(joint2_handle.get_value(), EPS)); } } diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 33a14f92d1..026264e79b 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -15,12 +15,15 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "transmission_interface/simple_transmission.hpp" using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using std::vector; using transmission_interface::ActuatorHandle; using transmission_interface::Exception; @@ -62,12 +65,13 @@ TEST(PreconditionsTest, AccessorValidation) TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) { SimpleTransmission trans(2.0, -1.0); - double dummy; - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &dummy); - auto actuator2_handle = ActuatorHandle("act2", HW_IF_POSITION, &dummy); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &dummy); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, &dummy); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION))); + auto actuator2_handle = + ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(HW_IF_POSITION))); + auto joint_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION))); + auto joint2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION))); EXPECT_THROW(trans.configure({}, {}), transmission_interface::Exception); EXPECT_THROW(trans.configure({joint_handle}, {}), transmission_interface::Exception); @@ -80,8 +84,10 @@ TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) trans.configure({joint_handle, joint2_handle}, {actuator_handle}), transmission_interface::Exception); - auto invalid_actuator_handle = ActuatorHandle("act1", HW_IF_VELOCITY, nullptr); - auto invalid_joint_handle = JointHandle("joint1", HW_IF_VELOCITY, nullptr); + auto invalid_actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY))); + auto invalid_joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY))); EXPECT_THROW( trans.configure({invalid_joint_handle}, {invalid_actuator_handle}), transmission_interface::Exception); @@ -95,15 +101,6 @@ TEST(PreconditionsTest, ConfigureFailsWithInvalidHandles) class TransmissionSetup { protected: - // Input/output transmission data - double a_val = 0.0; - double j_val = 0.0; - - void reset_values() - { - a_val = 0.0; - j_val = 0.0; - } }; /// Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -121,15 +118,19 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam(), EPS)); + actuator_handle.set_value(0.0); + joint_handle.set_value(0.0); } } }; @@ -141,23 +142,15 @@ TEST_P(BlackBoxTest, IdentityMap) // Test transmission for positive, zero, and negative inputs testIdentityMap(trans, HW_IF_POSITION, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_POSITION, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_POSITION, -1.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_VELOCITY, -1.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, 1.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, 0.0); - reset_values(); testIdentityMap(trans, HW_IF_EFFORT, -1.0); } @@ -180,53 +173,61 @@ TEST_F(WhiteBoxTest, MoveJoint) SimpleTransmission trans(10.0, 1.0); - a_val = 1.0; - // Effort interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_EFFORT, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_EFFORT, &j_val); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, "1.0", "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(10.0, DoubleNear(j_val, EPS)); + EXPECT_THAT(10.0, DoubleNear(joint_handle.get_value(), EPS)); } // Velocity interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_VELOCITY, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_VELOCITY, &j_val); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, "1.0", "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.1, DoubleNear(j_val, EPS)); + EXPECT_THAT(0.1, DoubleNear(joint_handle.get_value(), EPS)); } // Position interface { - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &a_val); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &j_val); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "0.0", "double"))); trans.configure({joint_handle}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(1.1, DoubleNear(j_val, EPS)); + EXPECT_THAT(1.1, DoubleNear(joint_handle.get_value(), EPS)); } // Mismatched interface is ignored { double unique_value = 13.37; - auto actuator_handle = ActuatorHandle("act1", HW_IF_POSITION, &a_val); - auto actuator_handle2 = ActuatorHandle("act1", HW_IF_VELOCITY, &unique_value); - auto joint_handle = JointHandle("joint1", HW_IF_POSITION, &j_val); - auto joint_handle2 = JointHandle("joint1", HW_IF_VELOCITY, &unique_value); + auto actuator_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + auto actuator_handle2 = ActuatorHandle(InterfaceDescription( + "act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(unique_value), "double"))); + auto joint_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "1.0", "double"))); + auto joint_handle2 = JointHandle(InterfaceDescription( + "joint1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(unique_value), "double"))); trans.configure({joint_handle, joint_handle2}, {actuator_handle}); trans.actuator_to_joint(); - EXPECT_THAT(unique_value, DoubleNear(13.37, EPS)); + EXPECT_THAT(joint_handle.get_value(), DoubleNear(13.37, EPS)); trans.configure({joint_handle}, {actuator_handle, actuator_handle2}); trans.actuator_to_joint(); - EXPECT_THAT(unique_value, DoubleNear(13.37, EPS)); + EXPECT_THAT(joint_handle.get_value(), DoubleNear(13.37, EPS)); } } From 28d3ebd9b58fed292d4596bdee0d320ea060d23a Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 23 Aug 2024 13:22:11 +0200 Subject: [PATCH 33/35] add orderd vector of StateInterfaces and CommandInterfaces --- .../hardware_interface/actuator_interface.hpp | 19 ++++++++++-- .../hardware_interface/sensor_interface.hpp | 29 ++++++++++--------- .../hardware_interface/system_interface.hpp | 27 +++++++++++++++-- 3 files changed, 58 insertions(+), 17 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 47281f659b..df2a805d47 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -252,6 +252,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); actuator_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -259,6 +260,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { auto state_interface = std::make_shared(descr); actuator_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -336,6 +338,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod unlisted_command_interfaces_.insert(std::make_pair(name, description)); auto command_interface = std::make_shared(description); actuator_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -343,6 +346,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { auto command_interface = std::make_shared(descr); actuator_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -494,13 +498,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod protected: HardwareInfo info_; + + rclcpp_lifecycle::State lifecycle_state_; + // interface names to InterfaceDescription std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> joint_states_; + std::vector> joint_commands_; + + std::vector> unlisted_states_; + std::vector> unlisted_commands_; + private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger actuator_logger_; + // interface names to Handle accessed through getters/setters std::unordered_map> actuator_states_; std::unordered_map> actuator_commands_; @@ -509,8 +526,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::shared_ptr error_signal_message_; std::shared_ptr warning_signal_; std::shared_ptr warning_signal_message_; - - rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 545548104e..a103d2207c 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -227,7 +227,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto name = description.get_name(); unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); - sensor_states_.insert(std::make_pair(name, state_interface)); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -236,7 +237,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is // inserted auto state_interface = std::make_shared(descr); - sensor_states_.insert(std::make_pair(name, state_interface)); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -281,12 +283,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const std::string & interface_name, const double & value) { - sensor_states_.at(interface_name)->set_value(value); + sensor_states_map_.at(interface_name)->set_value(value); } double get_state(const std::string & interface_name) const { - return sensor_states_.at(interface_name)->get_value(); + return sensor_states_map_.at(interface_name)->get_value(); } void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } @@ -328,19 +330,20 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; - + rclcpp_lifecycle::State lifecycle_state_; + // interface names to InterfaceDescription std::unordered_map sensor_state_interfaces_; 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_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> sensor_states_; + std::vector> unlisted_states_; - rclcpp_lifecycle::State lifecycle_state_; +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger sensor_logger_; + // interface names to Handle accessed through getters/setters + std::unordered_map> sensor_states_map_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 399e59f3fb..1552b6c8e4 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -272,6 +272,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI unlisted_state_interfaces_.insert(std::make_pair(name, description)); auto state_interface = std::make_shared(description); system_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -279,18 +280,21 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : sensor_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } for (const auto & [name, descr] : gpio_state_interfaces_) { auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); + gpio_states_.push_back(state_interface); state_interfaces.push_back(state_interface); } @@ -369,6 +373,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI unlisted_command_interfaces_.insert(std::make_pair(name, description)); auto command_interface = std::make_shared(description); system_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -376,6 +381,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto command_interface = std::make_shared(descr); system_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } @@ -383,6 +389,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { auto command_interface = std::make_shared(descr); system_commands_.insert(std::make_pair(name, command_interface)); + gpio_commands_.push_back(command_interface); command_interfaces.push_back(command_interface); } return command_interfaces; @@ -533,6 +540,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI protected: HardwareInfo info_; + + rclcpp_lifecycle::State lifecycle_state_; + // interface names to InterfaceDescription std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; @@ -544,7 +554,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_state_interfaces_; std::unordered_map unlisted_command_interfaces_; + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> joint_states_; + std::vector> joint_commands_; + + std::vector> sensor_states_; + + std::vector> gpio_states_; + std::vector> gpio_commands_; + + std::vector> unlisted_states_; + std::vector> unlisted_commands_; + private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger system_logger_; + // interface names to Handle accessed through getters/setters std::unordered_map> system_states_; std::unordered_map> system_commands_; @@ -553,8 +578,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::shared_ptr error_signal_message_; std::shared_ptr warning_signal_; std::shared_ptr warning_signal_message_; - - rclcpp_lifecycle::State lifecycle_state_; }; } // namespace hardware_interface From bb6dd63173758a6013e764c479883e1a7a2382f3 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 23 Aug 2024 15:07:33 +0200 Subject: [PATCH 34/35] ordered refernce and exported stateinterfaecs --- .../chainable_controller_interface.hpp | 17 ++-- .../src/chainable_controller_interface.cpp | 86 ++++++++++++++++--- 2 files changed, 85 insertions(+), 18 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 131307c324..e24ad214b5 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -119,11 +119,18 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Storage of values for reference interfaces - // BEGIN (Handle export change): for backward compatibility - std::vector reference_interfaces_; - std::unordered_map> ref_interface_to_value_; - // END + /// Storage of values for state interfaces + std::vector exported_state_interface_names_; + // storage for the exported StateInterfaces + std::vector> + ordered_exported_state_interfaces_; + std::unordered_map> + exported_state_interfaces_; + + // interface_names are in order they have been exported + std::vector exported_reference_interface_names_; + // storage for the exported CommandInterfaces + std::vector> ordered_reference_interfaces_; std::unordered_map> reference_interfaces_ptrs_; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index de9db31443..761b816508 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,12 +44,64 @@ return_type ChainableControllerInterface::update( return ret; } +std::vector> +ChainableControllerInterface::export_state_interfaces() +{ + auto state_interfaces = on_export_state_interfaces(); + std::vector> state_interfaces_ptrs_vec; + state_interfaces_ptrs_vec.reserve(state_interfaces.size()); + ordered_exported_state_interfaces_.reserve(state_interfaces.size()); + exported_state_interface_names_.reserve(state_interfaces.size()); + + // check if the names of the controller state interfaces begin with the controller's name + for (const auto & interface : state_interfaces) + { + if (interface.get_prefix_name() != get_node()->get_name()) + { + std::string error_msg = + "The prefix of the interface '" + interface.get_prefix_name() + + "' does not equal the controller's name '" + get_node()->get_name() + + "'. This is mandatory for state interfaces. No state interface will be exported. Please " + "correct and recompile the controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + auto state_interface = std::make_shared(interface); + const auto interface_name = state_interface->get_name(); + auto [it, succ] = exported_state_interfaces_.insert({inteface_name, state_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert StateInterface<" + inteface_name + + "> into exported_state_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + exported_state_interfaces_.clear(); + exported_state_interface_names_.clear(); + state_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); + } + ordered_exported_state_interfaces_.push_back(state_interface); + exported_state_interface_names_.push_back(interface_name); + state_interfaces_ptrs_vec.push_back(state_interface); + } + + return state_interfaces_ptrs_vec; +} + std::vector> ChainableControllerInterface::export_reference_interfaces() { auto reference_interfaces = on_export_reference_interfaces(); std::vector> reference_interfaces_ptrs_vec; reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + exported_reference_interface_names_.reserve(reference_interfaces.size()); + ordered_reference_interfaces_.reserve(reference_interfaces.size()); // BEGIN (Handle export change): for backward compatibility // check if the "reference_interfaces_" variable is resized to number of interfaces @@ -83,23 +135,31 @@ ChainableControllerInterface::export_reference_interfaces() throw std::runtime_error(error_msg); } - std::shared_ptr interface_ptr = + std::shared_ptr reference_interface = std::make_shared(std::move(interface)); - if ( - reference_interfaces_ptrs_.find(interface_ptr->get_name()) != - reference_interfaces_ptrs_.end()) + const auto inteface_name = reference_interface->get_name(); + // check the exported interface name is unique + auto [it, succ] = reference_interfaces_ptrs_.insert({inteface_name, reference_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) { - std::string error_msg = "The controller " + std::string(get_node()->get_name()) + - "exports reference the reference interface with the name:'" + - interface_ptr->get_name() + - "' twice. Names of interfaces have to be unique"; + std::string error_msg = + "Could not insert Reference interface<" + inteface_name + + "> into reference_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + reference_interfaces_.clear(); + exported_reference_interface_names_.clear(); + reference_interfaces_ptrs_vec.clear(); throw std::runtime_error(error_msg); } - reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr)); - // BEGIN (Handle export change): for backward compatibility - ref_interface_to_value_.insert({interface_ptr->get_name(), std::ref(reference_interfaces_[i])}); - // END - reference_interfaces_ptrs_vec.push_back(interface_ptr); + ordered_reference_interfaces_.push_back(reference_interface); + exported_reference_interface_names_.push_back(inteface_name); + reference_interfaces_ptrs_vec.push_back(reference_interface); } if (reference_interfaces_ptrs_.size() != ref_interface_size) From cfdeacdb83ab715624b0b886deb6ad85fafd5bd2 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 26 Aug 2024 11:47:31 +0200 Subject: [PATCH 35/35] fixup cherry-pick --- .../chainable_controller_interface.hpp | 2 +- .../src/chainable_controller_interface.cpp | 120 ++++++++++-------- 2 files changed, 68 insertions(+), 54 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index e24ad214b5..1fbb8ec810 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -121,7 +121,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// Storage of values for state interfaces std::vector exported_state_interface_names_; - // storage for the exported StateInterfaces + // storage for the exported CommandInterfaces std::vector> ordered_exported_state_interfaces_; std::unordered_map> diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 761b816508..8115197c06 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -47,27 +47,30 @@ return_type ChainableControllerInterface::update( std::vector> ChainableControllerInterface::export_state_interfaces() { - auto state_interfaces = on_export_state_interfaces(); + auto state_interfaces_descr = export_state_interface_descriptions(); std::vector> state_interfaces_ptrs_vec; - state_interfaces_ptrs_vec.reserve(state_interfaces.size()); - ordered_exported_state_interfaces_.reserve(state_interfaces.size()); - exported_state_interface_names_.reserve(state_interfaces.size()); + state_interfaces_ptrs_vec.reserve(state_interfaces_descr.size()); + exported_state_interface_names_.reserve(state_interfaces_descr.size()); + ordered_exported_state_interfaces_.reserve(state_interfaces_descr.size()); + exported_state_interfaces_.reserve(state_interfaces_descr.size()); // check if the names of the controller state interfaces begin with the controller's name - for (const auto & interface : state_interfaces) + for (auto & descr : state_interfaces_descr) { - if (interface.get_prefix_name() != get_node()->get_name()) + if (descr.prefix_name != get_node()->get_name()) { std::string error_msg = - "The prefix of the interface '" + interface.get_prefix_name() + + "The prefix of the interface description'" + descr.prefix_name + "' does not equal the controller's name '" + get_node()->get_name() + "'. This is mandatory for state interfaces. No state interface will be exported. Please " "correct and recompile the controller with name '" + get_node()->get_name() + "' and try again."; throw std::runtime_error(error_msg); } - auto state_interface = std::make_shared(interface); - const auto interface_name = state_interface->get_name(); + + auto state_interface = std::make_shared(descr); + const auto inteface_name = state_interface->get_name(); + // check the exported interface name is unique auto [it, succ] = exported_state_interfaces_.insert({inteface_name, state_interface}); // either we have name duplicate which we want to avoid under all circumstances since interfaces // need to be uniquely identify able or something else really went wrong. In any case abort and @@ -81,14 +84,35 @@ ChainableControllerInterface::export_state_interfaces() it->second->get_name() + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " "interface names are unique."; - exported_state_interfaces_.clear(); - exported_state_interface_names_.clear(); state_interfaces_ptrs_vec.clear(); + exported_state_interface_names_.clear(); + ordered_exported_state_interfaces_.clear(); + exported_state_interfaces_.clear(); throw std::runtime_error(error_msg); } - ordered_exported_state_interfaces_.push_back(state_interface); - exported_state_interface_names_.push_back(interface_name); state_interfaces_ptrs_vec.push_back(state_interface); + exported_state_interface_names_.push_back(inteface_name); + ordered_exported_state_interfaces_.push_back(state_interface); + } + + // check that all are equal + if ( + state_interfaces_descr.size() != state_interfaces_ptrs_vec.size() || + state_interfaces_descr.size() != exported_state_interface_names_.size() || + state_interfaces_descr.size() != ordered_exported_state_interfaces_.size() || + state_interfaces_descr.size() != exported_state_interfaces_.size()) + { + std::string error_msg = + "The size of the exported StateInterfaceDescriptions (" + + std::to_string(state_interfaces_descr.size()) + ") of controller <" + get_node()->get_name() + + "> does not match with the size of one of the following: state_interfaces_ptrs_vec (" + + std::to_string(state_interfaces_ptrs_vec.size()) + "), exported_state_interface_names_ (" + + std::to_string(exported_state_interface_names_.size()) + + "), ordered_exported_state_interfaces_ (" + + std::to_string(ordered_exported_state_interfaces_.size()) + + ") or exported_state_interfaces_ (" + std::to_string(exported_state_interfaces_.size()) + + ")."; + throw std::runtime_error(error_msg); } return state_interfaces_ptrs_vec; @@ -97,37 +121,19 @@ ChainableControllerInterface::export_state_interfaces() std::vector> ChainableControllerInterface::export_reference_interfaces() { - auto reference_interfaces = on_export_reference_interfaces(); + auto reference_interface_descr = export_reference_interface_descriptions(); std::vector> reference_interfaces_ptrs_vec; - reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); - exported_reference_interface_names_.reserve(reference_interfaces.size()); - ordered_reference_interfaces_.reserve(reference_interfaces.size()); - - // BEGIN (Handle export change): for backward compatibility - // check if the "reference_interfaces_" variable is resized to number of interfaces - if (reference_interfaces_.size() != reference_interfaces.size()) - { - // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the - // framework - std::string error_msg = - "The internal storage for reference values 'reference_interfaces_' variable has size '" + - std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + - std::to_string(reference_interfaces.size()) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; - throw std::runtime_error(error_msg); - } - // END + reference_interfaces_ptrs_vec.reserve(reference_interface_descr.size()); + exported_reference_interface_names_.reserve(reference_interface_descr.size()); + ordered_reference_interfaces_.reserve(reference_interface_descr.size()); + reference_interfaces_ptrs_.reserve(reference_interface_descr.size()); // check if the names of the reference interfaces begin with the controller's name - const auto ref_interface_size = reference_interfaces.size(); - for (size_t i = 0; i < reference_interfaces.size(); ++i) + for (auto & descr : reference_interface_descr) { - auto & interface = reference_interfaces[i]; - if (interface.get_prefix_name() != get_node()->get_name()) + if (descr.prefix_name != get_node()->get_name()) { - std::string error_msg = "The name of the interface " + interface.get_name() + + std::string error_msg = "The name of the interface descr " + descr.prefix_name + " does not begin with the controller's name. This is mandatory for " "reference interfaces. Please " "correct and recompile the controller with name " + @@ -135,11 +141,10 @@ ChainableControllerInterface::export_reference_interfaces() throw std::runtime_error(error_msg); } - std::shared_ptr reference_interface = - std::make_shared(std::move(interface)); + auto reference_interface = std::make_shared(descr); const auto inteface_name = reference_interface->get_name(); // check the exported interface name is unique - auto [it, succ] = reference_interfaces_ptrs_.insert({inteface_name, reference_interface}); + auto [it, succ] = reference_interfaces_.insert({inteface_name, reference_interface}); // either we have name duplicate which we want to avoid under all circumstances since interfaces // need to be uniquely identify able or something else really went wrong. In any case abort and // inform cm by throwing exception @@ -152,25 +157,34 @@ ChainableControllerInterface::export_reference_interfaces() it->second->get_name() + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " "interface names are unique."; - reference_interfaces_.clear(); - exported_reference_interface_names_.clear(); reference_interfaces_ptrs_vec.clear(); + exported_reference_interface_names_.clear(); + ordered_reference_interfaces_.clear(); + reference_interfaces_ptrs_.clear(); throw std::runtime_error(error_msg); } - ordered_reference_interfaces_.push_back(reference_interface); - exported_reference_interface_names_.push_back(inteface_name); reference_interfaces_ptrs_vec.push_back(reference_interface); + exported_reference_interface_names_.push_back(inteface_name); + ordered_reference_interfaces_.push_back(reference_interface); } - if (reference_interfaces_ptrs_.size() != ref_interface_size) + if ( + reference_interface_descr.size() != reference_interfaces_ptrs_vec.size() || + reference_interface_descr.size() != exported_reference_interface_names_.size() || + reference_interface_descr.size() != ordered_reference_interfaces_.size() || + reference_interface_descr.size() != reference_interfaces_ptrs_.size()) { std::string error_msg = - "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + - std::to_string(reference_interfaces_ptrs_.size()) + - "', but it is expected to have the size '" + std::to_string(ref_interface_size) + - "' equal to the number of exported reference interfaces. Please correct and recompile the " - "controller with name '" + - get_node()->get_name() + "' and try again."; + "The size of the exported ReferenceInterfaceDescriptions (" + + std::to_string(reference_interface_descr.size()) + ") of controller <" + + get_node()->get_name() + + "> does not match with the size of one of the following: reference_interfaces_ptrs_vec (" + + std::to_string(reference_interfaces_ptrs_vec.size()) + + "), exported_reference_interface_names_ (" + + std::to_string(exported_reference_interface_names_.size()) + + "), ordered_reference_interfaces_ (" + std::to_string(ordered_reference_interfaces_.size()) + + ") or reference_interfaces_ptrs_ (" + std::to_string(reference_interfaces_ptrs_.size()) + + ")."; throw std::runtime_error(error_msg); }