From 0433960580c5834b11af9703e3e34ace86824d01 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Oct 2024 18:28:31 +0200 Subject: [PATCH] [PR-1689] Follow-up PR of the controller interface variants integration (#1779) --- .../chainable_controller_interface.hpp | 5 ++- .../include/controller_interface/helpers.hpp | 10 +++++ .../src/chainable_controller_interface.cpp | 37 +++++++++++++------ controller_manager/src/controller_manager.cpp | 8 ++-- 4 files changed, 43 insertions(+), 17 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index e4e4ec662c..6775724bfa 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -145,9 +145,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; // END - std::vector ordered_reference_interfaces_; + std::vector + ordered_exported_reference_interfaces_; std::unordered_map - reference_interfaces_ptrs_; + exported_reference_interfaces_; private: /// A flag marking if a chainable controller is currently preceded by another controller. diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index b571751f55..dfd29841bf 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -78,6 +78,16 @@ inline bool interface_list_contains_interface_type( interface_type_list.end(); } +template +void add_element_to_list(std::vector & list, const T & element) +{ + if (std::find(list.begin(), list.end(), element) == list.end()) + { + // Only add to the list if it doesn't exist + list.push_back(element); + } +} + } // namespace controller_interface #endif // CONTROLLER_INTERFACE__HELPERS_HPP_ diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index a6d427fe2b..409578be9b 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -16,6 +16,7 @@ #include +#include "controller_interface/helpers.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -67,7 +68,7 @@ ChainableControllerInterface::export_state_interfaces() throw std::runtime_error(error_msg); } auto state_interface = std::make_shared(interface); - const auto interface_name = state_interface->get_name(); + const auto interface_name = state_interface->get_interface_name(); auto [it, succ] = exported_state_interfaces_.insert({interface_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 @@ -87,11 +88,24 @@ ChainableControllerInterface::export_state_interfaces() throw std::runtime_error(error_msg); } ordered_exported_state_interfaces_.push_back(state_interface); - exported_state_interface_names_.push_back(interface_name); + add_element_to_list(exported_state_interface_names_, interface_name); state_interfaces_ptrs_vec.push_back( std::const_pointer_cast(state_interface)); } + if (exported_state_interfaces_.size() != state_interfaces.size()) + { + std::string error_msg = + "The internal storage for state interface ptrs 'exported_state_interfaces_' variable has " + "size '" + + std::to_string(exported_state_interfaces_.size()) + + "', but it is expected to have the size '" + std::to_string(state_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + return state_interfaces_ptrs_vec; } @@ -102,7 +116,7 @@ ChainableControllerInterface::export_reference_interfaces() std::vector reference_interfaces_ptrs_vec; reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); exported_reference_interface_names_.reserve(reference_interfaces.size()); - ordered_reference_interfaces_.reserve(reference_interfaces.size()); + ordered_exported_reference_interfaces_.reserve(reference_interfaces.size()); // BEGIN (Handle export change): for backward compatibility // check if the "reference_interfaces_" variable is resized to number of interfaces @@ -135,16 +149,16 @@ ChainableControllerInterface::export_reference_interfaces() hardware_interface::CommandInterface::SharedPtr reference_interface = std::make_shared(std::move(interface)); - const auto inteface_name = reference_interface->get_name(); + const auto interface_name = reference_interface->get_interface_name(); // check the exported interface name is unique - auto [it, succ] = reference_interfaces_ptrs_.insert({inteface_name, reference_interface}); + auto [it, succ] = exported_reference_interfaces_.insert({interface_name, reference_interface}); // either we have name duplicate which we want to avoid under all circumstances since interfaces // need to be uniquely identify able or something else really went wrong. In any case abort and // inform cm by throwing exception if (!succ) { std::string error_msg = - "Could not insert Reference interface<" + inteface_name + + "Could not insert Reference interface<" + interface_name + "> into reference_interfaces_ map. Check if you export duplicates. The " "map returned iterator with interface_name<" + it->second->get_name() + @@ -155,16 +169,17 @@ ChainableControllerInterface::export_reference_interfaces() reference_interfaces_ptrs_vec.clear(); throw std::runtime_error(error_msg); } - ordered_reference_interfaces_.push_back(reference_interface); - exported_reference_interface_names_.push_back(inteface_name); + ordered_exported_reference_interfaces_.push_back(reference_interface); + add_element_to_list(exported_reference_interface_names_, interface_name); reference_interfaces_ptrs_vec.push_back(reference_interface); } - if (reference_interfaces_ptrs_.size() != ref_interface_size) + if (exported_reference_interfaces_.size() != ref_interface_size) { std::string error_msg = - "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + - std::to_string(reference_interfaces_ptrs_.size()) + + "The internal storage for exported reference ptrs 'exported_reference_interfaces_' variable " + "has size '" + + std::to_string(exported_reference_interfaces_.size()) + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + "' equal to the number of exported reference interfaces. Please correct and recompile the " "controller with name '" + diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ad0ed298d2..b8a587f92e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -802,16 +802,16 @@ controller_interface::return_type ControllerManager::configure_controller( // 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 '%s' is chainable, but does not export any state or 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) + catch (const std::exception & e) { RCLCPP_FATAL( - get_logger(), "Creation of the reference interfaces failed with following error: %s", + get_logger(), "Export of the state or reference interfaces failed with following error: %s", e.what()); return controller_interface::return_type::ERROR; }