From ab84b74b079a9b6df887d36a84d8965b5342d19c Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 1 Oct 2024 15:11:34 +0100 Subject: [PATCH 01/15] Bump version of pre-commit hooks (#1770) --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 43bb778260..63e7f08682 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,7 +63,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.0 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -109,7 +109,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.2 + rev: 0.29.3 hooks: - id: check-github-workflows args: ["--verbose"] From 34d2b368ecf35e7e63102f7feb07c3498ffb1d89 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 7 Oct 2024 19:34:52 +0200 Subject: [PATCH 02/15] Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (#1689) --------- Co-authored-by: Bence Magyar --- .../chainable_controller_interface.hpp | 16 ++- .../controller_interface.hpp | 4 +- .../controller_interface_base.hpp | 5 +- .../src/chainable_controller_interface.cpp | 129 ++++++++++++++---- .../src/controller_interface.cpp | 6 +- .../test_chainable_controller_interface.cpp | 31 +++-- controller_manager/src/controller_manager.cpp | 29 ++-- .../loaned_command_interface.hpp | 11 +- .../loaned_state_interface.hpp | 16 ++- .../hardware_interface/resource_manager.hpp | 4 +- hardware_interface/src/resource_manager.cpp | 28 ++-- .../test/test_resource_manager.cpp | 4 +- 12 files changed, 202 insertions(+), 81 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 2e39f038b1..912e224c74 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -15,7 +15,9 @@ #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#include #include +#include #include #include "controller_interface/controller_interface_base.hpp" @@ -57,10 +59,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_state_interfaces() 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; @@ -131,11 +133,21 @@ class ChainableControllerInterface : public ControllerInterfaceBase /// Storage of values for state interfaces std::vector exported_state_interface_names_; + std::vector ordered_exported_state_interfaces_; + std::unordered_map + exported_state_interfaces_; + // BEGIN (Handle export change): for backward compatibility std::vector state_interfaces_values_; + // END /// Storage of values for reference interfaces std::vector exported_reference_interface_names_; + // BEGIN (Handle export change): for backward compatibility std::vector reference_interfaces_; + // END + 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 43fd269803..5c321b0e42 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -47,7 +47,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_state_interfaces() final; /** * Controller has no reference interfaces. @@ -55,7 +55,7 @@ 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 74077969d3..1f350abeef 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -235,7 +235,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; /** * Export interfaces for a chainable controller that can be used as state interface by other @@ -244,7 +245,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_state_interfaces() = 0; + virtual std::vector export_state_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 ae5cd326b6..ce19eff9b9 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,53 +44,134 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector +std::vector ChainableControllerInterface::export_state_interfaces() { auto state_interfaces = on_export_state_interfaces(); + std::vector state_interfaces_ptrs_vec; + state_interfaces_ptrs_vec.reserve(state_interfaces.size()); + ordered_exported_state_interfaces_.reserve(state_interfaces.size()); + exported_state_interface_names_.reserve(state_interfaces.size()); // check if the names of the controller state interfaces begin with the controller's name for (const auto & interface : state_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory for state interfaces. No state 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()); - state_interfaces.clear(); - break; + std::string error_msg = + "The prefix of the interface '" + interface.get_prefix_name() + + "' does not equal the controller's name '" + get_node()->get_name() + + "'. This is mandatory for state interfaces. No state interface will be exported. Please " + "correct and recompile the controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + auto state_interface = std::make_shared(interface); + const auto interface_name = state_interface->get_name(); + auto [it, succ] = exported_state_interfaces_.insert({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 + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert StateInterface<" + interface_name + + "> into exported_state_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + exported_state_interfaces_.clear(); + exported_state_interface_names_.clear(); + state_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); } + ordered_exported_state_interfaces_.push_back(state_interface); + exported_state_interface_names_.push_back(interface_name); + state_interfaces_ptrs_vec.push_back(state_interface); } - return state_interfaces; + return state_interfaces_ptrs_vec; } -std::vector +std::vector ChainableControllerInterface::export_reference_interfaces() { auto reference_interfaces = on_export_reference_interfaces(); + std::vector reference_interfaces_ptrs_vec; + reference_interfaces_ptrs_vec.reserve(reference_interfaces.size()); + exported_reference_interface_names_.reserve(reference_interfaces.size()); + ordered_reference_interfaces_.reserve(reference_interfaces.size()); + + // BEGIN (Handle export change): for backward compatibility + // check if the "reference_interfaces_" variable is resized to number of interfaces + if (reference_interfaces_.size() != reference_interfaces.size()) + { + std::string error_msg = + "The internal storage for reference values 'reference_interfaces_' variable has size '" + + std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" + + std::to_string(reference_interfaces.size()) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + // END // check if the names of the reference interfaces begin with the controller's name - for (const auto & interface : reference_interfaces) + const auto ref_interface_size = reference_interfaces.size(); + for (auto & interface : reference_interfaces) { if (interface.get_prefix_name() != get_node()->get_name()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "The name of the interface '%s' does not begin with the controller's name. This is " - "mandatory " - " for reference interfaces. No reference interface will be exported. Please correct and " - "recompile the controller with name '%s' and try again.", - interface.get_name().c_str(), get_node()->get_name()); - reference_interfaces.clear(); - break; + std::string error_msg = "The name of the interface " + interface.get_name() + + " does not begin with the controller's name. This is mandatory for " + "reference interfaces. Please " + "correct and recompile the controller with name " + + get_node()->get_name() + " and try again."; + throw std::runtime_error(error_msg); } + + hardware_interface::CommandInterface::SharedPtr reference_interface = + std::make_shared(std::move(interface)); + const auto inteface_name = reference_interface->get_name(); + // check the exported interface name is unique + auto [it, succ] = reference_interfaces_ptrs_.insert({inteface_name, reference_interface}); + // either we have name duplicate which we want to avoid under all circumstances since interfaces + // need to be uniquely identify able or something else really went wrong. In any case abort and + // inform cm by throwing exception + if (!succ) + { + std::string error_msg = + "Could not insert Reference interface<" + inteface_name + + "> into reference_interfaces_ map. Check if you export duplicates. The " + "map returned iterator with interface_name<" + + it->second->get_name() + + ">. If its a duplicate adjust exportation of InterfacesDescription so that all the " + "interface names are unique."; + reference_interfaces_.clear(); + exported_reference_interface_names_.clear(); + reference_interfaces_ptrs_vec.clear(); + throw std::runtime_error(error_msg); + } + ordered_reference_interfaces_.push_back(reference_interface); + exported_reference_interface_names_.push_back(inteface_name); + reference_interfaces_ptrs_vec.push_back(reference_interface); } - return reference_interfaces; + if (reference_interfaces_ptrs_.size() != ref_interface_size) + { + std::string error_msg = + "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" + + std::to_string(reference_interfaces_ptrs_.size()) + + "', but it is expected to have the size '" + std::to_string(ref_interface_size) + + "' equal to the number of exported reference interfaces. Please correct and recompile the " + "controller with name '" + + get_node()->get_name() + "' and try again."; + throw std::runtime_error(error_msg); + } + + return reference_interfaces_ptrs_vec; } bool ChainableControllerInterface::set_chained_mode(bool chained_mode) @@ -130,8 +211,8 @@ ChainableControllerInterface::on_export_state_interfaces() std::vector state_interfaces; for (size_t i = 0; i < exported_state_interface_names_.size(); ++i) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i])); + state_interfaces.emplace_back( + get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]); } return state_interfaces; } diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index a039501aa1..4a22d78666 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -22,12 +22,14 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector ControllerInterface::export_state_interfaces() +std::vector +ControllerInterface::export_state_interfaces() { return {}; } -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 2f04273f3e..5233fe0278 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -15,6 +15,7 @@ #include "test_chainable_controller_interface.hpp" #include +#include using ::testing::IsEmpty; using ::testing::SizeIs; @@ -48,10 +49,10 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces) auto exported_state_interfaces = controller.export_state_interfaces(); ASSERT_THAT(exported_state_interfaces, SizeIs(1)); - EXPECT_EQ(exported_state_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME); - EXPECT_EQ(exported_state_interfaces[0].get_interface_name(), "test_state"); + EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state"); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE); } TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) @@ -68,10 +69,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) auto reference_interfaces = controller.export_reference_interfaces(); ASSERT_THAT(reference_interfaces, SizeIs(1)); - 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, interfaces_prefix_is_not_node_name) @@ -88,10 +89,15 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); // expect empty return because interface prefix is not equal to the node name - auto reference_interfaces = controller.export_reference_interfaces(); - ASSERT_THAT(reference_interfaces, IsEmpty()); + std::vector exported_reference_interfaces; + EXPECT_THROW( + { exported_reference_interfaces = controller.export_reference_interfaces(); }, + std::runtime_error); + ASSERT_THAT(exported_reference_interfaces, IsEmpty()); // expect empty return because interface prefix is not equal to the node name - auto exported_state_interfaces = controller.export_state_interfaces(); + std::vector exported_state_interfaces; + EXPECT_THROW( + { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error); ASSERT_THAT(exported_state_interfaces, IsEmpty()); } @@ -114,8 +120,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(exported_state_interfaces[0].get_value(), EXPORTED_STATE_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()); @@ -124,11 +129,11 @@ 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()); - EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); + EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE); controller.configure(); EXPECT_TRUE(controller.set_chained_mode(false)); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b4f18dd90e..bad55e2e1c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -771,15 +771,28 @@ 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 state_interfaces = controller->export_state_interfaces(); - auto ref_interfaces = controller->export_reference_interfaces(); - if (ref_interfaces.empty() && state_interfaces.empty()) + std::vector state_interfaces; + std::vector ref_interfaces; + try { - // TODO(destogl): Add test for this! - RCLCPP_ERROR( - get_logger(), - "Controller '%s' is chainable, but does not export any state or reference interfaces.", - controller_name.c_str()); + state_interfaces = controller->export_state_interfaces(); + ref_interfaces = controller->export_reference_interfaces(); + if (ref_interfaces.empty() && state_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, ref_interfaces); diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..aa306870a1 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -28,16 +28,23 @@ class LoanedCommandInterface public: using Deleter = std::function; - explicit LoanedCommandInterface(CommandInterface & command_interface) + [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedCommandInterface( + CommandInterface & command_interface) : LoanedCommandInterface(command_interface, nullptr) { } - LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) + [[deprecated("Replaced by the new version using shared_ptr")]] LoanedCommandInterface( + CommandInterface & command_interface, Deleter && deleter) : command_interface_(command_interface), deleter_(std::forward(deleter)) { } + LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter) + : command_interface_(*command_interface), deleter_(std::forward(deleter)) + { + } + LoanedCommandInterface(const LoanedCommandInterface & other) = delete; LoanedCommandInterface(LoanedCommandInterface && other) = default; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..8fcc5bdb0b 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -28,16 +28,28 @@ class LoanedStateInterface public: using Deleter = std::function; - explicit LoanedStateInterface(StateInterface & state_interface) + [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedStateInterface( + StateInterface & state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) + [[deprecated("Replaced by the new version using shared_ptr")]] LoanedStateInterface( + StateInterface & state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } + explicit LoanedStateInterface(StateInterface::SharedPtr state_interface) + : LoanedStateInterface(state_interface, nullptr) + { + } + + LoanedStateInterface(StateInterface::SharedPtr state_interface, Deleter && deleter) + : state_interface_(*state_interface), deleter_(std::forward(deleter)) + { + } + LoanedStateInterface(const LoanedStateInterface & other) = delete; LoanedStateInterface(LoanedStateInterface && other) = default; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 8f9f142c25..ece45e3146 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -141,7 +141,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ void import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces); + const std::string & controller_name, std::vector & interfaces); /// Get list of exported tate interface of a controller. /** @@ -195,7 +195,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void import_controller_reference_interfaces( const std::string & controller_name, - std::vector & interfaces); + const std::vector & interfaces); /// Get list of reference interface of a controller. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index fe5e9ae904..4d0c41b242 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -656,10 +656,10 @@ class ResourceStorage { try { - std::vector interfaces = hardware.export_command_interfaces(); - + auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); + // TODO(Manuel) END: for backward compatibility } catch (const std::exception & ex) { @@ -1207,7 +1207,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 @@ -1241,16 +1241,10 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto & interface : interfaces) - { - interface_ptrs.push_back(std::make_shared(interface)); - } - auto interface_names = resource_storage_->add_state_interfaces(interface_ptrs); + auto interface_names = resource_storage_->add_state_interfaces(interfaces); resource_storage_->controllers_exported_state_interfaces_map_[controller_name] = interface_names; } @@ -1310,16 +1304,10 @@ void ResourceManager::remove_controller_exported_state_interfaces( // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, - std::vector & interfaces) + const std::vector & interfaces) { std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto & interface : interfaces) - { - interface_ptrs.push_back(std::make_shared(std::move(interface))); - } - auto interface_names = resource_storage_->add_command_interfaces(interface_ptrs); + auto interface_names = resource_storage_->add_command_interfaces(interfaces); resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } @@ -1452,7 +1440,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_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 51d81a90ab..e72a4a8214 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1230,12 +1230,12 @@ 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( + reference_interfaces.push_back(std::make_shared( CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &(reference_interface_values[i]))); } From b3615eaa0bdd1de12b3f4442efd2e57973e4f8ad Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 7 Oct 2024 19:15:08 +0100 Subject: [PATCH 03/15] Update changelogs --- controller_interface/CHANGELOG.rst | 7 +++++++ controller_manager/CHANGELOG.rst | 7 +++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 7 +++++++ hardware_interface_testing/CHANGELOG.rst | 5 +++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 5 +++++ ros2controlcli/CHANGELOG.rst | 5 +++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 51 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 7d9a50d899..ef8c8f7890 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* [ControllerInterface] Fix to properly propagate the controller NodeOptions (`#1762 `_) +* [Controller Interface] Make assign and release interfaces virtual (`#1743 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + 4.17.0 (2024-09-11) ------------------- * Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (`#1683 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index ab422cd13a..00d638ebb0 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* Add test coverage for `params_file` parameter in spawner/unspawner tests (`#1754 `_) +* [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota, Santosh Govindaraj + 4.17.0 (2024-09-11) ------------------- * Log exception type when catching the exception (`#1749 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 485af0d0f1..e3a38a26a1 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-09-11) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 19e0e6d980..0d26e63421 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_) +* [RM] Execute `error` callback of component on returning ERROR or with exception (`#1730 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + 4.17.0 (2024-09-11) ------------------- * Log exception type when catching the exception (`#1749 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index d70c618e2c..51dabde564 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) +* Contributors: Manuel Muth + 4.17.0 (2024-09-11) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 62076925b9..463549bda8 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-09-11) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 6e4e2435cd..d7b96bea1d 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-09-11) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 5112940255..5d27c3d9bf 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_) +* Contributors: Manuel Muth + 4.17.0 (2024-09-11) ------------------- * [HWItfs] Add key-value-storage to the InterfaceInfo (`#1421 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 0dc41eecdc..39ea5fdba5 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_) +* Contributors: Sai Kishor Kothakota + 4.17.0 (2024-09-11) ------------------- * [ros2controlcli] fix list_controllers when no controllers are loaded (`#1721 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index d28a2ab28f..56b185f5b5 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-09-11) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 2e523b066d..4c9b62267d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.17.0 (2024-09-11) ------------------- From 112baaf851362be779e2d203d94a6d4acddb5dd0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 7 Oct 2024 19:15:35 +0100 Subject: [PATCH 04/15] 4.18.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index ef8c8f7890..173f6f1bcf 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) * [ControllerInterface] Fix to properly propagate the controller NodeOptions (`#1762 `_) * [Controller Interface] Make assign and release interfaces virtual (`#1743 `_) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index e7719e053c..55ad0c5916 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.17.0 + 4.18.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 00d638ebb0..f714f056d9 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) * Add test coverage for `params_file` parameter in spawner/unspawner tests (`#1754 `_) * [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index d17db9c4ae..030fcd3f7a 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.17.0 + 4.18.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index e3a38a26a1..6b5007c6a9 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- 4.17.0 (2024-09-11) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 6f967ebda8..e733b3921b 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.17.0 + 4.18.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 0d26e63421..b21be43f60 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) * Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_) * [RM] Execute `error` callback of component on returning ERROR or with exception (`#1730 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 48bb6ae030..6af9bd002f 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.17.0 + 4.18.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 51dabde564..8a9835038c 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- * Adapt controller Reference/StateInterfaces to New Way of Exporting (variant support) (`#1689 `_) * Contributors: Manuel Muth diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 7c8e8e0b25..7d8ad0a7cd 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.17.0 + 4.18.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 463549bda8..cc31c4505f 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- 4.17.0 (2024-09-11) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 68bc4fc0a3..5f929868e0 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.17.0 + 4.18.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index d7b96bea1d..8d78dbdd63 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- 4.17.0 (2024-09-11) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index c0cf9ae4e1..08f08112df 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.17.0 + 4.18.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 5d27c3d9bf..a4750ed2e4 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- * Automatic Creation of Handles in HW, Adding Getters/Setters (variant support) (`#1688 `_) * Contributors: Manuel Muth diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index bb2c67dc3d..c3f22e7917 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.17.0 + 4.18.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 39ea5fdba5..f9c320c74f 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- * [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (`#1703 `_) * Contributors: Sai Kishor Kothakota diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index a7c714877c..c2e39ca926 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.17.0 + 4.18.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 41e2ac2c18..2634d726ef 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.17.0", + version="4.18.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 56b185f5b5..386b3b7f78 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- 4.17.0 (2024-09-11) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index b6fd5c2fe8..ad25593e34 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.17.0 + 4.18.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index e08ed3d41d..4e1bb84627 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.17.0", + version="4.18.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 4c9b62267d..1875547497 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.18.0 (2024-10-07) +------------------- 4.17.0 (2024-09-11) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 77bd1c22f8..1531e1e173 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.17.0 + 4.18.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 1c7a5d1cd0f4ae0da26c62459a4bf0611e3c95a7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 7 Oct 2024 20:32:32 +0200 Subject: [PATCH 05/15] [CM] Throw an exception when the components initially fail to be in the required state (#1729) --- controller_manager/src/controller_manager.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index bad55e2e1c..52bec445a3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -346,7 +346,14 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript RCLCPP_INFO( get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), state.label().c_str()); - resource_manager_->set_component_state(component, state); + if ( + resource_manager_->set_component_state(component, state) == + hardware_interface::return_type::ERROR) + { + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + state.label()); + } components_to_activate.erase(component); } } @@ -370,7 +377,14 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript { rclcpp_lifecycle::State active_state( State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - resource_manager_->set_component_state(component, active_state); + if ( + resource_manager_->set_component_state(component, active_state) == + hardware_interface::return_type::ERROR) + { + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + active_state.label()); + } } robot_description_notification_timer_->cancel(); } From a6e44abcfdc9a1246ac1c3e0d0e0091226f67953 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 8 Oct 2024 09:08:23 +0200 Subject: [PATCH 06/15] [RM/HW] Constify the exported state interfaces using ConstSharedPtr (#1767) --- .../chainable_controller_interface.hpp | 2 +- .../controller_interface/controller_interface.hpp | 2 +- .../controller_interface_base.hpp | 3 ++- .../src/chainable_controller_interface.cpp | 7 ++++--- controller_interface/src/controller_interface.cpp | 2 +- .../test/test_chainable_controller_interface.cpp | 2 +- controller_manager/src/controller_manager.cpp | 2 +- .../include/hardware_interface/actuator.hpp | 2 +- .../hardware_interface/actuator_interface.hpp | 10 +++++----- .../include/hardware_interface/handle.hpp | 1 + .../hardware_interface/loaned_state_interface.hpp | 10 +++++----- .../hardware_interface/resource_manager.hpp | 2 +- .../include/hardware_interface/sensor.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 10 +++++----- .../include/hardware_interface/system.hpp | 2 +- .../hardware_interface/system_interface.hpp | 14 +++++++------- hardware_interface/src/actuator.cpp | 6 +++--- hardware_interface/src/resource_manager.cpp | 11 ++++++----- hardware_interface/src/sensor.cpp | 6 +++--- hardware_interface/src/system.cpp | 6 +++--- 20 files changed, 53 insertions(+), 49 deletions(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 912e224c74..e4e4ec662c 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -59,7 +59,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_chainable() const final; CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_state_interfaces() final; CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 5c321b0e42..3455227e1d 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -47,7 +47,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC - std::vector export_state_interfaces() final; + std::vector export_state_interfaces() final; /** * Controller has no reference interfaces. diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 1f350abeef..2bd01cc326 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -245,7 +245,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns list of state interfaces for preceding controllers. */ CONTROLLER_INTERFACE_PUBLIC - virtual std::vector export_state_interfaces() = 0; + virtual std::vector + export_state_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 ce19eff9b9..a6d427fe2b 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -44,11 +44,11 @@ return_type ChainableControllerInterface::update( return ret; } -std::vector +std::vector ChainableControllerInterface::export_state_interfaces() { auto state_interfaces = on_export_state_interfaces(); - std::vector state_interfaces_ptrs_vec; + std::vector state_interfaces_ptrs_vec; state_interfaces_ptrs_vec.reserve(state_interfaces.size()); ordered_exported_state_interfaces_.reserve(state_interfaces.size()); exported_state_interface_names_.reserve(state_interfaces.size()); @@ -88,7 +88,8 @@ ChainableControllerInterface::export_state_interfaces() } ordered_exported_state_interfaces_.push_back(state_interface); exported_state_interface_names_.push_back(interface_name); - state_interfaces_ptrs_vec.push_back(state_interface); + state_interfaces_ptrs_vec.push_back( + std::const_pointer_cast(state_interface)); } return state_interfaces_ptrs_vec; diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 4a22d78666..b254da79d8 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -22,7 +22,7 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {} bool ControllerInterface::is_chainable() const { return false; } -std::vector +std::vector ControllerInterface::export_state_interfaces() { return {}; diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 5233fe0278..12d5599d45 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -95,7 +95,7 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name) std::runtime_error); ASSERT_THAT(exported_reference_interfaces, IsEmpty()); // expect empty return because interface prefix is not equal to the node name - std::vector exported_state_interfaces; + std::vector exported_state_interfaces; EXPECT_THROW( { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error); ASSERT_THAT(exported_state_interfaces, IsEmpty()); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 52bec445a3..a0d7586308 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -785,7 +785,7 @@ controller_interface::return_type ControllerManager::configure_controller( get_logger(), "Controller '%s' is chainable. Interfaces are being exported to resource manager.", controller_name.c_str()); - std::vector state_interfaces; + std::vector state_interfaces; std::vector ref_interfaces; try { diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 6834b9a74b..3b16a65261 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -71,7 +71,7 @@ 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(); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 123576d686..4cdd81b60f 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -169,7 +169,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is " "handled " "by the Framework.")]] virtual std::vector @@ -201,13 +201,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_unlisted_state_interface_descriptions(); - std::vector state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); @@ -220,7 +220,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod 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); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : joint_state_interfaces_) @@ -228,7 +228,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod auto state_interface = std::make_shared(descr); actuator_states_.insert(std::make_pair(name, state_interface)); joint_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } return state_interfaces; } diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index dddcfef6b7..6fe4f25663 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -139,6 +139,7 @@ class StateInterface : public Handle using Handle::Handle; using SharedPtr = std::shared_ptr; + using ConstSharedPtr = std::shared_ptr; }; class CommandInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 8fcc5bdb0b..96cc3e89df 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -29,23 +29,23 @@ class LoanedStateInterface using Deleter = std::function; [[deprecated("Replaced by the new version using shared_ptr")]] explicit LoanedStateInterface( - StateInterface & state_interface) + const StateInterface & state_interface) : LoanedStateInterface(state_interface, nullptr) { } [[deprecated("Replaced by the new version using shared_ptr")]] LoanedStateInterface( - StateInterface & state_interface, Deleter && deleter) + const StateInterface & state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } - explicit LoanedStateInterface(StateInterface::SharedPtr state_interface) + explicit LoanedStateInterface(StateInterface::ConstSharedPtr state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(StateInterface::SharedPtr state_interface, Deleter && deleter) + LoanedStateInterface(StateInterface::ConstSharedPtr state_interface, Deleter && deleter) : state_interface_(*state_interface), deleter_(std::forward(deleter)) { } @@ -78,7 +78,7 @@ class LoanedStateInterface double get_value() const { return state_interface_.get_value(); } protected: - StateInterface & state_interface_; + const StateInterface & state_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index ece45e3146..a5592fc492 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -141,7 +141,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] interfaces list of controller's state interfaces as StateInterfaces. */ void import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces); + const std::string & controller_name, std::vector & interfaces); /// Get list of exported tate interface of a controller. /** diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index edcd30cf21..ca570b78aa 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -71,7 +71,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 5d859cc4f1..50d79d1a45 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -154,7 +154,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -185,13 +185,13 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - virtual std::vector on_export_state_interfaces() + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_unlisted_state_interface_descriptions(); - std::vector state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + sensor_state_interfaces_.size()); @@ -203,7 +203,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : sensor_state_interfaces_) @@ -212,7 +212,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } return state_interfaces; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 432ccc1fc8..09adaa7190 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -71,7 +71,7 @@ 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(); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 4814f6f559..6b0652d3fe 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -190,7 +190,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \return vector of state interfaces */ [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " + "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " "by the Framework.")]] virtual std::vector export_state_interfaces() @@ -221,13 +221,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \return vector of shared pointers to the created and stored StateInterfaces */ - std::vector on_export_state_interfaces() + std::vector on_export_state_interfaces() { // import the unlisted interfaces std::vector unlisted_interface_descriptions = export_unlisted_state_interface_descriptions(); - std::vector state_interfaces; + std::vector state_interfaces; state_interfaces.reserve( unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); @@ -241,7 +241,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } for (const auto & [name, descr] : joint_state_interfaces_) @@ -249,21 +249,21 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI auto state_interface = std::make_shared(descr); system_states_.insert(std::make_pair(name, state_interface)); joint_states_.push_back(state_interface); - state_interfaces.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(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); + state_interfaces.push_back(std::const_pointer_cast(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); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); } return state_interfaces; } diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index aae16a79ee..b0ed1f7c80 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -204,7 +204,7 @@ const rclcpp_lifecycle::State & Actuator::error() return impl_->get_lifecycle_state(); } -std::vector Actuator::export_state_interfaces() +std::vector Actuator::export_state_interfaces() { // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed @@ -222,11 +222,11 @@ std::vector Actuator::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { - interface_ptrs.emplace_back(std::make_shared(interface)); + interface_ptrs.emplace_back(std::make_shared(interface)); } return interface_ptrs; // END: for backward compatibility diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4d0c41b242..e5445491c8 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -608,7 +608,7 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { - std::vector interfaces = hardware.export_state_interfaces(); + auto interfaces = hardware.export_state_interfaces(); const auto interface_names = add_state_interfaces(interfaces); RCLCPP_WARN( @@ -678,7 +678,7 @@ class ResourceStorage } } - std::string add_state_interface(StateInterface::SharedPtr interface) + std::string add_state_interface(StateInterface::ConstSharedPtr interface) { auto interface_name = interface->get_name(); const auto [it, success] = state_interface_map_.emplace(interface_name, interface); @@ -702,7 +702,8 @@ class ResourceStorage * \returns list of interface names that are added into internal storage. The output is used to * avoid additional iterations to cache interface names, e.g., for initializing info structures. */ - std::vector add_state_interfaces(std::vector & interfaces) + std::vector add_state_interfaces( + std::vector & interfaces) { std::vector interface_names; interface_names.reserve(interfaces.size()); @@ -1068,7 +1069,7 @@ 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_; @@ -1241,7 +1242,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_exported_state_interfaces( - const std::string & controller_name, std::vector & interfaces) + const std::string & controller_name, std::vector & interfaces) { std::lock_guard guard(resource_interfaces_lock_); auto interface_names = resource_storage_->add_state_interfaces(interfaces); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index f3dc7efba5..5da01368c9 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -203,7 +203,7 @@ const rclcpp_lifecycle::State & Sensor::error() return impl_->get_lifecycle_state(); } -std::vector Sensor::export_state_interfaces() +std::vector Sensor::export_state_interfaces() { // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed @@ -221,11 +221,11 @@ std::vector Sensor::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { - interface_ptrs.emplace_back(std::make_shared(interface)); + interface_ptrs.emplace_back(std::make_shared(interface)); } return interface_ptrs; // END: for backward compatibility diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 9f1a35d736..abae924dfc 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -202,7 +202,7 @@ const rclcpp_lifecycle::State & System::error() return impl_->get_lifecycle_state(); } -std::vector System::export_state_interfaces() +std::vector System::export_state_interfaces() { // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed @@ -220,11 +220,11 @@ std::vector System::export_state_interfaces() // BEGIN (Handle export change): for backward compatibility, can be removed if // export_command_interfaces() method is removed - std::vector interface_ptrs; + std::vector interface_ptrs; interface_ptrs.reserve(interfaces.size()); for (auto const & interface : interfaces) { - interface_ptrs.emplace_back(std::make_shared(interface)); + interface_ptrs.emplace_back(std::make_shared(interface)); } return interface_ptrs; // END: for backward compatibility From 88441b09b348c8b4c4159ca266184ec63dc4120f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Oct 2024 15:14:46 +0200 Subject: [PATCH 07/15] Improve launch utils to support the multiple controller names (#1782) --- .../controller_manager/launch_utils.py | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 3bbb050433..c64b893156 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -19,8 +19,8 @@ from launch_ros.actions import Node -def generate_load_controller_launch_description( - controller_name, controller_params_file=None, extra_spawner_args=[] +def generate_controllers_spawner_launch_description( + controller_names: list, controller_params_file=None, extra_spawner_args=[] ): """ Generate launch description for loading a controller using spawner. @@ -32,11 +32,11 @@ def generate_load_controller_launch_description( Examples -------- # Assuming the controller parameters are known to the controller_manager - generate_load_controller_launch_description('joint_state_broadcaster') + generate_controllers_spawner_launch_description(['joint_state_broadcaster']) # Passing controller parameter file to load the controller (Controller type is retrieved from config file) - generate_load_controller_launch_description( - 'joint_state_broadcaster', + generate_controllers_spawner_launch_description( + ['joint_state_broadcaster'], controller_params_file=os.path.join(get_package_share_directory('my_pkg'), 'config', 'controller_params.yaml'), extra_spawner_args=[--load-only] @@ -54,11 +54,13 @@ def generate_load_controller_launch_description( description="Wait until the node is interrupted and then unload controller", ) - spawner_arguments = [ - controller_name, - "--controller-manager", - LaunchConfiguration("controller_manager_name"), - ] + spawner_arguments = controller_names + spawner_arguments.extend( + [ + "--controller-manager", + LaunchConfiguration("controller_manager_name"), + ] + ) if controller_params_file: spawner_arguments += ["--param-file", controller_params_file] @@ -94,3 +96,13 @@ def generate_load_controller_launch_description( spawner, ] ) + + +def generate_load_controller_launch_description( + controller_name: str, controller_params_file=None, extra_spawner_args=[] +): + return generate_controllers_spawner_launch_description( + controller_names=[controller_name], + controller_params_file=controller_params_file, + extra_spawner_args=extra_spawner_args, + ) From 8cdded3a4a7fed085dc824bbebe4960f80cdf2b9 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Wed, 9 Oct 2024 17:08:43 +0200 Subject: [PATCH 08/15] Add `PoseSensor` semantic component (#1775) --- controller_interface/CMakeLists.txt | 10 ++ .../semantic_components/pose_sensor.hpp | 110 ++++++++++++++++++ controller_interface/package.xml | 1 + .../test/test_pose_sensor.cpp | 98 ++++++++++++++++ .../test/test_pose_sensor.hpp | 59 ++++++++++ doc/release_notes.rst | 1 + 6 files changed, 279 insertions(+) create mode 100644 controller_interface/include/semantic_components/pose_sensor.hpp create mode 100644 controller_interface/test/test_pose_sensor.cpp create mode 100644 controller_interface/test/test_pose_sensor.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 3dc3bc4d0a..cad5810ee5 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -34,6 +34,7 @@ target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BU if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) ament_add_gmock(test_controller_interface test/test_controller_interface.cpp) @@ -74,6 +75,15 @@ if(BUILD_TESTING) ament_target_dependencies(test_imu_sensor sensor_msgs ) + + ament_add_gmock(test_pose_sensor test/test_pose_sensor.cpp) + target_link_libraries(test_pose_sensor + controller_interface + hardware_interface::hardware_interface + ) + ament_target_dependencies(test_pose_sensor + geometry_msgs + ) endif() install( diff --git a/controller_interface/include/semantic_components/pose_sensor.hpp b/controller_interface/include/semantic_components/pose_sensor.hpp new file mode 100644 index 0000000000..60dbecd718 --- /dev/null +++ b/controller_interface/include/semantic_components/pose_sensor.hpp @@ -0,0 +1,110 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// 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 SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ +#define SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "semantic_components/semantic_component_interface.hpp" + +namespace semantic_components +{ + +class PoseSensor : public SemanticComponentInterface +{ +public: + /// Constructor for a standard pose sensor with interface names set based on sensor name. + explicit PoseSensor(const std::string & name) : SemanticComponentInterface{name, 7} + { + // Use standard interface names + interface_names_.emplace_back(name_ + '/' + "position.x"); + interface_names_.emplace_back(name_ + '/' + "position.y"); + interface_names_.emplace_back(name_ + '/' + "position.z"); + interface_names_.emplace_back(name_ + '/' + "orientation.x"); + interface_names_.emplace_back(name_ + '/' + "orientation.y"); + interface_names_.emplace_back(name_ + '/' + "orientation.z"); + interface_names_.emplace_back(name_ + '/' + "orientation.w"); + + // Set all sensor values to default value NaN + std::fill(position_.begin(), position_.end(), std::numeric_limits::quiet_NaN()); + std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits::quiet_NaN()); + } + + virtual ~PoseSensor() = default; + + /// Update and return position. + /*! + * Update and return current pose position from state interfaces. + * + * \return Array of position coordinates. + */ + std::array get_position() + { + for (size_t i = 0; i < 3; ++i) + { + position_[i] = state_interfaces_[i].get().get_value(); + } + + return position_; + } + + /// Update and return orientation + /*! + * Update and return current pose orientation from state interfaces. + * + * \return Array of orientation coordinates in xyzw convention. + */ + std::array get_orientation() + { + for (size_t i = 3; i < 7; ++i) + { + orientation_[i - 3] = state_interfaces_[i].get().get_value(); + } + + return orientation_; + } + + /// Fill pose message with current values. + /** + * Fill a pose message with current position and orientation from the state interfaces. + */ + bool get_values_as_message(geometry_msgs::msg::Pose & message) + { + // Update state from state interfaces + get_position(); + get_orientation(); + + // Set message values from current state + message.position.x = position_[0]; + message.position.y = position_[1]; + message.position.z = position_[2]; + message.orientation.x = orientation_[0]; + message.orientation.y = orientation_[1]; + message.orientation.z = orientation_[2]; + message.orientation.w = orientation_[3]; + + return true; + } + +protected: + std::array position_; + std::array orientation_; +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__POSE_SENSOR_HPP_ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 55ad0c5916..dce1d79f86 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -19,6 +19,7 @@ rclcpp_lifecycle ament_cmake_gmock + geometry_msgs sensor_msgs diff --git a/controller_interface/test/test_pose_sensor.cpp b/controller_interface/test/test_pose_sensor.cpp new file mode 100644 index 0000000000..1ceb7c32a6 --- /dev/null +++ b/controller_interface/test/test_pose_sensor.cpp @@ -0,0 +1,98 @@ +// Copyright (c) 2024, FZI Forschungszentrum Informatik +// +// 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 "test_pose_sensor.hpp" + +void PoseSensorTest::SetUp() +{ + full_interface_names_.reserve(size_); + for (const auto & interface_name : interface_names_) + { + full_interface_names_.emplace_back(sensor_name_ + '/' + interface_name); + } +} + +void PoseSensorTest::TearDown() { pose_sensor_.reset(nullptr); } + +TEST_F(PoseSensorTest, validate_all) +{ + // Create sensor + pose_sensor_ = std::make_unique(sensor_name_); + EXPECT_EQ(pose_sensor_->name_, sensor_name_); + + // Validate reserved space for interface_names_ and state_interfaces_ + // As state_interfaces_ are not defined yet, use capacity() + ASSERT_EQ(pose_sensor_->interface_names_.size(), size_); + ASSERT_EQ(pose_sensor_->state_interfaces_.capacity(), size_); + + // Validate default interface_names_ + EXPECT_TRUE(std::equal( + pose_sensor_->interface_names_.cbegin(), pose_sensor_->interface_names_.cend(), + full_interface_names_.cbegin(), full_interface_names_.cend())); + + // Get interface names + std::vector interface_names = pose_sensor_->get_state_interface_names(); + + // Assign values to position + hardware_interface::StateInterface position_x{ + sensor_name_, interface_names_[0], &position_values_[0]}; + hardware_interface::StateInterface position_y{ + sensor_name_, interface_names_[1], &position_values_[1]}; + hardware_interface::StateInterface position_z{ + sensor_name_, interface_names_[2], &position_values_[2]}; + + // Assign values to orientation + hardware_interface::StateInterface orientation_x{ + sensor_name_, interface_names_[3], &orientation_values_[0]}; + hardware_interface::StateInterface orientation_y{ + sensor_name_, interface_names_[4], &orientation_values_[1]}; + hardware_interface::StateInterface orientation_z{ + sensor_name_, interface_names_[5], &orientation_values_[2]}; + hardware_interface::StateInterface orientation_w{ + sensor_name_, interface_names_[6], &orientation_values_[3]}; + + // Create state interface vector in jumbled order + std::vector temp_state_interfaces; + temp_state_interfaces.reserve(7); + + temp_state_interfaces.emplace_back(position_z); + temp_state_interfaces.emplace_back(orientation_y); + temp_state_interfaces.emplace_back(orientation_x); + temp_state_interfaces.emplace_back(position_x); + temp_state_interfaces.emplace_back(orientation_w); + temp_state_interfaces.emplace_back(position_y); + temp_state_interfaces.emplace_back(orientation_z); + + // Assign interfaces + pose_sensor_->assign_loaned_state_interfaces(temp_state_interfaces); + EXPECT_EQ(pose_sensor_->state_interfaces_.size(), size_); + + // Validate correct position and orientation + EXPECT_EQ(pose_sensor_->get_position(), position_values_); + EXPECT_EQ(pose_sensor_->get_orientation(), orientation_values_); + + // Validate generated message + geometry_msgs::msg::Pose temp_message; + ASSERT_TRUE(pose_sensor_->get_values_as_message(temp_message)); + EXPECT_EQ(temp_message.position.x, position_values_[0]); + EXPECT_EQ(temp_message.position.y, position_values_[1]); + EXPECT_EQ(temp_message.position.z, position_values_[2]); + EXPECT_EQ(temp_message.orientation.x, orientation_values_[0]); + EXPECT_EQ(temp_message.orientation.y, orientation_values_[1]); + EXPECT_EQ(temp_message.orientation.z, orientation_values_[2]); + EXPECT_EQ(temp_message.orientation.w, orientation_values_[3]); + + // Release state interfaces + pose_sensor_->release_interfaces(); + ASSERT_EQ(pose_sensor_->state_interfaces_.size(), 0); +} diff --git a/controller_interface/test/test_pose_sensor.hpp b/controller_interface/test/test_pose_sensor.hpp new file mode 100644 index 0000000000..c2344caaa2 --- /dev/null +++ b/controller_interface/test/test_pose_sensor.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2024, FZI Forschungszentrum Informatik +// +// 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_POSE_SENSOR_HPP_ +#define TEST_POSE_SENSOR_HPP_ + +#include + +#include +#include +#include +#include + +#include "semantic_components/pose_sensor.hpp" + +class TestablePoseSensor : public semantic_components::PoseSensor +{ + FRIEND_TEST(PoseSensorTest, validate_all); + +public: + // Use default interface names + explicit TestablePoseSensor(const std::string & name) : PoseSensor{name} {} + + virtual ~TestablePoseSensor() = default; +}; + +class PoseSensorTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + +protected: + const size_t size_ = 7; + const std::string sensor_name_ = "test_pose_sensor"; + + std::vector full_interface_names_; + const std::vector interface_names_ = { + "position.x", "position.y", "position.z", "orientation.x", + "orientation.y", "orientation.z", "orientation.w"}; + + std::array position_values_ = {{1.1, 2.2, 3.3}}; + std::array orientation_values_ = {{4.4, 5.5, 6.6, 7.7}}; + + std::unique_ptr pose_sensor_; +}; + +#endif // TEST_POSE_SENSOR_HPP_ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 8320a81705..7d28b4390d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -25,6 +25,7 @@ For details see the controller_manager section. * The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) +* The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) controller_manager ****************** From 55eb3a89d17ec7a7196695ab4e3b775c81a41990 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Oct 2024 18:26:06 +0200 Subject: [PATCH 09/15] Improve diagnostics of Controllers, Hardware Components and Controller Manager (#1764) --- .../controller_manager/controller_manager.hpp | 4 + controller_manager/src/controller_manager.cpp | 104 +++++++++++++++++- 2 files changed, 105 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7c09377bb9..b8c05efcc0 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -443,6 +443,10 @@ class ControllerManager : public rclcpp::Node void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + void hardware_components_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + + void controller_manager_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + /** * @brief determine_controller_node_options - A method that retrieves the controller defined node * options and adapts them, based on if there is a params file to be loaded or the use_sim_time diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a0d7586308..ad0ed298d2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -281,6 +281,12 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + diagnostics_updater_.add( + "Hardware Components Activity", this, + &ControllerManager::hardware_components_diagnostic_callback); + diagnostics_updater_.add( + "Controller Manager Activity", this, + &ControllerManager::controller_manager_diagnostic_callback); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) @@ -2762,6 +2768,16 @@ controller_interface::return_type ControllerManager::check_preceeding_controller void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + atleast_one_hw_active = true; + break; + } + } // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); @@ -2775,13 +2791,95 @@ void ControllerManager::controller_activity_diagnostic_callback( stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); } - if (all_active) + if (!atleast_one_hw_active) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "No hardware components are currently active to activate controllers"); + } + else + { + if (controllers.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + } + } +} + +void ControllerManager::hardware_components_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + bool all_active = true; + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + stat.add(component_name, component_info.state.label()); + if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + all_active = false; + } + else + { + atleast_one_hw_active = true; + } + } + if (!is_resource_manager_initialized()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + } + else if (hw_components_info.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + } + else + { + if (!atleast_one_hw_active) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + "No hardware components are currently active"); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, all_active + ? "All hardware components are active" + : "Not all hardware components are active"); + } + } +} + +void ControllerManager::controller_manager_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("update_rate", std::to_string(get_update_rate())); + if (is_resource_manager_initialized()) { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "All controllers are active"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not all controllers are active"); + if (robot_description_.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Waiting for robot description...."); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "Resource Manager is not initialized properly!"); + } } } From 0433960580c5834b11af9703e3e34ace86824d01 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Oct 2024 18:28:31 +0200 Subject: [PATCH 10/15] [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; } From 0ba1428654cbbc69050f8d58eefc39c56b098cb6 Mon Sep 17 00:00:00 2001 From: Santosh Govindaraj <75157452+SantoshGovindaraj@users.noreply.github.com> Date: Tue, 15 Oct 2024 22:42:33 +0100 Subject: [PATCH 11/15] fix: call configure_controller on 'unconfigured' state instead load_controller (#1794) --- .../rqt_controller_manager/controller_manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 4cb6c01e09..20b2f47a1a 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -260,7 +260,7 @@ def _on_ctrl_menu(self, pos): if action is action_configure: configure_controller(self._node, self._cm_name, ctrl.name) elif action is action_spawn: - load_controller(self._node, self._cm_name, ctrl.name) + configure_controller(self._node, self._cm_name, ctrl.name) self._activate_controller(ctrl.name) else: # Assume controller isn't loaded From 0a2838a57638e97d474b3891cbe582e08c9e9f1f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 16 Oct 2024 16:45:03 +0200 Subject: [PATCH 12/15] [ros2controlcli] Fix the missing exported state interface printing (#1800) --- .../ros2controlcli/verb/list_controllers.py | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 6b6a42e3ce..593df643a7 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import warnings from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments @@ -50,10 +51,14 @@ def print_controller_state(c, args, col_width_name, col_width_state, col_width_t for connection in c.chain_connections: for reference in connection.reference_interfaces: print(f"\t\t{reference:20s}") - if args.reference_interfaces or args.verbose: + if args.reference_interfaces or args.exported_interfaces or args.verbose: print("\texported reference interfaces:") - for reference_interfaces in c.reference_interfaces: - print(f"\t\t{reference_interfaces}") + for reference_interface in c.reference_interfaces: + print(f"\t\t{reference_interface}") + if args.reference_interfaces or args.exported_interfaces or args.verbose: + print("\texported state interfaces:") + for exported_state_interface in c.exported_state_interfaces: + print(f"\t\t{exported_state_interface}") class ListControllersVerb(VerbExtension): @@ -86,6 +91,11 @@ def add_arguments(self, parser, cli_name): action="store_true", help="List controller's exported references", ) + parser.add_argument( + "--exported-interfaces", + action="store_true", + help="List controller's exported state and reference interfaces", + ) parser.add_argument( "--verbose", "-v", @@ -98,6 +108,13 @@ def main(self, *, args): with NodeStrategy(args).direct_node as node: response = list_controllers(node, args.controller_manager) + if args.reference_interfaces: + warnings.filterwarnings("always") + warnings.warn( + "The '--reference-interfaces' argument is deprecated and will be removed in future releases. Use '--exported-interfaces' instead.", + DeprecationWarning, + ) + if not response.controller: print("No controllers are currently loaded!") return 0 From 1d2d6173d3929e3ee5187ec55cfa19665fc001d1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 16 Oct 2024 17:20:38 +0200 Subject: [PATCH 13/15] Check the update_rate set to the controllers to be a valid one (#1788) --- .../src/controller_interface_base.cpp | 22 ++++++++++- .../test/test_controller_interface.cpp | 37 ++++++++++++++++--- .../test/test_controller_manager.cpp | 2 +- .../test/test_load_controller.cpp | 4 +- 4 files changed, 55 insertions(+), 10 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 61d2c780b8..e2c1fa480a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -28,13 +28,14 @@ return_type ControllerInterfaceBase::init( const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { urdf_ = urdf; + update_rate_ = cm_update_rate; node_ = std::make_shared( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces try { - auto_declare("update_rate", cm_update_rate); + auto_declare("update_rate", update_rate_); auto_declare("is_async", false); } catch (const std::exception & e) @@ -85,7 +86,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Other solution is to add check into the LifecycleNode if a transition is valid to trigger if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); + const auto update_rate = get_node()->get_parameter("update_rate").as_int(); + if (update_rate < 0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!"); + return get_lifecycle_state(); + } + if (update_rate_ != 0u && update_rate > update_rate_) + { + RCLCPP_WARN( + get_node()->get_logger(), + "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the " + "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.", + update_rate, update_rate_); + } + else + { + update_rate_ = static_cast(update_rate); + } is_async_ = get_node()->get_parameter("is_async").as_bool(); } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index f5c0a6b3de..4204d32f45 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -19,6 +19,7 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -46,8 +47,8 @@ TEST(TestableControllerInterface, init) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - // update_rate is set to default 0 - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 10u); // Even after configure is 10 controller.configure(); @@ -56,6 +57,32 @@ TEST(TestableControllerInterface, init) rclcpp::shutdown(); } +TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) +{ + // mocks the declaration of overrides parameters in a yaml file + rclcpp::init(0, nullptr); + + TestableControllerInterface controller; + // initialize, create node + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=-100"); + node_options = node_options.arguments(node_options_arguments); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options), + controller_interface::return_type::OK); + + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 1000u); + + // The configure should fail and the update rate should stay the same + ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(controller.get_update_rate(), 1000u); + rclcpp::shutdown(); +} + TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file @@ -70,7 +97,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) node_options_arguments.push_back("update_rate:=2812"); node_options = node_options.arguments(node_options_arguments); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), + controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options), controller_interface::return_type::OK); // initialize executor to be able to get parameter update @@ -85,8 +112,8 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) using namespace std::chrono_literals; std::this_thread::sleep_for(50ms); - // update_rate is set to default 0 because it is set on configure - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 5000u); // Even after configure is 0 controller.configure(); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 28e9f0477c..3bf56dd900 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -398,7 +398,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // if we do 2 times of the controller_manager update rate, the internal counter should be // similarly incremented EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate())); - EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + EXPECT_EQ(test_controller->get_update_rate(), cm_->get_update_rate()); auto deactivate_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 3645c06c24..ce7285fc49 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -66,13 +66,13 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_lifecycle_state().id()); - controller_if->get_node()->set_parameter({"update_rate", 1337}); + controller_if->get_node()->set_parameter({"update_rate", 50}); cm_->configure_controller("test_controller_01"); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - EXPECT_EQ(1337u, controller_if->get_update_rate()); + EXPECT_EQ(50u, controller_if->get_update_rate()); } class TestLoadedController : public TestLoadController From 4eabd255827741dc4ab8aa0fa7b80e3360b893d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 16 Oct 2024 20:25:59 +0200 Subject: [PATCH 14/15] [Feature] Fallback controllers (#1789) Co-authored-by: Bence Magyar --- controller_manager/CMakeLists.txt | 1 + controller_manager/doc/userdoc.rst | 12 + .../controller_manager/controller_manager.hpp | 18 + controller_manager/src/controller_manager.cpp | 269 ++++- .../test/test_controller/test_controller.cpp | 5 + .../test/test_controller/test_controller.hpp | 2 + .../test/test_controller_manager.cpp | 919 ++++++++++++++++++ doc/release_notes.rst | 1 + 8 files changed, 1221 insertions(+), 6 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 5dea15c0d1..e44fb5fb9b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -91,6 +91,7 @@ if(BUILD_TESTING) target_link_libraries(test_controller_manager controller_manager test_controller + test_chainable_controller ros2_control_test_assets::ros2_control_test_assets ) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index bc9f75384e..d3dd5e90c8 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -87,6 +87,18 @@ update_rate (mandatory; integer) Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. +.params_file + The absolute path to the YAML file with parameters for the controller. + The file should contain the parameters for the controller in the standard ROS 2 YAML format. + +.fallback_controllers + List of controllers that are activated as a fallback strategy, when the spawned controllers fail by returning ``return_type::ERROR`` during the ``update`` cycle. + It is recommended to add all the controllers needed for the fallback strategy to the list, including the chainable controllers whose interfaces are used by the main fallback controllers. + +.. warning:: + The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. + It is recommended to test the fallback strategy in simulation before deploying it on the real robot. + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b8c05efcc0..273b48b022 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -128,6 +128,12 @@ class ControllerManager : public rclcpp::Node return add_controller_impl(controller_spec); } + controller_interface::ControllerInterfaceBaseSharedPtr add_controller( + const ControllerSpec & controller_spec) + { + return add_controller_impl(controller_spec); + } + /// configure_controller Configure controller by name calling their "configure" method. /** * \param[in] controller_name as a string. @@ -418,6 +424,18 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// Checks if the fallback controllers of the given controllers are in the right + /// state, so they can be activated immediately + /** + * \param[in] controllers is a list of controllers to activate. + * \param[in] controller_it is the iterator pointing to the controller to be activated. + * \return return_type::OK if all fallback controllers are in the right state, otherwise + * return_type::ERROR. + */ + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it); + /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b8a587f92e..72cebaa1e5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -170,6 +170,41 @@ void controller_chain_spec_cleanup( } ctrl_chain_spec.erase(controller); } + +// Gets the list of active controllers that use the command interface of the given controller +void get_active_controllers_using_command_interfaces_of_controller( + const std::string & controller_name, + const std::vector & controllers, + std::vector & controllers_using_command_interfaces) +{ + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (it == controllers.end()) + { + RCLCPP_ERROR( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' not found in the list of controllers.", controller_name.c_str()); + return; + } + const auto cmd_itfs = it->c->command_interface_configuration().names; + for (const auto & cmd_itf : cmd_itfs) + { + for (const auto & controller : controllers) + { + const auto ctrl_cmd_itfs = controller.c->command_interface_configuration().names; + // check if the controller is active and has the command interface and make sure that it + // doesn't exist in the list already + if ( + is_controller_active(controller.c) && + std::find(ctrl_cmd_itfs.begin(), ctrl_cmd_itfs.end(), cmd_itf) != ctrl_cmd_itfs.end()) + { + add_element_to_list(controllers_using_command_interfaces, controller.info.name); + } + } + } +} + } // namespace namespace controller_manager @@ -536,6 +571,15 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty()) { + if ( + std::find(fallback_controllers.begin(), fallback_controllers.end(), controller_name) != + fallback_controllers.end()) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' cannot be a fallback controller for itself.", + controller_name.c_str()); + return nullptr; + } controller_spec.info.fallback_controllers_names = fallback_controllers; } @@ -1082,6 +1126,11 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } + if (status == controller_interface::return_type::OK) + { + status = check_fallback_controllers_state_pre_activation(controllers, controller_it); + } + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( @@ -2360,16 +2409,68 @@ controller_interface::return_type ControllerManager::update( } if (!failed_controllers_list.empty()) { - std::string failed_controllers; + const auto FALLBACK_STACK_MAX_SIZE = 500; + std::vector active_controllers_using_interfaces(failed_controllers_list); + active_controllers_using_interfaces.reserve(FALLBACK_STACK_MAX_SIZE); + std::vector cumulative_fallback_controllers; + cumulative_fallback_controllers.reserve(FALLBACK_STACK_MAX_SIZE); + + for (const auto & failed_ctrl : failed_controllers_list) + { + auto ctrl_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, failed_ctrl)); + if (ctrl_it != rt_controller_list.end()) + { + for (const auto & fallback_controller : ctrl_it->info.fallback_controllers_names) + { + cumulative_fallback_controllers.push_back(fallback_controller); + get_active_controllers_using_command_interfaces_of_controller( + fallback_controller, rt_controller_list, active_controllers_using_interfaces); + } + } + } + std::string controllers_string; + controllers_string.reserve(500); for (const auto & controller : failed_controllers_list) { - failed_controllers += "\n\t- " + controller; + controllers_string.append(controller); + controllers_string.append(" "); } RCLCPP_ERROR( - get_logger(), "Deactivating following controllers as their update resulted in an error :%s", - failed_controllers.c_str()); - - deactivate_controllers(rt_controller_list, failed_controllers_list); + get_logger(), "Deactivating controllers : [ %s] as their update resulted in an error!", + controllers_string.c_str()); + if (active_controllers_using_interfaces.size() > failed_controllers_list.size()) + { + controllers_string.clear(); + for (size_t i = failed_controllers_list.size(); + i < active_controllers_using_interfaces.size(); i++) + { + controllers_string.append(active_controllers_using_interfaces[i]); + controllers_string.append(" "); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !controllers_string.empty(), + "Deactivating controllers : [ %s] using the command interfaces needed for the fallback " + "controllers to activate.", + controllers_string.c_str()); + } + if (!cumulative_fallback_controllers.empty()) + { + controllers_string.clear(); + for (const auto & controller : cumulative_fallback_controllers) + { + controllers_string.append(controller); + controllers_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str()); + } + deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); + if (!cumulative_fallback_controllers.empty()) + { + activate_controllers(rt_controller_list, cumulative_fallback_controllers); + } } // there are controllers to (de)activate @@ -2765,6 +2866,162 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +controller_interface::return_type +ControllerManager::check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it) +{ + for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) + { + auto fb_ctrl_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); + if (fb_ctrl_it == controllers.end()) + { + RCLCPP_ERROR( + get_logger(), + "Unable to find the fallback controller : '%s' of the controller : '%s' " + "within the controller list", + fb_ctrl.c_str(), controller_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + if (!(is_controller_inactive(fb_ctrl_it->c) || is_controller_active(fb_ctrl_it->c))) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" + " need to be configured and be in inactive/active state!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + for (const auto & fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) + { + if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_cmd_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_ref_itfs = + resource_manager_->get_controller_reference_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == + exported_ref_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' command interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + for (const auto & fb_state_itf : fb_ctrl_it->c->state_interface_configuration().names) + { + if (!resource_manager_->state_interface_is_available(fb_state_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_state_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_state_itfs = + resource_manager_->get_controller_exported_state_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_state_itfs.begin(), exported_state_itfs.end(), fb_state_itf) == + exported_state_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' state interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + } + } + return controller_interface::return_type::OK; +} + void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7b9af6ecfc..ac89239e09 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -137,6 +137,11 @@ std::vector TestController::get_state_interface_data() const return state_intr_data; } +void TestController::set_external_commands_for_testing(const std::vector & commands) +{ + external_commands_for_testing_ = commands; +} + } // namespace test_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 4e5f827cd0..d57fd9ddd9 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -70,6 +70,8 @@ class TestController : public controller_interface::ControllerInterface const std::string & getRobotDescription() const; + void set_external_commands_for_testing(const std::vector & commands); + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 3bf56dd900..a016b20440 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -20,6 +20,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -531,3 +532,921 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); + +class TestControllerManagerFallbackControllers +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_same_controller_in_fallback_list) +{ + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + + const std::vector fallback_controllers = {test_controller_1_name, "random_ctrl2"}; + rclcpp::Parameter fallback_ctrls_parameter( + test_controller_1_name + std::string(".fallback_controllers"), fallback_controllers); + cm_->set_parameter(fallback_ctrls_parameter); + { + ControllerManagerRunner cm_runner(this); + ASSERT_EQ( + nullptr, + cm_->load_controller(test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME)); + } +} + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_controller_not_configured) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activation_simple_case) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_command_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {"random_non_existing_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_state_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.names = {"non_existing_state_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_valid_activation_if_one_or_more_fallback_controllers_are_already_active) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_3->set_command_interface_configuration(itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + } + + EXPECT_EQ(3u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + /// @note Here the order is important do not change the starting order + for (const auto & start_controller : {test_controller_3_name, test_controller_1_name}) + { + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {start_controller}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_multiple_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"joint1/position"}); + test_controller_4->set_exported_state_interface_names({"joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + + controller_spec.c = test_controller_4; + controller_spec.info.name = test_controller_4_name; + controller_spec.info.type = "test_chainable_controller::TestChainableController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_3 alone and it should fail + // as it doesn't have the chained state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_2, test_controller_3 and + // test_controller_4, and now it should work as all the controllers are in the list + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + // It is expected to place all the chainable interfaces first, so they can make the interfaces + // available + controller_spec.info.fallback_controllers_names = { + test_controller_4_name, test_controller_3_name, test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing( + {std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_4->get_lifecycle_state().id()); + } +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_other_failing_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"modified_joint1/position"}); + test_controller_4->set_exported_state_interface_names({"modified_joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + + controller_spec.c = test_controller_4; + controller_spec.info.name = test_controller_4_name; + controller_spec.info.type = "test_chainable_controller::TestChainableController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + { + // Now the controller_1 is set with test_controller_2 and test_controller_4_name and it should + // fail as it has an non existing state interface Start controller, will take effect at the end + // of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + } + + // Now unload the controller_1 and set it up again with test_controller_3 and + // test_controller_4_name and it should fail as it has an non existing state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + cm_->unload_controller(test_controller_4_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_3_name, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } +} diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 7d28b4390d..7fa7ae7f29 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -26,6 +26,7 @@ For details see the controller_manager section. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) +* The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) controller_manager ****************** From 83fff77e15631ff3982d427d16150c73eb8764bd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 17 Oct 2024 11:40:10 +0200 Subject: [PATCH 15/15] [Spawner] Add support for wildcard entries in the controller param files (#1724) --- .../controller_manager_services.py | 61 +++++-- controller_manager/doc/userdoc.rst | 54 ++++++ .../test_controller_spawner_with_type.yaml | 29 +++- .../test/test_spawner_unspawner.cpp | 160 +++++++++++++++--- doc/release_notes.rst | 1 + 5 files changed, 256 insertions(+), 49 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a1d3c0f5ad..102531f8e8 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -253,24 +253,57 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti ) -def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): +def get_parameter_from_param_file( + node, controller_name, namespace, parameter_file, parameter_name +): with open(parameter_file) as f: namespaced_controller = ( - controller_name if namespace == "/" else f"{namespace}/{controller_name}" + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" ) + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" parameters = yaml.safe_load(f) - if namespaced_controller in parameters: - value = parameters[namespaced_controller] - if not isinstance(value, dict) or "ros__parameters" not in value: + controller_param_dict = None + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}{namespaced_controller}", + ]: + if key in parameters: + if key == controller_name and namespace != "/": + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + ) + break + controller_param_dict = parameters[key] + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY][key] + + if controller_param_dict and ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - else: - return None + if ( + controller_param_dict + and ROS_PARAMS_KEY in controller_param_dict + and parameter_name in controller_param_dict[ROS_PARAMS_KEY] + ): + break + + if controller_param_dict is None: + node.get_logger().fatal( + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}" + ) + if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + + return None def set_controller_parameters( @@ -324,7 +357,7 @@ def set_controller_parameters_from_param_file( ) controller_type = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "type" + node, controller_name, spawner_namespace, parameter_file, "type" ) if controller_type: if not set_controller_parameters( @@ -333,7 +366,7 @@ def set_controller_parameters_from_param_file( return False fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "fallback_controllers" + node, controller_name, spawner_namespace, parameter_file, "fallback_controllers" ) if fallback_controllers: if not set_controller_parameters( diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index d3dd5e90c8..312e7c68ef 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -183,6 +183,60 @@ There are two scripts to interact with controller manager from launch files: param file +The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: + + .. code-block:: yaml + + /**/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /rrbot_1/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + ``unspawner`` ^^^^^^^^^^^^^^^^ diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 892427bab7..087994bd23 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -3,25 +3,40 @@ ctrl_with_parameters_and_type: type: "controller_manager/test_controller" joint_names: ["joint0"] -chainable_ctrl_with_parameters_and_type: - ros__parameters: - type: "controller_manager/test_chainable_controller" - joint_names: ["joint1"] +/**: + chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + + wildcard_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] -/foo_namespace/ctrl_with_parameters_and_type: +/foo_namespace/ns_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint1"] -/foo_namespace/chainable_ctrl_with_parameters_and_type: +/foo_namespace/ns_chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +/foo_namespace/ns_ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] + +/**/wildcard_chainable_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_chainable_controller" joint_names: ["joint1"] -/foo_namespace/ctrl_with_parameters_and_no_type: +/**/wildcard_ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index cba832f8bf..74e1efeeed 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -674,14 +674,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 0); @@ -689,17 +689,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -707,12 +708,12 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail as no type is defined!"; @@ -720,21 +721,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); } TEST_F( @@ -747,28 +745,28 @@ TEST_F( // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + test_file_path + " --ros-args -r __ns:=/random_namespace"), 256) << "Should fail when parsed namespace through both way with different namespaces"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail when parsed namespace through both ways even with same namespacing name"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace -p " + test_file_path), 0) @@ -777,17 +775,18 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -795,12 +794,13 @@ TEST_F( chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace " + "-p " + test_file_path), 256) << "Should fail as no type is defined!"; @@ -808,19 +808,123 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); +} + +TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it due to timeout but can find the parameters"; + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, + "wildcard_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "wildcard_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F( + TestLoadControllerWithNamespacedCM, + spawner_test_fail_namespaced_controllers_with_non_wildcard_entries) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 256) + << "Should fail even namespacing it as ctrl_with_parameters_and_type is not a wildcard entry"; + EXPECT_EQ( + call_spawner( + "chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 0) + << "Should work as chainable_ctrl_with_parameters_and_type is a wildcard entry"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 7fa7ae7f29..611e8d8176 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -75,6 +75,7 @@ controller_manager * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). +* Added support for the wildcard entries for the controller configuration files (`#1724 `_). hardware_interface ******************