diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9100fde7e6..31ed3c3f09 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -489,13 +489,14 @@ controller_interface::return_type ControllerManager::configure_controller( rt_controllers_wrapper_.get_unused_list(guard); unused_controller_list = controllers; - std::unordered_map preceeding_controller_count = {}; + std::unordered_map preceding_controller_count = {}; std::unordered_map> following_controllers = {}; for (const auto & controller1 : unused_controller_list) { - if (!controller1.c->is_chainable() || ( - !is_controller_active(controller1.c) && !is_controller_inactive(controller1.c)) ) + if ( + !controller1.c->is_chainable() || + (!is_controller_active(controller1.c) && !is_controller_inactive(controller1.c))) { continue; } @@ -510,18 +511,19 @@ controller_interface::return_type ControllerManager::configure_controller( continue; } for (const auto & reference1 : - resource_manager_->get_controller_reference_interface_names(controller1.info.name)) + resource_manager_->get_controller_reference_interface_names(controller1.info.name)) { const auto controller2_commands = controller2.c->command_interface_configuration().names; std::string commands_string = ""; - for (auto command : controller2_commands){ + for (auto command : controller2_commands) + { commands_string += " " + command; } auto same = std::find(controller2_commands.begin(), controller2_commands.end(), reference1); if (same != controller2_commands.end()) { - // controller 2 preceeds controller1 + // controller 2 precedes controller1 if (following_controllers.find(controller2.info.name) != following_controllers.end()) { following_controllers[controller2.info.name].push_back(controller1.info.name); @@ -529,16 +531,17 @@ controller_interface::return_type ControllerManager::configure_controller( else { following_controllers.emplace( - controller2.info.name, - std::initializer_list{controller1.info.name}); + controller2.info.name, std::initializer_list{controller1.info.name}); } - if (preceeding_controller_count.find(controller1.info.name) != - preceeding_controller_count.end()){ - preceeding_controller_count[controller1.info.name] += 1; + if ( + preceding_controller_count.find(controller1.info.name) != + preceding_controller_count.end()) + { + preceding_controller_count[controller1.info.name] += 1; } else { - preceeding_controller_count[controller1.info.name] = 1; + preceding_controller_count[controller1.info.name] = 1; } break; } @@ -549,7 +552,7 @@ controller_interface::return_type ControllerManager::configure_controller( std::deque controllers_to_add = {}; for (const auto & controller : unused_controller_list) { - if (preceeding_controller_count.find(controller.info.name) == preceeding_controller_count.end()) + if (preceding_controller_count.find(controller.info.name) == preceding_controller_count.end()) { controllers_to_add.push_back(controller.info.name); } @@ -557,18 +560,18 @@ controller_interface::return_type ControllerManager::configure_controller( for (auto i = unused_controller_list.begin(); i != unused_controller_list.end(); i++) { - std::string controller_name_to_find = controllers_to_add.front(); + std::string controller_name_to_find = controllers_to_add.front(); controllers_to_add.pop_front(); auto found_it = std::find_if( - unused_controller_list.begin(), unused_controller_list.end(), - std::bind(controller_name_compare, std::placeholders::_1, controller_name_to_find)); + unused_controller_list.begin(), unused_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name_to_find)); iter_swap(i, found_it); if (following_controllers.find(controller_name_to_find) != following_controllers.end()) { for (const auto & controller_name : following_controllers[controller_name_to_find]) { - preceeding_controller_count[controller_name] -= 1; - if (preceeding_controller_count[controller_name] == 0) + preceding_controller_count[controller_name] -= 1; + if (preceding_controller_count[controller_name] == 0) { controllers_to_add.push_back(controller_name); } @@ -577,7 +580,8 @@ controller_interface::return_type ControllerManager::configure_controller( } std::string controller_names = ""; - for (auto & cont : unused_controller_list){ + for (auto & cont : unused_controller_list) + { controller_names += cont.info.name + " "; } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index c7e8b0e383..f12fc0761c 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -52,9 +52,7 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); - FRIEND_TEST( - TestControllerChainingWithControllerManager, - test_chained_controllers_random_order); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_random_order); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -87,9 +85,7 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); - FRIEND_TEST( - TestControllerChainingWithControllerManager, - test_chained_controllers_random_order); + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_random_order); public: TestableControllerManager( @@ -981,7 +977,6 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); } - // new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain // TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_random_order) @@ -989,7 +984,7 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_ran SetupControllers(); // add all controllers - CONTROLLERS HAVE TO ADDED IN REVERSE EXECUTION ORDER - cm_->add_controller( + cm_->add_controller( pid_left_wheel_controller, PID_LEFT_WHEEL, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); cm_->add_controller( @@ -1066,10 +1061,8 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_ran // of controller execution reference = {1024.0, 4096.0}; UpdateAllControllerAndCheck(reference, 2u); - } - INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, testing::Values(strict, best_effort));