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.

Sign up for GitHub

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

Fix multiple chainable controller activation bug #1401

Merged
merged 5 commits into from
Feb 22, 2024
Merged

Fix multiple chainable controller activation bug #1401

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
ece6a41
add unit test to expose the issue
AugusteBourgois Feb 16, 2024
8cb1154
Fix the multiple chainable controller activation failure bug (Fixes #…
saikishor Feb 16, 2024
17e50a7
Update controller_manager/src/controller_manager.cpp
bmagyar Feb 18, 2024
f2faf25
Merge branch 'master' into fix/multiple/chainable_controller_activation
saikishor Feb 19, 2024
c2c2f44
Merge branch 'master' into fix/multiple/chainable_controller_activation
saikishor Feb 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 10 additions & 3 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -840,6 +840,12 @@ void ControllerManager::clear_requests()
{
deactivate_request_.clear();
activate_request_.clear();
// Set these interfaces are unavailable when clearing requests to avoid leaving them in available
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
// state without the controller being in active state
for (const auto & controller_name : to_chained_mode_request_)
{
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
}
to_chained_mode_request_.clear();
from_chained_mode_request_.clear();
activate_command_interface_request_.clear();
Expand Down Expand Up @@ -1429,9 +1435,6 @@ void ControllerManager::switch_chained_mode(
auto controller = found_it->c;
if (!is_controller_active(*controller))
{
// if it is a chainable controller, make the reference interfaces available on preactivation
// (This is needed when you activate a couple of chainable controller altogether)
resource_manager_->make_controller_reference_interfaces_available(controller_name);
if (!controller->set_chained_mode(to_chained_mode))
{
RCLCPP_ERROR(
Expand Down Expand Up @@ -2368,6 +2371,10 @@ controller_interface::return_type ControllerManager::check_following_controllers
if (found_it == to_chained_mode_request_.end())
{
to_chained_mode_request_.push_back(following_ctrl_it->info.name);
// if it is a chainable controller, make the reference interfaces available on preactivation
// (This is needed when you activate a couple of chainable controller altogether)
resource_manager_->make_controller_reference_interfaces_available(
following_ctrl_it->info.name);
RCLCPP_DEBUG(
get_logger(), "Adding controller '%s' in 'to chained mode' request.",
following_ctrl_it->info.name.c_str());
Expand Down
197 changes: 197 additions & 0 deletions controller_manager/test/test_controller_manager_srvs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1729,3 +1729,200 @@ TEST_F(TestControllerManagerSrvs, list_hardware_interfaces_srv)
}
}
}

TEST_F(TestControllerManagerSrvs, activate_chained_controllers_one_by_one)
{
/// The simulated controller chaining is:
/// test_controller_name -> chain_ctrl_2 -> chain_ctrl_1
///
/// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported
/// from the controller B (or) the controller B is utilizing the expected interfaces exported from
/// the controller A

// create server client and request
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<ListControllers>::SharedPtr client =
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
auto request = std::make_shared<ListControllers::Request>();

// create set of chained controllers
static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1";
static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2";
auto test_chained_controller_1 = std::make_shared<TestChainableController>();
controller_interface::InterfaceConfiguration chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
controller_interface::InterfaceConfiguration chained_state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_1->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"});

auto test_chained_controller_2 = std::make_shared<TestChainableController>();
chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}};
test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_2->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"});

// create non-chained controller
auto test_controller = std::make_shared<TestController>();
controller_interface::InterfaceConfiguration cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position",
std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}};
controller_interface::InterfaceConfiguration state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_controller->set_command_interface_configuration(cmd_cfg);
test_controller->set_state_interface_configuration(state_cfg);
// add controllers
cm_->add_controller(
test_chained_controller_1, TEST_CHAINED_CONTROLLER_1,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_chained_controller_2, TEST_CHAINED_CONTROLLER_2,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
// get controller list before configure
auto result = call_service_and_wait(*client, request, srv_executor);
// check chainable controller
ASSERT_EQ(3u, result->controller.size());
ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2);
// check test controller
ASSERT_EQ(result->controller[1].name, "test_controller_name");

// configure controllers
for (const auto & controller :
{TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME,
TEST_CHAINED_CONTROLLER_2})
{
cm_->configure_controller(controller);
}

// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(3u, result->controller.size());

// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1);

// activate controllers one by one
auto res1 = cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(res1, controller_interface::return_type::OK);
auto res2 = cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_2}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(res2, controller_interface::return_type::OK);
auto res3 = cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(res3, controller_interface::return_type::OK);

RCLCPP_ERROR(srv_node->get_logger(), "Check successful!");
}

TEST_F(TestControllerManagerSrvs, activate_chained_controllers_all_at_once)
{
/// The simulated controller chaining is:
/// test_controller_name -> chain_ctrl_2 -> chain_ctrl_1
///
/// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported
/// from the controller B (or) the controller B is utilizing the expected interfaces exported from
/// the controller A

// create server client and request
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<ListControllers>::SharedPtr client =
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
auto request = std::make_shared<ListControllers::Request>();

// create set of chained controllers
static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1";
static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2";
auto test_chained_controller_1 = std::make_shared<TestChainableController>();
controller_interface::InterfaceConfiguration chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
controller_interface::InterfaceConfiguration chained_state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_1->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"});

auto test_chained_controller_2 = std::make_shared<TestChainableController>();
chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}};
test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_2->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"});

// create non-chained controller
auto test_controller = std::make_shared<TestController>();
controller_interface::InterfaceConfiguration cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position",
std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}};
controller_interface::InterfaceConfiguration state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_controller->set_command_interface_configuration(cmd_cfg);
test_controller->set_state_interface_configuration(state_cfg);
// add controllers
cm_->add_controller(
test_chained_controller_1, TEST_CHAINED_CONTROLLER_1,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_chained_controller_2, TEST_CHAINED_CONTROLLER_2,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
// get controller list before configure
auto result = call_service_and_wait(*client, request, srv_executor);
// check chainable controller
ASSERT_EQ(3u, result->controller.size());
ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2);
// check test controller
ASSERT_EQ(result->controller[1].name, "test_controller_name");

// configure controllers
for (const auto & controller :
{TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME,
TEST_CHAINED_CONTROLLER_2})
{
cm_->configure_controller(controller);
}

// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(3u, result->controller.size());

// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1);

// activate controllers all at once
auto res = cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
ASSERT_EQ(res, controller_interface::return_type::OK);

RCLCPP_ERROR(srv_node->get_logger(), "Check successful!");
}
Loading