diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2bdccefdc5..1fbb8ec810 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,6 +15,10 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include +#include +#include +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -56,7 +60,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 +119,20 @@ 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 values for state interfaces + std::vector exported_state_interface_names_; + // storage for the exported CommandInterfaces + 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_; 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..8115197c06 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,44 +44,151 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector -ChainableControllerInterface::export_reference_interfaces() +std::vector> +ChainableControllerInterface::export_state_interfaces() { - auto reference_interfaces = on_export_reference_interfaces(); + auto state_interfaces_descr = export_state_interface_descriptions(); + std::vector> state_interfaces_ptrs_vec; + state_interfaces_ptrs_vec.reserve(state_interfaces_descr.size()); + exported_state_interface_names_.reserve(state_interfaces_descr.size()); + ordered_exported_state_interfaces_.reserve(state_interfaces_descr.size()); + exported_state_interfaces_.reserve(state_interfaces_descr.size()); + + // check if the names of the controller state interfaces begin with the controller's name + for (auto & descr : state_interfaces_descr) + { + if (descr.prefix_name != get_node()->get_name()) + { + std::string error_msg = + "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(descr); + const auto inteface_name = state_interface->get_name(); + // check the exported interface name is unique + auto [it, succ] = exported_state_interfaces_.insert({inteface_name, state_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert StateInterface<" + inteface_name + + "> into exported_state_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + 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); + } + 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 if the "reference_interfaces_" variable is resized to number of interfaces - if (reference_interfaces_.size() != reference_interfaces.size()) + // check that all are equal + if ( + state_interfaces_descr.size() != state_interfaces_ptrs_vec.size() || + state_interfaces_descr.size() != exported_state_interface_names_.size() || + state_interfaces_descr.size() != ordered_exported_state_interfaces_.size() || + state_interfaces_descr.size() != exported_state_interfaces_.size()) { - // 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 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; +} + +std::vector> +ChainableControllerInterface::export_reference_interfaces() +{ + auto reference_interface_descr = export_reference_interface_descriptions(); + std::vector> reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interface_descr.size()); + 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 - for (const auto & interface : reference_interfaces) + for (auto & descr : reference_interface_descr) { - 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 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 " + + get_node()->get_name() + " and try again."; + throw std::runtime_error(error_msg); + } + + auto reference_interface = std::make_shared(descr); + const auto inteface_name = reference_interface->get_name(); + // check the exported interface name is unique + auto [it, succ] = reference_interfaces_.insert({inteface_name, reference_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) { - 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 = + "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_ptrs_vec.clear(); + exported_reference_interface_names_.clear(); + ordered_reference_interfaces_.clear(); + reference_interfaces_ptrs_.clear(); + throw std::runtime_error(error_msg); } + 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_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 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); } - 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 47487e019c..fd8bb12cdd 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_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_interface/test/test_force_torque_sensor.cpp b/controller_interface/test/test_force_torque_sensor.cpp index dd1e55e126..e60ec5c987 100644 --- a/controller_interface/test/test_force_torque_sensor.cpp +++ b/controller_interface/test/test_force_torque_sensor.cpp @@ -47,20 +47,42 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_y; + fts_info_y.name = fts_interface_names_[1]; + fts_info_y.initial_value = std::to_string(force_values_[1]); + hardware_interface::InterfaceDescription fts_descr_y(sensor_name_, fts_info_y); + hardware_interface::StateInterface force_y{fts_descr_y}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_x; + trq_info_x.name = fts_interface_names_[3]; + trq_info_x.initial_value = std::to_string(torque_values_[0]); + hardware_interface::InterfaceDescription torque_descr_x(sensor_name_, trq_info_x); + hardware_interface::StateInterface torque_x{torque_descr_x}; + + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; @@ -131,16 +153,30 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force.x and force.z - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque.y and torque.z - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; @@ -213,20 +249,42 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names) ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z"); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + hardware_interface::InterfaceInfo fts_info_x; + fts_info_x.name = fts_interface_names_[0]; + fts_info_x.initial_value = std::to_string(force_values_[0]); + hardware_interface::InterfaceDescription fts_descr_x(sensor_name_, fts_info_x); + hardware_interface::StateInterface force_x{fts_descr_x}; + + hardware_interface::InterfaceInfo fts_info_y; + fts_info_y.name = fts_interface_names_[1]; + fts_info_y.initial_value = std::to_string(force_values_[1]); + hardware_interface::InterfaceDescription fts_descr_y(sensor_name_, fts_info_y); + hardware_interface::StateInterface force_y{fts_descr_y}; + + hardware_interface::InterfaceInfo fts_info_z; + fts_info_z.name = fts_interface_names_[2]; + fts_info_z.initial_value = std::to_string(force_values_[2]); + hardware_interface::InterfaceDescription fts_descr_z(sensor_name_, fts_info_z); + hardware_interface::StateInterface force_z{fts_descr_z}; // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + hardware_interface::InterfaceInfo trq_info_x; + trq_info_x.name = fts_interface_names_[3]; + trq_info_x.initial_value = std::to_string(torque_values_[0]); + hardware_interface::InterfaceDescription torque_descr_x(sensor_name_, trq_info_x); + hardware_interface::StateInterface torque_x{torque_descr_x}; + + hardware_interface::InterfaceInfo trq_info_y; + trq_info_y.name = fts_interface_names_[4]; + trq_info_y.initial_value = std::to_string(torque_values_[1]); + hardware_interface::InterfaceDescription torque_descr_y(sensor_name_, trq_info_y); + hardware_interface::StateInterface torque_y{torque_descr_y}; + + hardware_interface::InterfaceInfo trq_info_z; + trq_info_z.name = fts_interface_names_[5]; + trq_info_z.initial_value = std::to_string(torque_values_[2]); + hardware_interface::InterfaceDescription torque_descr_z(sensor_name_, trq_info_z); + hardware_interface::StateInterface torque_z{torque_descr_z}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_imu_sensor.cpp b/controller_interface/test/test_imu_sensor.cpp index 1fbf205238..ca0b10b251 100644 --- a/controller_interface/test/test_imu_sensor.cpp +++ b/controller_interface/test/test_imu_sensor.cpp @@ -46,30 +46,67 @@ TEST_F(IMUSensorTest, validate_all) std::vector interface_names = imu_sensor_->get_state_interface_names(); // assign values to orientation - hardware_interface::StateInterface orientation_x{ - sensor_name_, imu_interface_names_[0], &orientation_values_[0]}; - hardware_interface::StateInterface orientation_y{ - sensor_name_, imu_interface_names_[1], &orientation_values_[1]}; - hardware_interface::StateInterface orientation_z{ - sensor_name_, imu_interface_names_[2], &orientation_values_[2]}; - hardware_interface::StateInterface orientation_w{ - sensor_name_, imu_interface_names_[3], &orientation_values_[4]}; + hardware_interface::InterfaceInfo orientation_x_info; + orientation_x_info.name = imu_interface_names_[0]; + orientation_x_info.initial_value = std::to_string(orientation_values_[0]); + hardware_interface::InterfaceDescription orientation_x_descr(sensor_name_, orientation_x_info); + hardware_interface::StateInterface orientation_x{orientation_x_descr}; + + hardware_interface::InterfaceInfo orientation_y_info; + orientation_y_info.name = imu_interface_names_[1]; + orientation_y_info.initial_value = std::to_string(orientation_values_[1]); + hardware_interface::InterfaceDescription orientation_y_descr(sensor_name_, orientation_y_info); + hardware_interface::StateInterface orientation_y{orientation_y_descr}; + + hardware_interface::InterfaceInfo orientation_z_info; + orientation_z_info.name = imu_interface_names_[2]; + orientation_z_info.initial_value = std::to_string(orientation_values_[2]); + hardware_interface::InterfaceDescription orientation_z_descr(sensor_name_, orientation_z_info); + hardware_interface::StateInterface orientation_z{orientation_z_descr}; + + hardware_interface::InterfaceInfo orientation_w_info; + orientation_w_info.name = imu_interface_names_[3]; + orientation_w_info.initial_value = std::to_string(orientation_values_[3]); + hardware_interface::InterfaceDescription orientation_w_descr(sensor_name_, orientation_w_info); + hardware_interface::StateInterface orientation_w{orientation_w_descr}; // assign values to angular velocity - hardware_interface::StateInterface angular_velocity_x{ - sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]}; - hardware_interface::StateInterface angular_velocity_y{ - sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]}; - hardware_interface::StateInterface angular_velocity_z{ - sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]}; + hardware_interface::InterfaceInfo angular_x_info; + angular_x_info.name = imu_interface_names_[4]; + angular_x_info.initial_value = std::to_string(angular_velocity_values_[0]); + hardware_interface::InterfaceDescription angular_x_descr(sensor_name_, angular_x_info); + hardware_interface::StateInterface angular_velocity_x{angular_x_descr}; + + hardware_interface::InterfaceInfo angular_y_info; + angular_y_info.name = imu_interface_names_[5]; + angular_y_info.initial_value = std::to_string(angular_velocity_values_[1]); + hardware_interface::InterfaceDescription angular_y_descr(sensor_name_, angular_y_info); + hardware_interface::StateInterface angular_velocity_y{angular_y_descr}; + + hardware_interface::InterfaceInfo angular_z_info; + angular_z_info.name = imu_interface_names_[6]; + angular_z_info.initial_value = std::to_string(angular_velocity_values_[2]); + hardware_interface::InterfaceDescription angular_z_descr(sensor_name_, angular_z_info); + hardware_interface::StateInterface angular_velocity_z{angular_z_descr}; // assign values to linear acceleration - hardware_interface::StateInterface linear_acceleration_x{ - sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]}; - hardware_interface::StateInterface linear_acceleration_y{ - sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]}; - hardware_interface::StateInterface linear_acceleration_z{ - sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]}; + hardware_interface::InterfaceInfo linear_x_info; + linear_x_info.name = imu_interface_names_[7]; + linear_x_info.initial_value = std::to_string(linear_acceleration_values_[0]); + hardware_interface::InterfaceDescription linear_x_descr(sensor_name_, linear_x_info); + hardware_interface::StateInterface linear_acceleration_x{linear_x_descr}; + + hardware_interface::InterfaceInfo linear_y_info; + linear_y_info.name = imu_interface_names_[8]; + linear_y_info.initial_value = std::to_string(linear_acceleration_values_[1]); + hardware_interface::InterfaceDescription linear_y_descr(sensor_name_, linear_y_info); + hardware_interface::StateInterface linear_acceleration_y{linear_y_descr}; + + hardware_interface::InterfaceInfo linear_z_info; + linear_z_info.name = imu_interface_names_[9]; + linear_z_info.initial_value = std::to_string(linear_acceleration_values_[2]); + hardware_interface::InterfaceDescription linear_z_descr(sensor_name_, linear_z_info); + hardware_interface::StateInterface linear_acceleration_z{linear_z_descr}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_semantic_component_interface.cpp b/controller_interface/test/test_semantic_component_interface.cpp index f81b4f64fe..a1b0e90cdf 100644 --- a/controller_interface/test/test_semantic_component_interface.cpp +++ b/controller_interface/test/test_semantic_component_interface.cpp @@ -94,9 +94,23 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces) std::vector interface_values = {1.1, 3.3, 5.5}; // assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5 - hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]}; - hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]}; - hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]}; + hardware_interface::InterfaceInfo info_1; + info_1.name = "1"; + info_1.initial_value = std::to_string(interface_values[0]); + hardware_interface::InterfaceDescription interface_1_decr(component_name_, info_1); + hardware_interface::StateInterface interface_1{interface_1_decr}; + + hardware_interface::InterfaceInfo info_3; + info_3.name = "3"; + info_3.initial_value = std::to_string(interface_values[1]); + hardware_interface::InterfaceDescription interface_3_decr(component_name_, info_3); + hardware_interface::StateInterface interface_3{interface_3_decr}; + + hardware_interface::InterfaceInfo info_5; + info_5.name = "5"; + info_5.initial_value = std::to_string(interface_values[2]); + hardware_interface::InterfaceDescription interface_5_decr(component_name_, info_5); + hardware_interface::StateInterface interface_5{interface_5_decr}; // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_manager/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/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/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 4ea7fdc2f7..4367af5d53 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -76,6 +76,15 @@ 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_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_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) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 4082863370..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 abfd8eb45a..df2a805d47 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,13 +15,20 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ +#include #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_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" @@ -97,31 +104,254 @@ 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_); + create_report_interfaces(); 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) + { + auto joint_state_interface_descriptions = + 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)); + } + } + + /** + * 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) + { + auto joint_command_interface_descriptions = + 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)); + } + } + + /** + * 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 = "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 = "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); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + 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 = "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); + } + /// Exports all state interfaces for this hardware interface. /** - * 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 */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is " + "handled " + "by the Framework.")]] virtual std::vector + export_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() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector 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 + * 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() + { + // 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)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(state_interface); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + 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); + } + + // 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; + } /// Exports all command interfaces for this hardware 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. " + "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() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * Note method name is going to be changed to export_command_interfaces() as soon as the + * deprecated version is removed. + * + * \return vector of descriptions to the unlisted CommandInterfaces + */ + virtual std::vector 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 + * assigned here and resides in the actuator_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces */ - virtual std::vector export_command_interfaces() = 0; + virtual std::vector> on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_command_interfaces_2(); + + 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)); + unlisted_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + for (const auto & [name, descr] : joint_command_interfaces_) + { + 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); + } + + return command_interfaces; + } /// Prepare for a new command interface switch. /** * Prepare for any mode-switching required by the new command interface combination. @@ -202,9 +432,100 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + actuator_states_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return actuator_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + actuator_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return actuator_commands_.at(interface_name)->get_value(); + } + + void set_emergency_stop(const bool & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + bool get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } + + std::vector get_error_code() const + { + return error_signal_->get_value>(); + } + + void set_error_message(std::vector error_messages) + { + error_signal_message_->set_value(error_messages); + } + + std::vector get_error_message() const + { + return error_signal_message_->get_value>(); + } + + void set_warning_code(std::vector warning_codes) + { + warning_signal_->set_value(warning_codes); + } + + std::vector get_warning_code() const + { + return warning_signal_->get_value>(); + } + + void set_warning_message(std::vector error_message) + { + warning_signal_message_->set_value(error_message); + } + + std::vector get_warning_message() const + { + return warning_signal_message_->get_value>(); + } + 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_; + + 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_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index d5d999cca8..bb68e72b12 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,5 +33,23 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +/** + * \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_state_interface_descriptions_from_hardware_info( + const std::vector & component_info); + +/** + * \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_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/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..97f4c3f167 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,47 +15,75 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include +#include #include +#include #include +#include +#include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" -#include "hardware_interface/visibility_control.h" +#include "hardware_interface/types/handle_datatype.hpp" namespace hardware_interface { + /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_name), + interface_name_(interface_description.interface_info.name) { + init_handle_value(interface_description.interface_info); } - explicit ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + 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_)); } - explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + template < + typename T, typename std::enable_if< + std::is_integral::value || std::is_floating_point::value, int>::type = 0> + void operator+=(const T & value) { + value_ = std::get(value_) + value; } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; - - ReadOnlyHandle(ReadOnlyHandle && other) = default; + template < + typename T, typename std::enable_if< + std::is_integral::value || std::is_floating_point::value, int>::type = 0> + void operator-=(const T & value) + { + value_ = std::get(value_) - value; + } - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + template ::value, int>::type = 0> + void operator=(const T & value) + { + value_ = value; + } - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(const Handle & other) = default; - virtual ~ReadOnlyHandle() = default; + Handle & operator=(Handle && other) = default; - /// Returns true if handle references a value. - inline operator bool() const { return value_ptr_ != nullptr; } + virtual ~Handle() = default; const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } @@ -70,62 +98,126 @@ class ReadOnlyHandle const std::string & get_prefix_name() const { return prefix_name_; } - double get_value() const + template ::value, int>::type = 0> + T get_value() const { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + return std::get(value_); } -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) + template ::value, int>::type = 0> + void set_value(T value) { + value_ = value; } - 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; +protected: + // used for the + bool correct_vector_size(const size_t & expected, const size_t & actual) + { + return expected == actual; + } - void set_value(double value) + void init_handle_value(const InterfaceInfo & interface_info) { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + if (interface_info.data_type == "bool") + { + value_ = interface_info.initial_value.empty() ? false + : (interface_info.initial_value == "true" || + interface_info.initial_value == "True"); + } + else if (interface_info.data_type == "int") + { + value_ = interface_info.initial_value.empty() ? 0 : std::stoi(interface_info.initial_value); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + value_ = std::vector(hardware_interface::warning_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if (interface_info.size != 0 && hardware_interface::error_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::error_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::error_signal_count, 0); + } + else if (interface_info.data_type == "vector") + { + if ( + interface_info.size != 0 && hardware_interface::warning_signal_count != interface_info.size) + { + throw std::runtime_error( + "The size:{" + std::to_string(interface_info.size) + "} for data_type{" + + interface_info.data_type + "} for the InterfaceInfo with name:{" + interface_info.name + + "} does not equal the expected size:{" + + std::to_string(hardware_interface::warning_signal_count) + "}."); + } + + value_ = std::vector(hardware_interface::warning_signal_count, ""); + } + else if (interface_info.data_type == "double") + { + value_ = interface_info.initial_value.empty() ? std::numeric_limits::quiet_NaN() + : std::stod(interface_info.initial_value); + } + // Default for empty is std::monostate + else if (interface_info.data_type.empty()) + { + value_ = {std::monostate()}; + } + // If not empty and it belongs to none of the above types, we still want to throw as there might + // be a typo in the data_type like "bol" or user wants some unsupported type + else + { + throw std::runtime_error( + "The data_type:{" + interface_info.data_type + "} for the InterfaceInfo with name:{" + + interface_info.name + + "} is not supported for Handles. Supported data_types are: bool, double, vector, " + "vector and vector."); + } } + + std::string prefix_name_; + std::string interface_name_; + HANDLE_DATATYPE value_; }; -class StateInterface : public ReadOnlyHandle +class StateInterface : public Handle { public: + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) + { + } + StateInterface(const StateInterface & other) = default; 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) + : Handle(interface_description) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -136,7 +228,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/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 97abc00438..83a9ac6dff 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -27,6 +27,37 @@ namespace hardware_interface */ struct InterfaceInfo { + // Add default constructor, so that e.g. size is initialized to sensible value + InterfaceInfo() + { + // cpp_lint complains about min and max include otherwise + name = ""; + min = ""; + max = ""; + initial_value = ""; + data_type = ""; + size = 0; + } + + explicit InterfaceInfo(const std::string & name_in) : InterfaceInfo() { name = name_in; } + + explicit InterfaceInfo(const std::string & name_in, const std::string & data_type_in) + : InterfaceInfo() + { + name = name_in; + data_type = data_type_in; + } + + explicit InterfaceInfo( + const std::string & name_in, const std::string & initial_value_in, + const std::string & data_type_in) + : InterfaceInfo() + { + name = name_in; + initial_value = initial_value_in; + data_type = data_type_in; + } + /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -38,9 +69,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; }; @@ -122,6 +153,38 @@ 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) + { + } + + 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. + */ + std::string prefix_name; + + /** + * 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_interface_type() const { return 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/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/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/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 5d0677c587..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 14a59e4588..a103d2207c 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,13 +15,19 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ +#include #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_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" @@ -97,19 +103,153 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; + import_state_interface_descriptions(info_); + create_report_interfaces(); 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) + { + auto sensor_state_interface_descriptions = + 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)); + } + } + + /** + * 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 = "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 = "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); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + 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 = "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); + } + /// Exports all state interfaces for this hardware interface. /** - * 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 */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_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() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector 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 + * 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() + { + // 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_map_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(state_interface); + } + + for (const auto & [name, descr] : sensor_state_interfaces_) + { + // TODO(Manuel) maybe check for duplicates otherwise only the first appearance of "name" is + // inserted + auto state_interface = std::make_shared(descr); + sensor_states_map_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); + 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; + } /// Read the current state values from the actuator. /** @@ -141,9 +281,69 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_state(const std::string & interface_name, const double & value) + { + sensor_states_map_.at(interface_name)->set_value(value); + } + + double get_state(const std::string & interface_name) const + { + return sensor_states_map_.at(interface_name)->get_value(); + } + + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } + + std::vector get_error_code() const + { + return error_signal_->get_value>(); + } + + void set_error_message(std::vector error_messages) + { + error_signal_message_->set_value(error_messages); + } + + std::vector get_error_message() const + { + return error_signal_message_->get_value>(); + } + + void set_warning_code(std::vector warning_codes) + { + warning_signal_->set_value(warning_codes); + } + + std::vector get_warning_code() const + { + return warning_signal_->get_value>(); + } + + void set_warning_message(std::vector error_message) + { + warning_signal_message_->set_value(error_message); + } + + std::vector get_warning_message() const + { + return warning_signal_message_->get_value>(); + } + protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + // interface names to InterfaceDescription + std::unordered_map sensor_state_interfaces_; + std::unordered_map unlisted_state_interfaces_; + + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector> sensor_states_; + std::vector> unlisted_states_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::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.hpp b/hardware_interface/include/hardware_interface/system.hpp index ece14f814d..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 e5c6f2f542..1552b6c8e4 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,13 +15,21 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ +#include #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_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" @@ -98,30 +106,294 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; + import_state_interface_descriptions(info_); + import_command_interface_descriptions(info_); + create_report_interfaces(); return CallbackReturn::SUCCESS; }; + /** + * 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) + { + auto joint_state_interface_descriptions = + 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_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_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)); + } + } + + /** + * 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) + { + auto joint_command_interface_descriptions = + 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_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)); + } + } + + /** + * 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 = "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 = "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); + + // WARNING + // create warning signal interface + InterfaceInfo warning_interface_info; + warning_interface_info.name = hardware_interface::WARNING_SIGNAL_INTERFACE_NAME; + 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 = "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); + } + /// Exports all state interfaces for this hardware interface. /** - * 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 */ - virtual std::vector export_state_interfaces() = 0; + [[deprecated( + "Replaced by vector> on_export_state_interfaces() method. " + "Exporting is handled " + "by the Framework.")]] virtual std::vector + export_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() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated + * version is removed. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector 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 + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + std::vector> on_export_state_interfaces() + { + // 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() + + 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)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(state_interface); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + 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); + } + + // 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; + } /// Exports all command interfaces for this hardware 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. " + "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() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * Note method name is going to be changed to export_command_interfaces() as soon as the + * deprecated version is removed. + * + * \return vector of descriptions to the unlisted CommandInterfaces */ - virtual std::vector export_command_interfaces() = 0; + 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 + * assigned here and resides in the system_interface. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + std::vector> on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_command_interfaces_2(); + + 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)); + unlisted_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : joint_command_interfaces_) + { + 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); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + 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; + } /// Prepare for a new command interface switch. /** @@ -202,10 +474,110 @@ 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); + } + + double get_state(const std::string & interface_name) const + { + return system_states_.at(interface_name)->get_value(); + } + + void set_command(const std::string & interface_name, const double & value) + { + system_commands_.at(interface_name)->set_value(value); + } + + double get_command(const std::string & interface_name) const + { + return system_commands_.at(interface_name)->get_value(); + } + + void set_emergency_stop(const bool & emergency_stop) + { + emergency_stop_->set_value(emergency_stop); + } + + bool get_emergency_stop() const { return emergency_stop_->get_value(); } + + void set_error_code(std::vector error_codes) { error_signal_->set_value(error_codes); } + + std::vector get_error_code() const + { + return error_signal_->get_value>(); + } + + void set_error_message(std::vector error_messages) + { + error_signal_message_->set_value(error_messages); + } + + std::vector get_error_message() const + { + return error_signal_message_->get_value>(); + } + + void set_warning_code(std::vector warning_codes) + { + warning_signal_->set_value(warning_codes); + } + + std::vector get_warning_code() const + { + return warning_signal_->get_value>(); + } + + void set_warning_message(std::vector error_message) + { + warning_signal_message_->set_value(error_message); + } + + std::vector get_warning_message() const + { + return warning_signal_message_->get_value>(); + } 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 sensor_state_interfaces_; + + std::unordered_map gpio_state_interfaces_; + std::unordered_map gpio_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> 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_; + + 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_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/handle_datatype.hpp b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp new file mode 100644 index 0000000000..0b84f6262f --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/handle_datatype.hpp @@ -0,0 +1,48 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ +#define HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" + +namespace hardware_interface +{ + +// std::monostate is that we have a well defined alternative empty default initialization +// !!! IF YOU ADD TYPES TO HANDLE_DATATYPE, std::monostate MUST ALWAYS REMAIN AT THE FIRST POSITION +// This i needed so that e.g.: HANDLE_DATATYPE our_variant = {}; -> defaults to std::monostate +using HANDLE_DATATYPE = std::variant< + std::monostate, bool, int, double, std::vector, std::vector, + std::vector>; + +// Define a type trait for allowed types +template +struct HANDLE_DATATYPE_TYPES +: std::disjunction< + std::is_same, std::is_same, std::is_same, + std::is_same>, std::is_same>, + std::is_same>> +{ +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HANDLE_DATATYPE_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp new file mode 100644 index 0000000000..c74941f9df --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_emergency_stop_signal.hpp @@ -0,0 +1,29 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ + +#include +#include + +namespace hardware_interface +{ +// Count of how many different emergency stop signals there are that can be reported. +const size_t emergency_stop_signal_count = 1; + +constexpr char EMERGENCY_STOP_SIGNAL[] = "EMERGENCY_STOP_SIGNAL"; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_EMERGENCY_STOP_SIGNAL_HPP_ diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp new file mode 100644 index 0000000000..b40011eb3a --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp @@ -0,0 +1,36 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_ + +#include +#include +#include + +namespace hardware_interface +{ + +// Count of how many different error signals there are that can be reported. +const size_t error_signal_count = 32; +using ERROR_SIGNALS = std::vector; + +constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL"; + +using ERROR_MESSAGES = std::vector; +constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE"; + +} // 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..cf4b5a8c0e --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_warning_signals.hpp @@ -0,0 +1,35 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ +#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ + +#include +#include +#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::vector; +constexpr char WARNING_SIGNAL_INTERFACE_NAME[] = "WARNING_SIGNAL"; + +using WARNING_MESSAGES = std::vector; +constexpr char WARNING_SIGNAL_MESSAGE_INTERFACE_NAME[] = "WARNING_SIGNAL_MESSAGE"; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_WARNING_SIGNALS_HPP_ 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/actuator.cpp b/hardware_interface/src/actuator.cpp index 6b58e365dc..76ac732e34 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -186,16 +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 + + // 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 + + // 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/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0841265e81..40a646dea4 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -722,4 +722,38 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +std::vector parse_state_interface_descriptions_from_hardware_info( + const std::vector & component_info) +{ + std::vector component_state_interface_descriptions; + component_state_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + component_state_interface_descriptions.emplace_back( + InterfaceDescription(component.name, state_interface)); + } + } + return component_state_interface_descriptions; +} + +std::vector parse_command_interface_descriptions_from_hardware_info( + const std::vector & component_info) +{ + std::vector component_command_interface_descriptions; + component_command_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + component_command_interface_descriptions.emplace_back( + InterfaceDescription(component.name, command_interface)); + } + } + return component_command_interface_descriptions; +} + } // namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 2d8a01a34f..a3fc0f97a3 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/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e8ccc7b1f..0fa1c15e5d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -433,24 +433,65 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { - auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; + std::vector> interfaces = hardware.export_state_interfaces(); + interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + for (auto const & interface : interfaces) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + const auto [it, success] = + state_interface_map_.insert(std::make_pair(interface->get_name(), interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + interface->get_name() + "]"); + throw std::runtime_error(msg); + } + interface_names.push_back(interface->get_name()); } + hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( available_state_interfaces_.capacity() + interface_names.size()); } + void insert_command_interface(const std::shared_ptr command_interface) + { + 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); + } + } + + // 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(); + 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); + } + } + // END: for backward compatibility + template void import_command_interfaces(HardwareT & hardware) { - auto interfaces = hardware.export_command_interfaces(); + std::vector> interfaces = + hardware.export_command_interfaces(); + hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } @@ -472,7 +513,25 @@ 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; + } + + std::vector add_command_interfaces( + const std::vector> & interfaces) + { + std::vector interface_names; + interface_names.reserve(interfaces.size()); + for (const 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 +768,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 +869,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 @@ -844,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); @@ -981,7 +1041,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..e758953369 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -184,9 +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(); + } + + // 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 ee942d6581..a64fb78fba 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -184,14 +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 + + // 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 System::export_command_interfaces() +std::vector> System::export_command_interfaces() { - return impl_->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/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 6dc1c394b0..f995d5d4d2 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 @@ -33,6 +34,9 @@ #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 @@ -41,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 @@ -49,22 +58,28 @@ namespace test_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -class DummyActuator : public hardware_interface::ActuatorInterface +class DummyActuatorDefault : public hardware_interface::ActuatorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override + 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 { - position_state_ = 0.0; - velocity_state_ = 0.0; + set_state("joint1/position", 0.0); + set_state("joint1/velocity", 0.0); if (recoverable_error_happened_) { - velocity_command_ = 0.0; + set_command("joint1/velocity", 0.0); } read_calls_ = 0; @@ -73,29 +88,7 @@ class DummyActuator : public hardware_interface::ActuatorInterface 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"; } + std::string get_name() const override { return "DummyActuatorDefault"; } hardware_interface::return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -118,16 +111,16 @@ class DummyActuator : public hardware_interface::ActuatorInterface { return hardware_interface::return_type::ERROR; } - - position_state_ += velocity_command_; - velocity_state_ = velocity_command_; + 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 { - velocity_state_ = 0; + set_state("joint1/velocity", 0.0); return CallbackReturn::SUCCESS; } @@ -146,42 +139,38 @@ class DummyActuator : public hardware_interface::ActuatorInterface } 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; }; -class DummySensor : public hardware_interface::SensorInterface +class DummySensorDefault : public hardware_interface::SensorInterface { - CallbackReturn on_init(const hardware_interface::HardwareInfo & /*info*/) override + 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 { - voltage_level_ = 0.0; + for (const auto & [name, descr] : sensor_state_interfaces_) + { + set_state(name, 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"; } + std::string get_name() const override { return "DummySensorDefault"; } hardware_interface::return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -193,7 +182,7 @@ class DummySensor : public hardware_interface::SensorInterface } // no-op, static value - voltage_level_ = voltage_level_hw_value_; + set_state("joint1/voltage", voltage_level_hw_value_); return hardware_interface::return_type::OK; } @@ -212,18 +201,22 @@ class DummySensor : public hardware_interface::SensorInterface } 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; }; - -class DummySystem : public hardware_interface::SystemInterface +class DummySystemDefault : public hardware_interface::SystemInterface { - 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; } @@ -232,15 +225,15 @@ class DummySystem : public hardware_interface::SystemInterface { for (auto i = 0ul; i < 3; ++i) { - position_state_[i] = 0.0; - velocity_state_[i] = 0.0; + 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) { - velocity_command_[i] = 0.0; + set_command(velocity_commands_[i], 0.0); } } @@ -250,41 +243,7 @@ class DummySystem : public hardware_interface::SystemInterface 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"; } + std::string get_name() const override { return "DummySystemDefault"; } hardware_interface::return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -310,17 +269,18 @@ class DummySystem : public hardware_interface::SystemInterface for (auto i = 0; i < 3; ++i) { - position_state_[i] += velocity_command_[0]; - velocity_state_[i] = velocity_command_[0]; + 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 (auto i = 0ul; i < 3; ++i) + for (const auto & velocity_state : velocity_states_) { - velocity_state_[i] = 0.0; + set_state(velocity_state, 0.0); } return CallbackReturn::SUCCESS; } @@ -340,13 +300,12 @@ class DummySystem : public hardware_interface::SystemInterface } 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}}; + 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; @@ -363,13 +322,6 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface return CallbackReturn::SUCCESS; } - std::vector export_state_interfaces() override { return {}; } - - std::vector export_command_interfaces() override - { - return {}; - } - std::string get_name() const override { return "DummySystemPreparePerform"; } hardware_interface::return_type read( @@ -419,42 +371,68 @@ class DummySystemPreparePerform : public hardware_interface::SystemInterface }; } // namespace test_components - -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()); + 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()); - 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)); } @@ -468,8 +446,11 @@ 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[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)); } @@ -483,8 +464,10 @@ 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[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)); } @@ -498,8 +481,9 @@ 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[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)); } @@ -510,96 +494,177 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch({""}, {""})); } -TEST(TestComponentInterfaces, dummy_sensor) +TEST(TestComponentInterfaces, dummy_sensor_default) { - hardware_interface::Sensor sensor_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info); + 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())); + ASSERT_EQ(1u + warnig_signals_size + error_signals_size, state_interfaces.size()); + { + 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()); } -TEST(TestComponentInterfaces, dummy_system) +TEST(TestComponentInterfaces, dummy_system_default) { - hardware_interface::System system_hw(std::make_unique()); - - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); + 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(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()); + ASSERT_EQ(6u + 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()); + } 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)); } @@ -613,12 +678,21 @@ 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[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)); } @@ -632,12 +706,18 @@ 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[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)); } @@ -651,12 +731,15 @@ 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[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)); } @@ -696,12 +779,20 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) system_hw.perform_command_mode_switch(two_keys, one_key)); } -TEST(TestComponentInterfaces, dummy_actuator_read_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()); @@ -727,9 +818,12 @@ TEST(TestComponentInterfaces, dummy_actuator_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()); @@ -755,12 +849,19 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) +TEST(TestComponentInterfaces, dummy_actuator_default_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); + 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()); @@ -786,9 +887,12 @@ TEST(TestComponentInterfaces, dummy_actuator_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()); @@ -814,12 +918,18 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) +TEST(TestComponentInterfaces, dummy_sensor_default_read_error_behavior) { - hardware_interface::Sensor sensor_hw(std::make_unique()); + hardware_interface::Sensor sensor_hw(std::make_unique()); - hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info); + 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 @@ -841,15 +951,10 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) 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 + 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()); @@ -866,24 +971,24 @@ 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()); - // 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()); } -TEST(TestComponentInterfaces, dummy_system_read_error_behavior) +TEST(TestComponentInterfaces, dummy_system_default_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); + 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()); @@ -912,11 +1017,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()); @@ -942,12 +1047,18 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); } -TEST(TestComponentInterfaces, dummy_system_write_error_behavior) +TEST(TestComponentInterfaces, dummy_system_default_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); + 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()); @@ -976,11 +1087,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()); 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..757bbb68dd --- /dev/null +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -0,0 +1,384 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "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" +#include "test_components.hpp" + +// 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 + +// Values to send over command interface to trigger error in write and read methods + +namespace +{ +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 +{ + std::string get_name() const override { return "DummyActuatorDefault"; } + + std::vector export_state_interfaces_2() override + { + 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 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; + } + + 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 export_state_interfaces_2() override + { + 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; + } + + 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 export_state_interfaces_2() override + { + 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 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; + } + + 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()); + + const auto listed_interface_size = 2u; + const auto interfaces_size = listed_interface_size + report_signals_size; + auto state_interfaces = actuator_hw.export_state_interfaces(); + // 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"); + 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()); + { + 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) +{ + 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 listed_interface_size = 1u; + 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_sizes + 1u, state_interfaces.size()); + { + 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) +{ + 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 listed_interface_size = 6u; + 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_sizes + 1u, 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()); + } + { + 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()); + { + 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_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 968d41ed97..71f428053d 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_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"); + + 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_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"); + 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_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"); + 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_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"); + 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_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"); + + 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_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_ 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..d90b616a4b --- /dev/null +++ b/hardware_interface/test/test_error_warning_codes.cpp @@ -0,0 +1,1316 @@ +// Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/sensor_interface.hpp" +#include "hardware_interface/system.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_emergency_stop_signal.hpp" +#include "hardware_interface/types/hardware_interface_error_signals.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_warning_signals.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_components.hpp" +// Values to send over command interface to trigger error in write and read methods + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000; +const auto emergency_stop_signal_size = 1; +const auto warnig_signals_size = 2; +const auto error_signals_size = 2; +const auto report_signals_size = + emergency_stop_signal_size + warnig_signals_size + error_signals_size; + +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 + +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(false); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); + + if (recoverable_error_happened_) + { + 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(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); + return hardware_interface::return_type::ERROR; + } + + // 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(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); + return hardware_interface::return_type::ERROR; + } + auto position_state = get_state("joint1/position"); + 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(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; + } + + 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(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; + } + + // 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(false); + set_error_code(empty_error_code_vec); + set_error_message(empty_error_msg_vec); + set_warning_code(empty_warn_code_vec); + set_warning_message(empty_warn_msg_vec); + // reset command only if error is initiated + if (recoverable_error_happened_) + { + 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(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); + return hardware_interface::return_type::ERROR; + } + + // 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(true); + set_error_code(error_code_vec); + set_error_message(error_msg_vec); + set_warning_code(warn_code_vec); + set_warning_message(warn_msg_vec); + return hardware_interface::return_type::ERROR; + } + + 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 listed_interface_size = 2; + auto state_interfaces = actuator_hw.export_state_interfaces(); + ASSERT_EQ(listed_interface_size + 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 listed_interface_size = 1; + auto state_interfaces = sensor_hw.export_state_interfaces(); + ASSERT_EQ( + listed_interface_size + 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 listed_interface_size = 6; + auto state_interfaces = system_hw.export_state_interfaces(); + ASSERT_EQ(listed_interface_size + report_signals_size, state_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/position"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/position", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_POSITION, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(state_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", state_interfaces[position]->get_name()); + EXPECT_EQ(hardware_interface::HW_IF_VELOCITY, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", state_interfaces[position]->get_prefix_name()); + } + // EMERGENCY STOP + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::EMERGENCY_STOP_SIGNAL), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::EMERGENCY_STOP_SIGNAL, state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + // ERROR_SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::ERROR_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + // WARNING SIGNAL + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + std::string(hardware_interface::WARNING_SIGNAL_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = test_components::vector_contains( + state_interfaces, "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME)); + EXPECT_TRUE(contains); + EXPECT_EQ( + "RRBotSystemWithGPIO/" + + std::string(hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME), + state_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::WARNING_SIGNAL_MESSAGE_INTERFACE_NAME, + state_interfaces[position]->get_interface_name()); + EXPECT_EQ("RRBotSystemWithGPIO", state_interfaces[position]->get_prefix_name()); + } + + auto command_interfaces = system_hw.export_command_interfaces(); + ASSERT_EQ(3u, command_interfaces.size()); + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint1/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint1/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint1", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint2/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint2/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint2", command_interfaces[position]->get_prefix_name()); + } + { + auto [contains, position] = + test_components::vector_contains(command_interfaces, "joint3/velocity"); + EXPECT_TRUE(contains); + EXPECT_EQ("joint3/velocity", command_interfaces[position]->get_name()); + EXPECT_EQ( + hardware_interface::HW_IF_VELOCITY, command_interfaces[position]->get_interface_name()); + EXPECT_EQ("joint3", command_interfaces[position]->get_prefix_name()); + } +} + +TEST(TestComponentInterfaces, dummy_actuator_default_read_error_behavior) +{ + hardware_interface::Actuator actuator_hw( + std::make_unique()); + + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_dummy_actuator_only + + ros2_control_test_assets::urdf_tail; + const std::vector control_resources = + hardware_interface::parse_control_resources_from_urdf(urdf_to_test); + const hardware_interface::HardwareInfo dummy_actuator = control_resources[0]; + auto state = actuator_hw.initialize(dummy_actuator); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); + + 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(), 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(), 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()); + 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(), 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(), 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()); + 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()); + + 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(), 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(), 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()); + 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); + + 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>(), 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>(), 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()); + 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()); + + 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(), 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(), 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()); + 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(), 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(), 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()); + 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()); + + 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(), 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(), 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()); + 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()); +} diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..8184a6210f 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -13,9 +13,16 @@ // 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; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -24,43 +31,789 @@ constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; } // namespace -TEST(TestHandle, command_interface) +TEST(TestHandle, ci_empty_handle_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, true); +} + +TEST(TestHandle, si_empty_handle_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, true); +} + +TEST(TestHandle, ci_empty_handle_prefix_empty_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_empty_value) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_inteface_name_empty_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_inteface_name_empty_value) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, ci_empty_handle_prefix_and_inteface_name) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_and_inteface_name) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_prefix_and_inteface_name_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix_and_inteface_name_value) +{ + InterfaceInfo info; + info.name = ""; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_prefix) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_prefix) +{ + InterfaceInfo info; + info.name = ""; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_handle_interface_name) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, si_empty_handle_interface_name) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr("", info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); + EXPECT_EQ(interface, false); +} + +TEST(TestHandle, ci_empty_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, ci_true_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, ci_empty_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, ci_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), 1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, ci_negative_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, ci_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::invalid_argument); +} + +TEST(TestHandle, ci_int8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, 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, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, ci_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, ci_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, ci_throw_on_get_wrong_type_bool_from_int8_vec) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, ci_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(CommandInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, ci_throw_on_get_double_from_monostate) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + CommandInterface interface{descr}; + EXPECT_EQ(interface, false); + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, si_empty_bool_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), false); + EXPECT_NO_THROW(interface.set_value(true)); + EXPECT_EQ(interface.get_value(), true); +} + +TEST(TestHandle, si_true_bool_initialization) { - double value = 1.337; - CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "bool"; + info.initial_value = "true"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), true); + EXPECT_NO_THROW(interface.set_value(false)); + EXPECT_EQ(interface.get_value(), false); +} + +TEST(TestHandle, si_empty_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_TRUE(std::isnan(interface.get_value())); + EXPECT_NO_THROW(interface.set_value(1.5)); + EXPECT_EQ(interface.get_value(), 1.5); +} + +TEST(TestHandle, si_pos_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), 1.5); EXPECT_NO_THROW(interface.set_value(0.0)); - EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_negative_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "-1.5"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value(), -1.5); + EXPECT_NO_THROW(interface.set_value(0.0)); + EXPECT_EQ(interface.get_value(), 0.0); +} + +TEST(TestHandle, si_invalid_double_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + info.initial_value = "abc"; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::invalid_argument); } -TEST(TestHandle, state_interface) +TEST(TestHandle, si_int8_t_vector_default_size_initialization) { - double value = 1.337; - StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(interface.get_value(), value); - // interface.set_value(5); compiler error, no set_value function + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_int8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_int8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_uint8_t_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_uint8_t_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, 0); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), zero_vector); + zero_vector[1] = 1; + zero_vector[2] = 2; + zero_vector[4] = 4; + zero_vector[8] = 8; + EXPECT_NO_THROW(interface.set_value(zero_vector)); + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_default_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector zero_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface.get_value>(), zero_vector); +} + +TEST(TestHandle, si_string_vector_wrong_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = 5; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr}, std::runtime_error); +} + +TEST(TestHandle, si_string_vector_correct_size_initialization) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + std::vector empty_str_vector(hardware_interface::warning_signal_count, ""); + + EXPECT_EQ(interface, true); + EXPECT_EQ(interface.get_value>(), empty_str_vector); + empty_str_vector[1] = "Warning message number 1"; + empty_str_vector[2] = "Warn msg no. 2"; + empty_str_vector[4] = "message no. 3"; + empty_str_vector[8] = "Warning message no. 4"; + + EXPECT_NO_THROW(interface.set_value(empty_str_vector)); + EXPECT_EQ(interface.get_value>(), empty_str_vector); +} + +TEST(TestHandle, si_throw_on_get_wrong_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "vector"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + + EXPECT_THROW(interface.get_value(), std::bad_variant_access); +} + +TEST(TestHandle, si_throw_on_not_know_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "banana"; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + EXPECT_THROW(StateInterface interface{descr};, std::runtime_error); +} + +TEST(TestHandle, si_throw_on_empty_type) +{ + InterfaceInfo info; + info.name = FOO_INTERFACE; + info.size = hardware_interface::warning_signal_count; + InterfaceDescription descr(JOINT_NAME, info); + + StateInterface interface{descr}; + EXPECT_TRUE(std::isnan(interface.get_value())); } TEST(TestHandle, name_getters_work) { - StateInterface handle{JOINT_NAME, FOO_INTERFACE}; + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + StateInterface handle{descr}; + + EXPECT_EQ(handle, false); EXPECT_EQ(handle.get_name(), std::string(JOINT_NAME) + "/" + std::string(FOO_INTERFACE)); EXPECT_EQ(handle.get_interface_name(), FOO_INTERFACE); EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME); } -TEST(TestHandle, value_methods_throw_for_nullptr) +TEST(TestHandle, ci_value_methods) { - CommandInterface handle{JOINT_NAME, FOO_INTERFACE}; - EXPECT_ANY_THROW(handle.get_value()); - EXPECT_ANY_THROW(handle.set_value(0.0)); + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + info.data_type = "double"; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + CommandInterface handle{descr}; + + EXPECT_EQ(handle, true); + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); + EXPECT_NO_THROW(handle.set_value(0.0)); + EXPECT_EQ(handle.get_value(), 0.0); } -TEST(TestHandle, value_methods_work_on_non_nullptr) +TEST(TestHandle, si_value_methods) { - double value = 1.337; - CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value}; - EXPECT_DOUBLE_EQ(handle.get_value(), value); + hardware_interface::InterfaceInfo info; + info.name = FOO_INTERFACE; + hardware_interface::InterfaceDescription descr(JOINT_NAME, info); + StateInterface handle{descr}; + + EXPECT_EQ(handle, true); + EXPECT_NO_THROW(handle.get_value()); + EXPECT_TRUE(std::isnan(handle.get_value())); EXPECT_NO_THROW(handle.set_value(0.0)); - EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); + EXPECT_EQ(handle.get_value(), 0.0); +} + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + info.data_type = "double"; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle, true); + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + (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.data_type = "double"; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle, true); + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); } diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index 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..3f1bfcbb24 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"; } @@ -1186,13 +1190,17 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]}; - std::vector reference_interfaces; + std::vector> reference_interfaces; std::vector reference_interface_values = {1.0, 2.0, 3.0}; for (size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size(); ++i) { - reference_interfaces.push_back(hardware_interface::CommandInterface( - CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); + hardware_interface::InterfaceInfo info; + info.name = REFERENCE_INTERFACE_NAMES[i]; + info.initial_value = std::to_string(reference_interface_values[i]); + hardware_interface::InterfaceDescription ref_interface(CONTROLLER_NAME, info); + reference_interfaces.push_back( + std::make_shared(ref_interface)); } rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces); diff --git a/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..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 @@ -400,6 +400,81 @@ 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/SingleJointVoltageSensor + 2 + + + + + +)"; + +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_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"( 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/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 diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 809472ab75..229a2bbdb3 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -20,6 +20,7 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "transmission_interface/exception.hpp" #include "transmission_interface/transmission.hpp" @@ -128,13 +129,13 @@ class SimpleTransmission : public Transmission double reduction_; double jnt_offset_; - JointHandle joint_position_ = {"", "", nullptr}; - JointHandle joint_velocity_ = {"", "", nullptr}; - JointHandle joint_effort_ = {"", "", nullptr}; + JointHandle joint_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + JointHandle joint_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + JointHandle joint_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; - ActuatorHandle actuator_position_ = {"", "", nullptr}; - ActuatorHandle actuator_velocity_ = {"", "", nullptr}; - ActuatorHandle actuator_effort_ = {"", "", nullptr}; + ActuatorHandle actuator_position_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + ActuatorHandle actuator_velocity_{hardware_interface::InterfaceDescription{{}, std::string{}}}; + ActuatorHandle actuator_effort_{hardware_interface::InterfaceDescription{{}, std::string{}}}; }; inline SimpleTransmission::SimpleTransmission( @@ -156,7 +157,9 @@ HandleType get_by_interface( [&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; }); if (result == handles.cend()) { - return HandleType(handles.cbegin()->get_prefix_name(), interface_name, nullptr); + // return new Handle where data is set to std::monotype by default + return HandleType(hardware_interface::InterfaceDescription( + handles.cbegin()->get_prefix_name(), interface_name)); } return *result; } @@ -218,17 +221,17 @@ inline void SimpleTransmission::actuator_to_joint() { if (joint_effort_ && actuator_effort_) { - joint_effort_.set_value(actuator_effort_.get_value() * reduction_); + joint_effort_.set_value(actuator_effort_.get_value() * reduction_); } if (joint_velocity_ && actuator_velocity_) { - joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); + joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_); } if (joint_position_ && actuator_position_) { - joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); + joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_); } } @@ -236,17 +239,17 @@ inline void SimpleTransmission::joint_to_actuator() { if (joint_effort_ && actuator_effort_) { - actuator_effort_.set_value(joint_effort_.get_value() / reduction_); + actuator_effort_.set_value(joint_effort_.get_value() / reduction_); } if (joint_velocity_ && actuator_velocity_) { - actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); + actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_); } if (joint_position_ && actuator_position_) { - actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); + actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_); } } diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 14ffe968bc..8c3c01e30c 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -23,6 +23,8 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using testing::DoubleNear; using transmission_interface::ActuatorHandle; using transmission_interface::DifferentialTransmission; @@ -92,14 +94,22 @@ void testConfigureWithBadHandles(std::string interface_name) DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0}); double dummy; - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + auto a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a2_handle = + ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto a3_handle = + ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(InterfaceInfo(interface_name)))); + auto j3_handle = + JointHandle(InterfaceDescription("joint3", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(InterfaceInfo(interface_name)))); + auto invalid_j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(InterfaceInfo(interface_name)))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -129,12 +139,6 @@ TEST(ConfigureTest, FailsWithBadHandles) class TransmissionSetup : public ::testing::Test { -protected: - // Input/output transmission data - double a_val[2]; - double j_val[2]; - std::vector a_vec = {&a_val[0], &a_val[1]}; - std::vector j_vec = {&j_val[0], &j_val[1]}; }; /// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map. @@ -151,23 +155,29 @@ class BlackBoxTest : public TransmissionSetup const std::string & interface_name) { // set actuator values to reference - a_val[0] = ref_val[0]; - a_val[1] = ref_val[1]; + auto a_val = ref_val[0]; + auto a_val_1 = ref_val[1]; + // create handles and configure - auto a1_handle = ActuatorHandle("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle.set_value(EXTREMAL_VALUE); + a2_handle.set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(ref_val[0], DoubleNear(a1_handle.get_value(), EPS)); + EXPECT_THAT(ref_val[1], DoubleNear(a2_handle.get_value(), EPS)); } // Generate a set of transmission instances @@ -237,43 +247,57 @@ TEST_F(WhiteBoxTest, DontMoveJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 0.0; - *a_vec[1] = 0.0; + auto a_val = 0.0; + auto a_val_1 = 0.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -285,43 +309,58 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = 10.0; + auto a_val = 10.0; + auto a_val_1 = 10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(400.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -333,43 +372,56 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) DifferentialTransmission trans(actuator_reduction, joint_reduction); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 10.0; - *a_vec[1] = -10.0; + auto a_val = 10.0; + auto a_val_1 = -10.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(400.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -386,42 +438,54 @@ TEST_F(WhiteBoxTest, MoveBothJoints) DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset); // Actuator input (used for effort, velocity and position) - *a_vec[0] = 3.0; - *a_vec[1] = 5.0; + auto a_val = 3.0; + auto a_val_1 = 5.0; // Effort interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(140.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(520.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.01250, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.06875, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.01250, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(4.06875, DoubleNear(joint2_handle.get_value(), EPS)); } } diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index f74e4def6a..b0d45d42df 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; @@ -94,16 +96,17 @@ TEST(PreconditionsTest, AccessorValidation) void testConfigureWithBadHandles(std::string interface_name) { FourBarLinkageTransmission trans({1.0, 1.0}, {1.0, 1.0}); - double dummy; - auto a1_handle = ActuatorHandle("act1", interface_name, &dummy); - auto a2_handle = ActuatorHandle("act2", interface_name, &dummy); - auto a3_handle = ActuatorHandle("act3", interface_name, &dummy); - auto j1_handle = JointHandle("joint1", interface_name, &dummy); - auto j2_handle = JointHandle("joint2", interface_name, &dummy); - auto j3_handle = JointHandle("joint3", interface_name, &dummy); - auto invalid_a1_handle = ActuatorHandle("act1", interface_name, nullptr); - auto invalid_j1_handle = JointHandle("joint1", interface_name, nullptr); + auto a1_handle = ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); + auto a2_handle = ActuatorHandle(InterfaceDescription("act2", InterfaceInfo(interface_name))); + auto a3_handle = ActuatorHandle(InterfaceDescription("act3", InterfaceInfo(interface_name))); + auto j1_handle = JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); + auto j2_handle = JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name))); + auto j3_handle = JointHandle(InterfaceDescription("joint3", InterfaceInfo(interface_name))); + auto invalid_a1_handle = + ActuatorHandle(InterfaceDescription("act1", InterfaceInfo(interface_name))); + auto invalid_j1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name))); EXPECT_THROW(trans.configure({}, {}), Exception); EXPECT_THROW(trans.configure({j1_handle}, {}), Exception); @@ -134,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. @@ -155,23 +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("act1", interface_name, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", interface_name, a_vec[1]); - auto joint1_handle = JointHandle("joint1", interface_name, j_vec[0]); - auto joint2_handle = JointHandle("joint2", interface_name, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(interface_name, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(interface_name, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(interface_name, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(interface_name, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); // actuator->joint->actuator roundtrip // but we also set actuator values to an extremal value // to ensure joint_to_actuator is not a no-op trans.actuator_to_joint(); - a_val[0] = a_val[1] = EXTREMAL_VALUE; + a1_handle.set_value(EXTREMAL_VALUE); + a2_handle.set_value(EXTREMAL_VALUE); trans.joint_to_actuator(); - EXPECT_THAT(ref_val[0], DoubleNear(a_val[0], EPS)); - EXPECT_THAT(ref_val[1], DoubleNear(a_val[1], EPS)); + EXPECT_THAT(a_val, DoubleNear(a1_handle.get_value(), EPS)); + EXPECT_THAT(a_val_1, DoubleNear(a2_handle.get_value(), EPS)); } // Generate a set of transmission instances @@ -241,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("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS)); - EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS)); + EXPECT_THAT(joint_offset[0], DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(joint_offset[1], DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -290,44 +305,56 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) // Effort interface { - a_val[0] = 5.0; - a_val[1] = 10.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a_val = 5.0; + auto a_val_1 = 10.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(100.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a_val = 10.0; + auto a_val_1 = 5.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - a_val[0] = 10.0; - a_val[1] = 5.0; - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a_val = 10.0; + auto a_val_1 = 5.0; + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.5, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.0, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -339,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("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(200.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(0.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(0.5, DoubleNear(joint2_handle.get_value(), EPS)); } } @@ -392,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("act1", HW_IF_EFFORT, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_EFFORT, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_EFFORT, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle( + InterfaceDescription("act2", InterfaceInfo(HW_IF_EFFORT, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_EFFORT, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_EFFORT, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-60.0, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-160.0, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-60.0, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(-160.0, DoubleNear(joint2_handle.get_value(), EPS)); } // Velocity interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_VELOCITY, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_VELOCITY, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_VELOCITY, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_VELOCITY, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_VELOCITY, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_VELOCITY, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_VELOCITY, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-0.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(-0.025, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-0.15, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(-0.025, DoubleNear(joint2_handle.get_value(), EPS)); } // Position interface { - auto a1_handle = ActuatorHandle("act1", HW_IF_POSITION, a_vec[0]); - auto a2_handle = ActuatorHandle("act2", HW_IF_POSITION, a_vec[1]); - auto joint1_handle = JointHandle("joint1", HW_IF_POSITION, j_vec[0]); - auto joint2_handle = JointHandle("joint2", HW_IF_POSITION, j_vec[1]); + auto a1_handle = ActuatorHandle( + InterfaceDescription("act1", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val), "double"))); + auto a2_handle = ActuatorHandle(InterfaceDescription( + "act2", InterfaceInfo(HW_IF_POSITION, std::to_string(a_val_1), "double"))); + auto joint1_handle = + JointHandle(InterfaceDescription("joint1", InterfaceInfo(HW_IF_POSITION, "double"))); + auto joint2_handle = + JointHandle(InterfaceDescription("joint2", InterfaceInfo(HW_IF_POSITION, "double"))); trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); trans.actuator_to_joint(); - EXPECT_THAT(-2.15, DoubleNear(j_val[0], EPS)); - EXPECT_THAT(3.975, DoubleNear(j_val[1], EPS)); + EXPECT_THAT(-2.15, DoubleNear(joint1_handle.get_value(), EPS)); + EXPECT_THAT(3.975, DoubleNear(joint2_handle.get_value(), EPS)); } } diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 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)); } } 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});