diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index a2b23f01ae..dd0c6b835f 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -14,6 +14,7 @@ add_library( SHARED src/controller_interface_base.cpp src/controller_interface.cpp + src/chainable_controller_interface.cpp ) target_include_directories( controller_interface @@ -52,6 +53,11 @@ if(BUILD_TESTING) target_link_libraries(test_controller_with_options controller_interface) target_include_directories(test_controller_with_options PRIVATE include) + ament_add_gmock(test_chainable_controller_interface test/test_chainable_controller_interface.cpp) + target_link_libraries(test_chainable_controller_interface controller_interface) + target_include_directories(test_chainable_controller_interface PRIVATE include) + ament_target_dependencies(test_chainable_controller_interface hardware_interface) + ament_add_gmock( test_semantic_component_interface test/test_semantic_component_interface.cpp diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp new file mode 100644 index 0000000000..7cf1f26111 --- /dev/null +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -0,0 +1,115 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ + +#include + +#include "controller_interface/controller_interface_base.hpp" +#include "controller_interface/visibility_control.h" +#include "hardware_interface/handle.hpp" + +namespace controller_interface +{ +/// Virtual class to implement when integrating a controller that can be preceded by other +/// controllers. +/** + * Specialization of ControllerInterface class to force implementation of methods specific for + * "chainable" controller, i.e., controller that can be preceded by an another controller, for + * example inner controller of an control cascade. + * + */ +class ChainableControllerInterface : public ControllerInterfaceBase +{ +public: + CONTROLLER_INTERFACE_PUBLIC + ChainableControllerInterface(); + + CONTROLLER_INTERFACE_PUBLIC + virtual ~ChainableControllerInterface() = default; + + CONTROLLER_INTERFACE_PUBLIC + return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) final; + + CONTROLLER_INTERFACE_PUBLIC + bool is_chainable() const final; + + CONTROLLER_INTERFACE_PUBLIC + std::vector export_reference_interfaces() final; + + CONTROLLER_INTERFACE_PUBLIC + bool set_chained_mode(bool chained_mode) final; + + CONTROLLER_INTERFACE_PUBLIC + bool is_in_chained_mode() const final; + +protected: + /// Virtual method that each chainable controller should implement to export its chainable + /// interfaces. + /** + * Each chainable controller implements this methods where all input (command) interfaces are + * exported. The method has the same meaning as `export_command_interface` method from + * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. + * + * \returns list of CommandInterfaces that other controller can use as their outputs. + */ + virtual std::vector on_export_reference_interfaces() = 0; + + /// Virtual method that each chainable controller should implement to switch chained mode. + /** + * Each chainable controller implements this methods to switch between "chained" and "external" + * mode. In "chained" mode all external interfaces like subscriber and service servers are + * disabled to avoid potential concurrency in input commands. + * + * \param[in] flag marking a switch to or from chained mode. + * + * \returns true if controller successfully switched between "chained" and "external" mode. \default returns true so the method don't have to be overridden if controller can always switch chained mode. + */ + virtual bool on_set_chained_mode(bool chained_mode); + + /// Update reference from input topics when not in chained mode. + /** + * Each chainable controller implements this method to update reference from subscribers when not + * in chained mode. + * + * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. + */ + virtual return_type update_reference_from_subscribers() = 0; + + /// Execute calculations of the controller and update command interfaces. + /** + * Update method for chainable controllers. + * In this method is valid to assume that \reference_interfaces_ hold the values for calculation + * of the commands in the current control step. + * This means that this method is called after \update_reference_from_subscribers if controller is + * not in chained mode. + * + * \returns return_type::OK if calculation and writing of interface is successfully, otherwise + * return_type::ERROR. + */ + virtual return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + + /// Storage of values for reference interfaces + std::vector reference_interfaces_; + +private: + /// A flag marking if a chainable controller is currently preceded by another controller. + bool in_chained_mode_ = false; +}; + +} // namespace controller_interface + +#endif // CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_ diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index 91f4e6eef8..efdab02d62 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -57,6 +57,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase */ CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; + /** * Controller can not be in chained mode. * diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp new file mode 100644 index 0000000000..aa6e39782b --- /dev/null +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -0,0 +1,116 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "controller_interface/chainable_controller_interface.hpp" + +#include + +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +namespace controller_interface +{ +ChainableControllerInterface::ChainableControllerInterface() : ControllerInterfaceBase() {} + +bool ChainableControllerInterface::is_chainable() const { return true; } + +return_type ChainableControllerInterface::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + return_type ret = return_type::ERROR; + + if (!is_in_chained_mode()) + { + ret = update_reference_from_subscribers(); + if (ret != return_type::OK) + { + return ret; + } + } + + ret = update_and_write_commands(time, period); + + return ret; +} + +std::vector +ChainableControllerInterface::export_reference_interfaces() +{ + auto reference_interfaces = on_export_reference_interfaces(); + + // check if the "reference_interfaces_" variable is resized to number of interfaces + if (reference_interfaces_.size() != reference_interfaces.size()) + { + // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the + // framework + RCLCPP_FATAL( + get_node()->get_logger(), + "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', " + "but it is expected to have the size '%zu' equal to the number of exported reference " + "interfaces. No reference interface will be exported. Please correct and recompile " + "the controller with name '%s' and try again.", + reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name()); + reference_interfaces.clear(); + } + + // check if the names of the reference interfaces begin with the controller's name + for (const auto & interface : reference_interfaces) + { + if (interface.get_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_full_name().c_str(), get_node()->get_name()); + reference_interfaces.clear(); + break; + } + } + + return reference_interfaces; +} + +bool ChainableControllerInterface::set_chained_mode(bool chained_mode) +{ + bool result = false; + + if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + result = on_set_chained_mode(chained_mode); + + if (result) + { + in_chained_mode_ = chained_mode; + } + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Can not change controller's chained mode because it is no in '%s' state. " + "Current state is '%s'.", + hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str()); + } + + return result; +} + +bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; } + +bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } + +} // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp new file mode 100644 index 0000000000..6839d595ea --- /dev/null +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -0,0 +1,174 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_chainable_controller_interface.hpp" + +#include + +TEST_F(ChainableControllerInterfaceTest, default_returns) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + EXPECT_TRUE(controller.is_chainable()); + EXPECT_FALSE(controller.is_in_chained_mode()); +} + +TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + auto reference_interfaces = controller.export_reference_interfaces(); + + ASSERT_EQ(reference_interfaces.size(), 1u); + EXPECT_EQ(reference_interfaces[0].get_name(), TEST_CONTROLLER_NAME); + EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf"); + + EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); +} + +TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + // expect empty return because storage is not resized + controller.reference_interfaces_.clear(); + auto reference_interfaces = controller.export_reference_interfaces(); + ASSERT_TRUE(reference_interfaces.empty()); +} + +TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node_name) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + 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_TRUE(reference_interfaces.empty()); +} + +TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + auto reference_interfaces = controller.export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), 1u); + + EXPECT_FALSE(controller.is_in_chained_mode()); + + // Fail setting chained mode + EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE); + + EXPECT_FALSE(controller.set_chained_mode(true)); + EXPECT_FALSE(controller.is_in_chained_mode()); + + EXPECT_FALSE(controller.set_chained_mode(false)); + EXPECT_FALSE(controller.is_in_chained_mode()); + + // Success setting chained mode + reference_interfaces[0].set_value(0.0); + + EXPECT_TRUE(controller.set_chained_mode(true)); + EXPECT_TRUE(controller.is_in_chained_mode()); + + controller.configure(); + EXPECT_TRUE(controller.set_chained_mode(false)); + EXPECT_FALSE(controller.is_in_chained_mode()); + + controller.get_node()->activate(); + // Can not change chained mode until in "ACTIVE" state + EXPECT_FALSE(controller.set_chained_mode(true)); + EXPECT_FALSE(controller.is_in_chained_mode()); + + controller.get_node()->deactivate(); + EXPECT_TRUE(controller.set_chained_mode(true)); + EXPECT_TRUE(controller.is_in_chained_mode()); + + // Can change 'chained' mode only in "UNCONFIGURED" state + controller.get_node()->cleanup(); + EXPECT_TRUE(controller.set_chained_mode(false)); + EXPECT_FALSE(controller.is_in_chained_mode()); +} + +TEST_F(ChainableControllerInterfaceTest, test_update_logic) +{ + TestableChainableControllerInterface controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + EXPECT_FALSE(controller.is_in_chained_mode()); + + // call update and update it from subscriber because not in chained mode + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); + + // Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec. + controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); + ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1); + + // Provoke error from update - return ERROR, but reference interface is updated and not reduced + controller.set_new_reference_interface_value(INTERFACE_VALUE_UPDATE_ERROR); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); + ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); + + controller.reference_interfaces_[0] = 0.0; + + EXPECT_TRUE(controller.set_chained_mode(true)); + EXPECT_TRUE(controller.is_in_chained_mode()); + + // Provoke error in update from subscribers - return OK because update of subscribers is not used + // reference interface is not updated (updated directly because in chained mode) + controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + ASSERT_EQ(controller.reference_interfaces_[0], -1.0); + + // Provoke error from update - return ERROR, but reference interface is updated directly + controller.set_new_reference_interface_value(INTERFACE_VALUE_SUBSCRIBER_ERROR); + controller.reference_interfaces_[0] = INTERFACE_VALUE_UPDATE_ERROR; + ASSERT_EQ( + controller.update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); + ASSERT_EQ(controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR); +} diff --git a/controller_interface/test/test_chainable_controller_interface.hpp b/controller_interface/test/test_chainable_controller_interface.hpp new file mode 100644 index 0000000000..38ce124582 --- /dev/null +++ b/controller_interface/test/test_chainable_controller_interface.hpp @@ -0,0 +1,135 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_ +#define TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_ + +#include +#include + +#include "gmock/gmock.h" + +#include "controller_interface/chainable_controller_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller"; +constexpr double INTERFACE_VALUE = 1989.0; +constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0; +constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0; +constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0; + +class TestableChainableControllerInterface +: public controller_interface::ChainableControllerInterface +{ +public: + FRIEND_TEST(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size); + FRIEND_TEST(ChainableControllerInterfaceTest, test_update_logic); + + TestableChainableControllerInterface() + { + reference_interfaces_.reserve(1); + reference_interfaces_.push_back(INTERFACE_VALUE); + } + + controller_interface::CallbackReturn on_init() override + { + // set default value + name_prefix_of_reference_interfaces_ = get_node()->get_name(); + + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + // Implementation of ChainableController virtual methods + std::vector on_export_reference_interfaces() override + { + std::vector command_interfaces; + + command_interfaces.push_back(hardware_interface::CommandInterface( + name_prefix_of_reference_interfaces_, "test_itf", &reference_interfaces_[0])); + + return command_interfaces; + } + + bool on_set_chained_mode(bool /*chained_mode*/) override + { + if (reference_interfaces_[0] == 0.0) + { + return true; + } + else + { + return false; + } + } + + controller_interface::return_type update_reference_from_subscribers() override + { + if (reference_interface_value_ == INTERFACE_VALUE_SUBSCRIBER_ERROR) + { + return controller_interface::return_type::ERROR; + } + + reference_interfaces_[0] = reference_interface_value_; + return controller_interface::return_type::OK; + } + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + if (reference_interfaces_[0] == INTERFACE_VALUE_UPDATE_ERROR) + { + return controller_interface::return_type::ERROR; + } + + reference_interfaces_[0] -= 1; + + return controller_interface::return_type::OK; + } + + void set_name_prefix_of_reference_interfaces(const std::string & prefix) + { + name_prefix_of_reference_interfaces_ = prefix; + } + + void set_new_reference_interface_value(const double ref_itf_value) + { + reference_interface_value_ = ref_itf_value; + } + + std::string name_prefix_of_reference_interfaces_; + double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF; +}; + +class ChainableControllerInterfaceTest : public ::testing::Test +{ +public: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() { rclcpp::shutdown(); } +}; + +#endif // TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_ diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 252d286635..f27fc43283 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -140,3 +140,21 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) rclcpp::shutdown(); } + +TEST(TestableControllerInterface, default_returns_for_chainable_controllers_methods) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + + TestableControllerInterface controller; + + EXPECT_FALSE(controller.is_chainable()); + EXPECT_TRUE(controller.export_reference_interfaces().empty()); + EXPECT_FALSE(controller.set_chained_mode(true)); + EXPECT_FALSE(controller.is_in_chained_mode()); + EXPECT_FALSE(controller.set_chained_mode(false)); + EXPECT_FALSE(controller.is_in_chained_mode()); + + rclcpp::shutdown(); +} diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 15b3a715b3..e3f253c5e7 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -20,6 +20,7 @@ #include #include +#include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" @@ -251,6 +252,8 @@ class ControllerManager : public rclcpp::Node std::shared_ptr executor_; std::shared_ptr> loader_; + std::shared_ptr> + chainable_loader_; /// Best effort (non real-time safe) callback group, e.g., service callbacks. /** diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 25ab6abbb5..19eaed5fa9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -33,6 +33,8 @@ namespace static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; static constexpr const char * kControllerInterfaceClassName = "controller_interface::ControllerInterface"; +static constexpr const char * kChainableControllerInterfaceClassName = + "controller_interface::ChainableControllerInterface"; // Changed services history QoS to keep all so we don't lose any client service calls static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { @@ -93,7 +95,10 @@ ControllerManager::ControllerManager( resource_manager_(std::make_unique()), executor_(executor), loader_(std::make_shared>( - kControllerInterfaceNamespace, kControllerInterfaceClassName)) + kControllerInterfaceNamespace, kControllerInterfaceClassName)), + chainable_loader_( + std::make_shared>( + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { if (!get_parameter("update_rate", update_rate_)) { @@ -120,7 +125,10 @@ ControllerManager::ControllerManager( resource_manager_(std::move(resource_manager)), executor_(executor), loader_(std::make_shared>( - kControllerInterfaceNamespace, kControllerInterfaceClassName)) + kControllerInterfaceNamespace, kControllerInterfaceClassName)), + chainable_loader_( + std::make_shared>( + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { init_services(); } @@ -232,7 +240,9 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c { RCLCPP_INFO(get_logger(), "Loading controller '%s'", controller_name.c_str()); - if (!loader_->isClassAvailable(controller_type)) + if ( + !loader_->isClassAvailable(controller_type) && + !chainable_loader_->isClassAvailable(controller_type)) { RCLCPP_ERROR(get_logger(), "Loader for controller '%s' not found.", controller_name.c_str()); RCLCPP_INFO(get_logger(), "Available classes:"); @@ -240,11 +250,23 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c { RCLCPP_INFO(get_logger(), " %s", available_class.c_str()); } + for (const auto & available_class : chainable_loader_->getDeclaredClasses()) + { + RCLCPP_INFO(get_logger(), " %s", available_class.c_str()); + } return nullptr; } controller_interface::ControllerInterfaceBaseSharedPtr controller; - controller = loader_->createSharedInstance(controller_type); + + if (loader_->isClassAvailable(controller_type)) + { + controller = loader_->createSharedInstance(controller_type); + } + if (chainable_loader_->isClassAvailable(controller_type)) + { + controller = chainable_loader_->createSharedInstance(controller_type); + } ControllerSpec controller_spec; controller_spec.c = controller; @@ -986,6 +1008,13 @@ void ControllerManager::list_controller_types_srv_cb( response->base_classes.push_back(kControllerInterfaceClassName); RCLCPP_DEBUG(get_logger(), "%s", cur_type.c_str()); } + cur_types = chainable_loader_->getDeclaredClasses(); + for (const auto & cur_type : cur_types) + { + response->types.push_back(cur_type); + response->base_classes.push_back(kChainableControllerInterfaceClassName); + RCLCPP_DEBUG(get_logger(), "%s", cur_type.c_str()); + } RCLCPP_DEBUG(get_logger(), "list types service finished"); } @@ -1177,6 +1206,9 @@ void ControllerManager::reload_controller_libraries_service_cb( // Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders) loader_ = std::make_shared>( kControllerInterfaceNamespace, kControllerInterfaceClassName); + chainable_loader_ = + std::make_shared>( + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName); RCLCPP_INFO( get_logger(), "Controller manager: reloaded controller libraries for '%s'", kControllerInterfaceNamespace);