diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 77ba823e1f..61ac5c8f20 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -101,6 +101,15 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) + ament_add_gmock(test_controller_manager_with_namespaced_controllers + test/test_controller_manager_with_namespaced_controllers.cpp + ) + target_link_libraries(test_controller_manager_with_namespaced_controllers + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + ament_add_gmock(test_controller_manager_hardware_error_handling test/test_controller_manager_hardware_error_handling.cpp ) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c4887db15a..4136610ef0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -68,6 +68,64 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const return a.info.name == name; } +// Pass in full_name and the namespace of the manager node, receive +/** + * \brief Creates controller naming + * + * A command, that based on the full name of the controller (e.g. from load_controller service call) + * calculates the namespace, name and parameter name of the controller. + * + * If the passed in name does not start with a '/' it is assumed to + * be relative to the manager namespace. + * + * If the passed in name starts with a '/' it is assumed to be + * relative to the root namespace. In this case the parameter + * is the full name with the initial '/' removed and all other + * '/' replaced with '.'. + * + * \param[in] passed_in_name + * \param[in] manager_namespace + * \param[out] node_namespace + * \param[out] node_name + * \param[out] node_parameter_name + */ +void determine_controller_namespace( + const std::string passed_in_name, const std::string manager_namespace, + std::string & node_namespace, std::string & node_name, std::string & node_parameter_name) +{ + const auto split_pos = passed_in_name.find_last_of('/'); + if (split_pos == std::string::npos) + { + node_name = passed_in_name; + } + else + { + node_name = passed_in_name.substr(split_pos + 1); + } + const auto first_occ = passed_in_name.find_first_of('/'); + if (first_occ == std::string::npos) + { + node_namespace = manager_namespace; + node_parameter_name = passed_in_name + ".type"; + } + else if (first_occ != 0) + { + node_namespace = manager_namespace + '/' + passed_in_name.substr(0, split_pos); + node_parameter_name = std::regex_replace(passed_in_name, std::regex("/"), ".") + ".type"; + } + else + { + node_namespace = passed_in_name.substr(0, split_pos); + node_parameter_name = + std::regex_replace(passed_in_name.substr(1), std::regex("/"), ".") + ".type"; + } + + RCLCPP_INFO( + rclcpp::get_logger("split_namespace_and_name"), + "node_namespace: %s, node_name: %s, node_param %s", node_namespace.c_str(), node_name.c_str(), + node_parameter_name.c_str()); +} + /// Checks if a command interface belongs to a controller based on its prefix. /** * A command interface can be provided by a controller in which case is called "reference" @@ -455,7 +513,10 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( const std::string & controller_name) { - const std::string param_name = controller_name + ".type"; + std::string controller_name_temp, controller_namespace, param_name; + calculate_controller_naming( + controller_name, get_namespace(), controller_namespace, controller_name_temp, param_name); + std::string controller_type; // We cannot declare the parameters for the controllers that will be loaded in the future, @@ -1138,9 +1199,12 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co controller.info.name.c_str()); return nullptr; } - + std::string controller_namespace_, controller_name_, controller_param_name_; + calculate_controller_naming( + controller.info.name, get_namespace(), controller_namespace_, controller_name_, + controller_param_name_); if ( - controller.c->init(controller.info.name, get_namespace()) == + controller.c->init(controller_name_, controller_namespace_) == controller_interface::return_type::ERROR) { to.clear(); diff --git a/controller_manager/test/test_controller_manager_with_namespaced_controllers.cpp b/controller_manager/test/test_controller_manager_with_namespaced_controllers.cpp new file mode 100644 index 0000000000..e971020653 --- /dev/null +++ b/controller_manager/test/test_controller_manager_with_namespaced_controllers.cpp @@ -0,0 +1,137 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright 2023 Christoph Hellmann Santos +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/list_controllers.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_controller/test_controller.hpp" + +using ::testing::_; +using ::testing::Return; + +class TestControllerManagerWithNamespacedControllers +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + void SetUp() + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf, true, true), + executor_, TEST_CM_NAME, "/test_namespace"); + run_updater_ = false; + } +}; + +TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_absolute_namespace) +{ + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER1_NAME[] = "/device1/test_controller_name"; + constexpr char TEST_CONTROLLER2_NAME[] = "/device2/test_controller_name"; + cm_->add_controller( + test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // Check if namespace is set correctly + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'", + cm_->get_namespace()); + EXPECT_STREQ(cm_->get_namespace(), "/test_namespace"); + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'", + test_controller->get_node()->get_namespace()); + EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/device1"); + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'", + test_controller2->get_node()->get_namespace()); + EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/device2"); +} + +TEST_P(TestControllerManagerWithNamespacedControllers, when_controller_is_defined_with_just_a_name_expect_it_relative_to_cm_namespace) +{ + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER1_NAME[] = "test_controller1_name"; + constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; + cm_->add_controller( + test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + + // Check if namespace is set correctly + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'", + cm_->get_namespace()); + EXPECT_STREQ(cm_->get_namespace(), "/test_namespace"); + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'", + test_controller->get_node()->get_namespace()); + EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/test_namespace"); + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'", + test_controller2->get_node()->get_namespace()); + EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/test_namespace"); +} + +TEST_P(TestControllerManagerWithNamespacedControllers, when_controller_has_relative_namespace_in_name_expect_it_under_cm_namspace_and_its_namespace) +{ + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER1_NAME[] = "device1/test_controller1_name"; + constexpr char TEST_CONTROLLER2_NAME[] = "device2/test_controller2_name"; + cm_->add_controller( + test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // Check if namespace is set correctly + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'", + cm_->get_namespace()); + EXPECT_STREQ(cm_->get_namespace(), "/test_namespace"); + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'", + test_controller->get_node()->get_namespace()); + EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/test_namespace/device1"); + RCLCPP_INFO( + rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'", + test_controller2->get_node()->get_namespace()); + EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/test_namespace/device2"); +} + +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerManagerWithNamespacedControllers, + testing::Values(strict, best_effort));