From 4fe9862a291325a8e09601d6e8acda65a08995f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 17 Jan 2024 16:16:04 +0100 Subject: [PATCH] Fixup the tests. --- controller_manager/src/controller_manager.cpp | 6 ++++++ .../test/test_controller_manager_srvs.cpp | 6 ++++++ ...llers_chaining_with_controller_manager.cpp | 19 +++++-------------- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d71b6f47c0..fd02489def 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1451,6 +1451,12 @@ void ControllerManager::activate_controllers() hardware_interface::lifecycle_state_names::ACTIVE, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } + + // if it is a chainable controller, make the reference interfaces available on activation + if (controller->is_chainable()) + { + resource_manager_->make_controller_reference_interfaces_available(controller_name); + } } // All controllers activated, switching done switch_params_.do_switch = false; diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index d0f101b9e6..afe63302bf 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -288,6 +288,12 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); ASSERT_EQ(res, controller_interface::return_type::OK); + // we should here wait for the first controller to be activated, i.e., for its reference + // interface to become available (mail loop runs on 100 Hz) - so we check the status at least once + while (result->controller[1].state != "active") + { + result = call_service_and_wait(*client, request, srv_executor); + } res = cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); 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 de7d9e760a..d1ab196ff7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -251,7 +251,7 @@ class TestControllerChainingWithControllerManager void check_after_de_activate( std::shared_ptr & controller, const std::vector & claimed_command_itfs, size_t expected_internal_counter, const controller_interface::return_type expected_return, - bool deactivated, bool claimed_interfaces_from_hw = false) + bool deactivated) { for (const auto & interface : claimed_command_itfs) { @@ -264,14 +264,7 @@ class TestControllerChainingWithControllerManager } else { - if (claimed_interfaces_from_hw) - { - EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); - } - else - { - EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); - } + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); } } @@ -303,14 +296,12 @@ class TestControllerChainingWithControllerManager void DeactivateAndCheckController( std::shared_ptr & controller, const std::string & controller_name, const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, - const bool claimed_interfaces_from_hw = false, const controller_interface::return_type expected_return = controller_interface::return_type::OK) { switch_test_controllers( {}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return); check_after_de_activate( - controller, claimed_command_itfs, expected_internal_counter, expected_return, true, - claimed_interfaces_from_hw); + controller, claimed_command_itfs, expected_internal_counter, expected_return, true); } void UpdateAllControllerAndCheck( @@ -612,9 +603,9 @@ TEST_P( // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( - pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u, true); + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u); DeactivateAndCheckController( - pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u, true); + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u); EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode());