From f27cab6b39cadca1d030db3e3e41f920821ecc56 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) (cherry picked from commit 1cc73c25e39567bc46a472d71ec9643e4fc40dab) # Conflicts: # controller_manager/src/controller_manager.cpp # controller_manager/test/test_controller_manager_srvs.cpp --- controller_manager/src/controller_manager.cpp | 14 + .../test/test_controller_manager_srvs.cpp | 358 ++++++++++++++++++ 2 files changed, 372 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 768df4a2ff..47345192a2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -830,6 +830,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(); @@ -1432,6 +1438,7 @@ void ControllerManager::switch_chained_mode( auto controller = found_it->c; if (!is_controller_active(*controller)) { +<<<<<<< HEAD if (controller->set_chained_mode(to_chained_mode)) { if (to_chained_mode) @@ -1444,6 +1451,9 @@ void ControllerManager::switch_chained_mode( } } else +======= + if (!controller->set_chained_mode(to_chained_mode)) +>>>>>>> 1cc73c2 (Fix multiple chainable controller activation bug (#1401)) { RCLCPP_ERROR( get_logger(), @@ -2354,6 +2364,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 6d9efd2cc7..503321969c 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1520,3 +1520,361 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) } RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); } +<<<<<<< HEAD +======= + +TEST_F(TestControllerManagerSrvs, list_hardware_interfaces_srv) +{ + // 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_hardware_interfaces"); + auto request = std::make_shared(); + + // create chained controller + auto test_chained_controller = 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->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller->set_state_interface_configuration(chained_state_cfg); + test_chained_controller->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_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position", + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/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, test_chainable_controller::TEST_CONTROLLER_NAME, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + // get hardware interface list before configure and loading + auto initial_result = call_service_and_wait(*client, request, srv_executor); + // As there is no controller, so all the interfaces should be available and unclaimed + for (const auto & cmd_itrf : initial_result->command_interfaces) + { + ASSERT_TRUE(cmd_itrf.is_available); + ASSERT_FALSE(cmd_itrf.is_claimed); + } + + // configure controllers + cm_->configure_controller(test_chainable_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + + // The interfaces should be same available and unclaimed until we activate the controllers + auto result = call_service_and_wait(*client, request, srv_executor); + + ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size()); + // There will be no increase in state interfaces + ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size()); + // As there is no controller, so all the interfaces should be available and unclaimed + for (const auto & cmd_itrf : result->command_interfaces) + { + // The controller command interface shouldn't be active until it's controller is active + if ( + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") + { + ASSERT_FALSE(cmd_itrf.is_available); + } + else + { + ASSERT_TRUE(cmd_itrf.is_available); + } + ASSERT_FALSE(cmd_itrf.is_claimed); + } + auto find_interface_in_list = [](const std::string & interface, auto & hw_interface_info) + { + return std::find_if( + hw_interface_info.begin(), hw_interface_info.end(), + [&](auto info) { return info.name == interface; }) != hw_interface_info.end(); + }; + ASSERT_TRUE(find_interface_in_list( + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position", + result->command_interfaces)); + ASSERT_TRUE(find_interface_in_list( + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity", + result->command_interfaces)); + + // activate controllers + cm_->switch_controller( + {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // On activating this chainable controller, we should see that there are 2 more command interfaces + // as this chainable controllers exports 2 reference interfaces + result = call_service_and_wait(*client, request, srv_executor); + + // There will be no increase upon activation + ASSERT_EQ(2u, result->command_interfaces.size() - initial_result->command_interfaces.size()); + ASSERT_EQ(0u, result->state_interfaces.size() - initial_result->state_interfaces.size()); + + for (const auto & cmd_itrf : result->command_interfaces) + { + ASSERT_TRUE(cmd_itrf.is_available); + // This interface is claimed by the chainable controller + if (cmd_itrf.name == "joint1/position") + { + EXPECT_TRUE(cmd_itrf.is_claimed); + } + else if ( + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") + { + // As these interfaces are exposed by the chainable controller and not yet claimed by other + // controllers + ASSERT_FALSE(cmd_itrf.is_claimed); + } + else + { + // Case for rest of the controllers + ASSERT_FALSE(cmd_itrf.is_claimed); + } + } + + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + + // Now as another controller using the interfaces of chainable controller is activated, those + // interfaces should reflect as claimed + result = call_service_and_wait(*client, request, srv_executor); + + for (const auto & cmd_itrf : result->command_interfaces) + { + ASSERT_TRUE(cmd_itrf.is_available); + // This interface is claimed by the chainable controller + if ( + cmd_itrf.name == "joint1/position" || cmd_itrf.name == "joint2/velocity" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/position" || + cmd_itrf.name == + std::string(test_chainable_controller::TEST_CONTROLLER_NAME) + "/joint1/velocity") + { + ASSERT_TRUE(cmd_itrf.is_claimed); + } + else + { + // Case for rest of the controllers + ASSERT_FALSE(cmd_itrf.is_claimed); + } + } +} + +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!"); +} +>>>>>>> 1cc73c2 (Fix multiple chainable controller activation bug (#1401))