From d50f54c1f2cf13571388096449af8ad884b7b5a9 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 24 Nov 2023 14:11:07 +0100 Subject: [PATCH] [ControllerManager] Fix all warnings from the latets features. (#1174) (cherry picked from commit 7d7946424be4b501476191e9ba6d5de956410f47) # Conflicts: # controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp # controller_manager/test/test_controller_manager.cpp --- controller_manager/src/controller_manager.cpp | 15 +++++++++------ .../test_controller_failed_init.cpp | 5 +++++ .../test/test_controller_manager.cpp | 15 +++++++++++++++ .../test/test_controller_manager_srvs.cpp | 4 ++-- 4 files changed, 31 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 450a4780a2..cf4e868ca9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2363,20 +2363,23 @@ bool ControllerManager::controller_sorting( // If there is no common parent, then they belong to 2 different sets auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); if (following_ctrls_b.empty()) return true; - auto find_first_element = [&](const auto & controllers_list) + auto find_first_element = [&](const auto & controllers_list) -> size_t { auto it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, controllers_list.back())); if (it != controllers.end()) { - int dist = std::distance(controllers.begin(), it); - return dist; + return std::distance(controllers.begin(), it); } + return 0; }; - const int ctrl_a_chain_first_controller = find_first_element(following_ctrls); - const int ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); - if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) return true; + const auto ctrl_a_chain_first_controller = find_first_element(following_ctrls); + const auto ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); + if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) + { + return true; + } } // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index 57d253d03c..b54de268c4 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -32,7 +32,12 @@ TestControllerFailedInit::on_init() } controller_interface::return_type TestControllerFailedInit::init( +<<<<<<< HEAD const std::string & /* controller_name */, const std::string & /*namespace_*/, +======= + const std::string & /* controller_name */, const std::string & /* urdf */, + unsigned int /*cm_update_rate*/, const std::string & /*namespace_*/, +>>>>>>> 7d79464 ([ControllerManager] Fix all warnings from the latets features. (#1174)) const rclcpp::NodeOptions & /*node_options*/) { return controller_interface::return_type::ERROR; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index a5566ee9b5..1ae1a29a4c 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -448,10 +448,25 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) if (update_counter % cm_update_rate == 0) { +<<<<<<< HEAD const auto no_of_secs_passed = update_counter / cm_update_rate; EXPECT_EQ( test_controller->internal_counter - initial_counter, (expected_controller_update_rate * no_of_secs_passed)); +======= + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_THAT( + test_controller->internal_counter - initial_counter, + testing::AnyOf( + testing::Eq(controller_update_rate * no_of_secs_passed), + testing::Eq((controller_update_rate * no_of_secs_passed) - 1))); +>>>>>>> 7d79464 ([ControllerManager] Fix all warnings from the latets features. (#1174)) } } } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 30849a3ba3..2f352a5a12 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -765,7 +765,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_6); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( result->controller.begin(), result->controller.end(), @@ -974,7 +974,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(10u, result->controller.size()); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( result->controller.begin(), result->controller.end(),