diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e6b377570a..83383aedbe 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -319,6 +319,10 @@ class ControllerManager : public rclcpp::Node */ void clear_requests(); + /// Topologically sort controllers in place + /// If controller A's command is controller B's reference then A proceeds B in the sorted vector + void sort_controllers(std::vector &); + /** * If a controller is deactivated all following controllers (if any exist) should be switched * 'from' the chained mode. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1fa2764f98..41b69a527d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -419,7 +419,8 @@ controller_interface::return_type ControllerManager::configure_controller( { RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str()); - const auto & controllers = get_loaded_controllers(); + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); auto found_it = std::find_if( controllers.begin(), controllers.end(), @@ -436,6 +437,7 @@ controller_interface::return_type ControllerManager::configure_controller( auto controller = found_it->c; auto state = controller->get_state(); + if ( state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -489,10 +491,17 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); - - // TODO(destogl): check and resort controllers in the vector } + std::vector & unused_controller_list = + rt_controllers_wrapper_.get_unused_list(guard); + unused_controller_list = controllers; + + sort_controllers(unused_controller_list); + + rt_controllers_wrapper_.switch_updated_list(guard); + rt_controllers_wrapper_.get_unused_list(guard).clear(); + return controller_interface::return_type::OK; } @@ -1893,6 +1902,100 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +void ControllerManager::sort_controllers(std::vector & unused_controller_list) +{ + 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))) + { + continue; + } + for (const auto & controller2 : unused_controller_list) + { + if (!is_controller_active(controller2.c) && !is_controller_inactive(controller2.c)) + { + continue; + } + if (controller2.info.name == controller1.info.name) + { + continue; + } + for (const auto & reference1 : + 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) + { + commands_string += " " + command; + } + auto same = std::find(controller2_commands.begin(), controller2_commands.end(), reference1); + if (same != controller2_commands.end()) + { + // controller 2 precedes controller1 + if (following_controllers.find(controller2.info.name) != following_controllers.end()) + { + following_controllers[controller2.info.name].push_back(controller1.info.name); + } + else + { + following_controllers.emplace( + controller2.info.name, std::initializer_list{controller1.info.name}); + } + if ( + preceding_controller_count.find(controller1.info.name) != + preceding_controller_count.end()) + { + preceding_controller_count[controller1.info.name] += 1; + } + else + { + preceding_controller_count[controller1.info.name] = 1; + } + break; + } + } + } + } + + // Use Kahn's algorithm for topological sorting + std::deque controllers_to_add = {}; + for (const auto & controller : unused_controller_list) + { + if (preceding_controller_count.find(controller.info.name) == preceding_controller_count.end()) + { + controllers_to_add.push_back(controller.info.name); + } + } + + for (auto i = unused_controller_list.begin(); i != unused_controller_list.end(); i++) + { + 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)); + 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]) + { + preceding_controller_count[controller_name] -= 1; + if (preceding_controller_count[controller_name] == 0) + { + controllers_to_add.push_back(controller_name); + } + } + } + } +} + void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 22221b6e21..54ec14f562 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -104,11 +104,24 @@ TEST_P(TestControllerManager, controller_lifecycle) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - cm_->configure_controller(TEST_CONTROLLER2_NAME); + auto configure_future = std::async( + std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, + test_controller::TEST_CONTROLLER_NAME); + configure_future.wait_for(std::chrono::milliseconds(100)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK); + + configure_future = std::async( + std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, + TEST_CONTROLLER2_NAME); + configure_future.wait_for(std::chrono::milliseconds(100)); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; @@ -216,11 +229,16 @@ TEST_P(TestControllerManager, per_controller_update_rate) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); test_controller->get_node()->set_parameter({"update_rate", 4}); + // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + auto configure_future = std::async( + std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, + test_controller::TEST_CONTROLLER_NAME); + configure_future.wait_for(std::chrono::milliseconds(100)); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index adb227d6fe..f001a8aa9f 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -262,24 +262,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(2u, result->controller.size()); // check chainable controller - ASSERT_EQ(result->controller[0].state, "inactive"); - ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u); - ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[0].is_chainable, true); - ASSERT_EQ(result->controller[0].is_chained, false); - ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u); - ; - ASSERT_EQ(result->controller[0].chain_connections.size(), 0u); - // check test controller + + // cm_->configure_controller() sorts the controllers + // test controller now precedes chainable controller + ASSERT_EQ(result->controller[1].name, "test_chainable_controller_name"); + ASSERT_EQ(result->controller[1].type, "controller_manager/test_chainable_controller"); ASSERT_EQ(result->controller[1].state, "inactive"); ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u); ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[1].is_chainable, false); + ASSERT_EQ(result->controller[1].is_chainable, true); ASSERT_EQ(result->controller[1].is_chained, false); - ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].chain_connections.size(), 1u); + ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u); + ASSERT_EQ(result->controller[1].chain_connections.size(), 0u); + // check test controller + ASSERT_EQ(result->controller[0].state, "inactive"); + ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u); + ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u); + ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u); + ASSERT_EQ(result->controller[0].is_chainable, false); + ASSERT_EQ(result->controller[0].is_chained, false); + ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u); + ASSERT_EQ(result->controller[0].chain_connections.size(), 1u); // activate controllers cm_->switch_controller( {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, @@ -290,18 +294,18 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) // get controller list after activate result = call_service_and_wait(*client, request, srv_executor); // check chainable controller - ASSERT_EQ(result->controller[0].state, "active"); - ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 1u); - ASSERT_EQ(result->controller[0].is_chained, true); - // check test controller ASSERT_EQ(result->controller[1].state, "active"); - ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 1u); + ASSERT_EQ(result->controller[1].is_chained, true); + // check test controller + ASSERT_EQ(result->controller[0].state, "active"); + ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 3u); ASSERT_EQ( test_chainable_controller::TEST_CONTROLLER_NAME, - result->controller[1].chain_connections[0].name); - ASSERT_EQ(2u, result->controller[1].chain_connections[0].reference_interfaces.size()); - ASSERT_EQ("joint1/position", result->controller[1].chain_connections[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[1].chain_connections[0].reference_interfaces[1]); + result->controller[0].chain_connections[0].name); + ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size()); + ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]); } TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) 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 62babe998d..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,6 +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); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -84,6 +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); public: TestableControllerManager( @@ -975,10 +977,91 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); } -// TODO(destogl): Add test case with controllers added in "random" order -// // new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain // +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_random_order) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN REVERSE EXECUTION ORDER + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + + EXPECT_THROW( + cm_->resource_manager_->make_controller_reference_interfaces_available( + POSITION_TRACKING_CONTROLLER), + std::out_of_range); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER + // (otherwise, interface will be missing) + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); + + // Diff-Drive Controller claims the reference interfaces of PID controllers + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); + + // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + // 'rot_z' reference interface is not claimed + for (const auto & interface : {"diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // update controllers + std::vector reference = {32.0, 128.0}; + + // update 'Position Tracking' controller + for (auto & value : diff_drive_controller->reference_interfaces_) + { + ASSERT_EQ(value, 0.0); // default reference values are 0.0 + } + + // update all controllers at once and see that all have expected values --> also checks the order + // of controller execution + reference = {1024.0, 4096.0}; + UpdateAllControllerAndCheck(reference, 2u); +} INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index ca2f7f6b1c..3ff6ee67a2 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -162,8 +162,17 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); // Activate configured controller - cm_->configure_controller(controller_name1); + auto configure_future = std::async( + std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, + controller_name1); + configure_future.wait_for(std::chrono::milliseconds(100)); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + configure_future.get(); start_test_controller(test_param.strictness); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -246,7 +255,17 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure test_controller->cleanup_calls = &cleanup_calls; // Configure from inactive state test_controller->simulate_cleanup_failure = false; - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + + auto configure_future = std::async( + std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, + controller_name1); + configure_future.wait_for(std::chrono::milliseconds(100)); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK); + ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -421,7 +440,13 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); - cm_->configure_controller(controller_name2); + auto configure_future = std::async( + std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, + controller_name2); + configure_future.wait_for(std::chrono::milliseconds(1000)); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1");