From 181cda0b559096fd1311c745e01377fba8c4abf9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 24 May 2024 18:31:22 +0000 Subject: [PATCH 1/3] Properly test if ctrl period is correct if activated at nonzero time --- .../test/test_controller/test_controller.cpp | 2 +- .../test/test_controller/test_controller.hpp | 2 +- .../test/test_controller_manager.cpp | 19 ++++++++++++++----- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index df41ebf34e..8f120bbd47 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -62,7 +62,7 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con controller_interface::return_type TestController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - update_period_ = period.seconds(); + update_period_ = period; ++internal_counter; // set value to hardware to produce and test different behaviors there diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 14ad753803..368aeae4a8 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -80,7 +80,7 @@ class TestController : public controller_interface::ControllerInterface // enables external setting of values to command interfaces - used for simulation of hardware // errors double set_first_command_interface_value_to; - double update_period_ = 0; + rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.); }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index e88b41f222..3e2f91e91a 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -374,9 +374,12 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); // In case of a non perfect divisor, the update period should respect the rule - // [controller_update_rate, 2*controller_update_rate) - ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate); - ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate); + // [cm_update_rate, 2*cm_update_rate) + EXPECT_THAT( + test_controller->update_period_, + testing::AllOf( + testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)), + testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate)))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -445,6 +448,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); // Start controller, will take effect at the end of the update function + time_ = test_controller->get_node()->now(); // set to something nonzero const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; std::vector stop_controllers = {}; @@ -472,6 +476,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) const auto controller_update_rate = test_controller->get_update_rate(); const auto initial_counter = test_controller->internal_counter; + // don't start with zero to check if the period is correct if controller is activated anytime rclcpp::Time time = time_; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { @@ -480,8 +485,12 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->update(time, rclcpp::Duration::from_seconds(0.01))); // In case of a non perfect divisor, the update period should respect the rule // [controller_update_rate, 2*controller_update_rate) - ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate); - ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate); + EXPECT_THAT( + test_controller->update_period_, + testing::AllOf( + testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)), + testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate)))) + << "update_counter: " << update_counter; time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) From ab8011333ffeeb362a81c8f962b983d8a755652c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 24 May 2024 18:31:55 +0000 Subject: [PATCH 2/3] Fix the initialization of the next update time --- controller_manager/src/controller_manager.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 41fe1ad95d..1b9a2a713d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2147,6 +2147,15 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + if ( + *loaded_controller.next_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + // it is zero after activation + RCLCPP_DEBUG(get_logger(), "Set next_update_cycle_time to %fs", time.seconds()); + *loaded_controller.next_update_cycle_time = time; + } + bool controller_go = (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || @@ -2182,12 +2191,6 @@ controller_interface::return_type ControllerManager::update( controller_ret = controller_interface::return_type::ERROR; } - if ( - *loaded_controller.next_update_cycle_time == - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) - { - *loaded_controller.next_update_cycle_time = time; - } *loaded_controller.next_update_cycle_time += controller_period; if (controller_ret != controller_interface::return_type::OK) From 28fb945aacbb16320d6b56cbd5a70b589543e206 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 24 May 2024 19:46:02 +0000 Subject: [PATCH 3/3] Improve debug output --- controller_manager/src/controller_manager.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1b9a2a713d..3b84fc07e4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2152,7 +2152,9 @@ controller_interface::return_type ControllerManager::update( rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { // it is zero after activation - RCLCPP_DEBUG(get_logger(), "Set next_update_cycle_time to %fs", time.seconds()); + RCLCPP_DEBUG( + get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s", + time.seconds(), loaded_controller.info.name.c_str()); *loaded_controller.next_update_cycle_time = time; }