diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 46369f9499..30961316e7 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -160,13 +160,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. * * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated * version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector> export_state_interfaces_2() + virtual std::vector export_state_interfaces_2() { // return empty vector by default. return {}; @@ -181,8 +182,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector> on_export_state_interfaces() { - std::vector> state_interfaces = export_state_interfaces_2(); - state_interfaces.reserve(joint_state_interfaces_.size()); + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interfaces_2(); + + std::vector> state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + actuator_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } for (const auto & [name, descr] : joint_state_interfaces_) { @@ -219,13 +236,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. * * Note method name is going to be changed to export_command_interfaces() as soon as the * deprecated version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted CommandInterfaces */ - virtual std::vector> export_command_interfaces_2() + virtual std::vector export_command_interfaces_2() { // return empty vector by default. return {}; @@ -240,9 +258,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual std::vector> on_export_command_interfaces() { - std::vector> command_interfaces = + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = export_command_interfaces_2(); - command_interfaces.reserve(joint_command_interfaces_.size()); + + std::vector> command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + actuator_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } for (const auto & [name, descr] : joint_command_interfaces_) { @@ -358,10 +391,14 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::unordered_map joint_state_interfaces_; std::unordered_map joint_command_interfaces_; - std::unordered_map> actuator_states_; - std::unordered_map> actuator_commands_; + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + +private: + std::unordered_map> actuator_states_; + std::unordered_map> actuator_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index c61abf4157..2eeeba3482 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -144,13 +144,14 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. * * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated * version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector> export_state_interfaces_2() + virtual std::vector export_state_interfaces_2() { // return empty vector by default. return {}; @@ -165,8 +166,24 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual std::vector> on_export_state_interfaces() { - std::vector> state_interfaces = export_state_interfaces_2(); - state_interfaces.reserve(sensor_state_interfaces_.size()); + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interfaces_2(); + + std::vector> state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + sensor_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } for (const auto & [name, descr] : sensor_state_interfaces_) { @@ -224,10 +241,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI HardwareInfo info_; std::unordered_map sensor_state_interfaces_; - - std::unordered_map> sensor_states_; + std::unordered_map unlisted_state_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + +private: + std::unordered_map> sensor_states_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 07135c72a6..a5dd048215 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -179,13 +179,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. * * Note method name is going to be changed to export_state_interfaces() as soon as the deprecated * version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted StateInterfaces */ - virtual std::vector> export_state_interfaces_2() + virtual std::vector export_state_interfaces_2() { // return empty vector by default. return {}; @@ -200,10 +201,25 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ std::vector> on_export_state_interfaces() { - std::vector> state_interfaces = export_state_interfaces_2(); + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_state_interfaces_2(); + + std::vector> state_interfaces; state_interfaces.reserve( - joint_state_interfaces_.size() + sensor_state_interfaces_.size() + - gpio_state_interfaces_.size()); + unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + system_states_.insert(std::make_pair(name, state_interface)); + state_interfaces.push_back(state_interface); + } for (const auto & [name, descr] : joint_state_interfaces_) { @@ -252,13 +268,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. * * Note method name is going to be changed to export_command_interfaces() as soon as the * deprecated version is removed. * - * \return vector of shared pointers to the created and stored StateInterfaces + * \return vector of descriptions to the unlisted CommandInterfaces */ - virtual std::vector> export_command_interfaces_2() + virtual std::vector export_command_interfaces_2() { // return empty vector by default. return {}; @@ -273,9 +290,25 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ std::vector> on_export_command_interfaces() { - std::vector> command_interfaces = + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = export_command_interfaces_2(); - command_interfaces.reserve(joint_command_interfaces_.size() + gpio_command_interfaces_.size()); + + std::vector> command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + + gpio_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + system_commands_.insert(std::make_pair(name, command_interface)); + command_interfaces.push_back(command_interface); + } for (const auto & [name, descr] : joint_command_interfaces_) { @@ -403,10 +436,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map gpio_state_interfaces_; std::unordered_map gpio_command_interfaces_; - std::unordered_map> system_states_; - std::unordered_map> system_commands_; + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + +private: + std::unordered_map> system_states_; + std::unordered_map> system_commands_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 09ff91467a..40a646dea4 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -756,21 +756,4 @@ std::vector parse_command_interface_descriptions_from_hard return component_command_interface_descriptions; } -std::vector parse_gpio_command_interface_descriptions_from_hardware_info( - const HardwareInfo & hw_info) -{ - std::vector gpio_command_interface_descriptions; - gpio_command_interface_descriptions.reserve(hw_info.gpios.size()); - - for (const auto & gpio : hw_info.gpios) - { - for (const auto & command_interface : gpio.command_interfaces) - { - gpio_command_interface_descriptions.emplace_back( - InterfaceDescription(gpio.name, command_interface)); - } - } - return gpio_command_interface_descriptions; -} - } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp index fe76dc21ac..7a5011892b 100644 --- a/hardware_interface/test/test_component_interfaces_custom_export.cpp +++ b/hardware_interface/test/test_component_interfaces_custom_export.cpp @@ -57,27 +57,23 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface { std::string get_name() const override { return "DummyActuatorDefault"; } - std::vector> export_state_interfaces_2() - override + std::vector export_state_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - actuator_states_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; } - std::vector> export_command_interfaces_2() - override + std::vector export_command_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - actuator_commands_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; @@ -100,14 +96,12 @@ class DummySensorDefault : public hardware_interface::SensorInterface { std::string get_name() const override { return "DummySensorDefault"; } - std::vector> export_state_interfaces_2() - override + std::vector export_state_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.sensors[0].name, "some_unlisted_interface", nullptr); - sensor_states_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.sensors[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; @@ -124,27 +118,23 @@ class DummySystemDefault : public hardware_interface::SystemInterface { std::string get_name() const override { return "DummySystemDefault"; } - std::vector> export_state_interfaces_2() - override + std::vector export_state_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - system_states_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces; } - std::vector> export_command_interfaces_2() - override + std::vector export_command_interfaces_2() override { - std::vector> interfaces; - auto unlisted_state_interface = std::make_shared( - info_.joints[0].name, "some_unlisted_interface", nullptr); - system_commands_.insert( - std::make_pair(unlisted_state_interface->get_name(), unlisted_state_interface)); + std::vector interfaces; + hardware_interface::InterfaceInfo info; + info.name = "some_unlisted_interface"; + hardware_interface::InterfaceDescription unlisted_state_interface(info_.joints[0].name, info); interfaces.push_back(unlisted_state_interface); return interfaces;