From 1cc73c25e39567bc46a472d71ec9643e4fc40dab Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 22 Feb 2024 15:13:40 +0100 Subject: [PATCH] Fix multiple chainable controller activation bug (#1401) --- controller_manager/src/controller_manager.cpp | 13 +- .../test/test_controller_manager_srvs.cpp | 197 ++++++++++++++++++ 2 files changed, 207 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1911bd6ff8..ee2ee87218 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -840,6 +840,12 @@ void ControllerManager::clear_requests() { deactivate_request_.clear(); activate_request_.clear(); + // Set these interfaces as unavailable when clearing requests to avoid leaving them in available + // 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(); @@ -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( @@ -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()); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index cdec4806c1..36d8aa7c4e 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -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("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // 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(); + 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(); + 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(); + 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("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // 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(); + 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(); + 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(); + 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!"); +}