Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add namespaced controllers #1074

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
9 changes: 9 additions & 0 deletions controller_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand Down
70 changes: 67 additions & 3 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this sentence complete?

/**
* \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"),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not use controller manager's logger

Suggested change
rclcpp::get_logger("split_namespace_and_name"),
get_logger(),

"node_namespace: %s, node_name: %s, node_param %s", node_namespace.c_str(), node_name.c_str(),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
"node_namespace: %s, node_name: %s, node_param %s", node_namespace.c_str(), node_name.c_str(),
"Determined controller's namespace: '%s', name: '%s', and type name '%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"
Expand Down Expand Up @@ -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(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
calculate_controller_naming(
determine_controller_namespace(

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Don't you want to use controller_name_temp after this call?

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,
Expand Down Expand Up @@ -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_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we have to use the suffix “_” on local variables?

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();
Expand Down
Original file line number Diff line number Diff line change
@@ -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 <gtest/gtest.h>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#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<controller_manager::ControllerManager>,
public testing::WithParamInterface<Strictness>
{
public:
void SetUp()
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
cm_ = std::make_shared<controller_manager::ControllerManager>(
std::make_unique<hardware_interface::ResourceManager>(
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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_absolute_namespace)
TEST_P(TestControllerManagerWithNamespacedControllers, when_absolute_namespace_is_use_expect_controller_in_it)

{
auto test_controller = std::make_shared<test_controller::TestController>();
auto test_controller2 = std::make_shared<test_controller::TestController>();
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<test_controller::TestController>();
auto test_controller2 = std::make_shared<test_controller::TestController>();
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<test_controller::TestController>();
auto test_controller2 = std::make_shared<test_controller::TestController>();
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));