From 0284fbb47fae1fd55ae7e3f593de01610ea9a6ac Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 30 Nov 2024 23:21:27 +0000 Subject: [PATCH 1/9] Check if a valid time is received --- controller_manager/src/controller_manager.cpp | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 82546aaeda..ebc432e910 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2367,18 +2367,30 @@ controller_interface::return_type ControllerManager::update( : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); const rclcpp::Time current_time = get_clock()->now(); + rclcpp::Duration controller_actual_period(0, 0); if ( - *loaded_controller.last_update_cycle_time == + current_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { - // it is zero after activation - *loaded_controller.last_update_cycle_time = current_time - controller_period; - RCLCPP_DEBUG( - get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", - loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); + // this can happen with sim_time until the /clock is received + RCLCPP_INFO(get_logger(), "No clock received, use default controller_period"); + controller_actual_period = controller_period; + } + else + { + if ( + *loaded_controller.last_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + // it is zero after activation + *loaded_controller.last_update_cycle_time = current_time - controller_period; + RCLCPP_DEBUG( + get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", + loaded_controller.last_update_cycle_time->seconds(), + loaded_controller.info.name.c_str()); + } + controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); } - const auto controller_actual_period = - (current_time - *loaded_controller.last_update_cycle_time); /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the /// jitter in the system sleep cycles. From ba2254134df4afa6be8a715cf54162b517a3ef0a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 1 Dec 2024 10:13:45 +0000 Subject: [PATCH 2/9] Use time argument instead --- controller_manager/src/controller_manager.cpp | 26 ++++++++----------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ebc432e910..c06338214d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2366,7 +2366,7 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - const rclcpp::Time current_time = get_clock()->now(); + rclcpp::Time current_time = get_clock()->now(); rclcpp::Duration controller_actual_period(0, 0); if ( current_time == @@ -2374,23 +2374,19 @@ controller_interface::return_type ControllerManager::update( { // this can happen with sim_time until the /clock is received RCLCPP_INFO(get_logger(), "No clock received, use default controller_period"); - controller_actual_period = controller_period; + current_time = time; } - else + if ( + *loaded_controller.last_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { - if ( - *loaded_controller.last_update_cycle_time == - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) - { - // it is zero after activation - *loaded_controller.last_update_cycle_time = current_time - controller_period; - RCLCPP_DEBUG( - get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", - loaded_controller.last_update_cycle_time->seconds(), - loaded_controller.info.name.c_str()); - } - controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); + // it is zero after activation + *loaded_controller.last_update_cycle_time = current_time - controller_period; + RCLCPP_DEBUG( + get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", + loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } + controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the /// jitter in the system sleep cycles. From 5ee083c8d7f14a79e70fcdefec597cb04ac03d2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 2 Dec 2024 22:21:43 +0100 Subject: [PATCH 3/9] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- controller_manager/src/controller_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c06338214d..a6430a5fb5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2367,13 +2367,12 @@ controller_interface::return_type ControllerManager::update( : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); rclcpp::Time current_time = get_clock()->now(); - rclcpp::Duration controller_actual_period(0, 0); if ( current_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { // this can happen with sim_time until the /clock is received - RCLCPP_INFO(get_logger(), "No clock received, use default controller_period"); + RCLCPP_WARN(get_logger(), "No clock received, using default controller_period!"); current_time = time; } if ( @@ -2386,7 +2385,8 @@ controller_interface::return_type ControllerManager::update( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } - controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); + const auto controller_actual_period = + (current_time - *loaded_controller.last_update_cycle_time); /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the /// jitter in the system sleep cycles. From 7dd5e900f66397593610c565674b3f0a033727bc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 2 Dec 2024 21:25:15 +0000 Subject: [PATCH 4/9] Revert "Apply suggestions from code review" This reverts commit 5ee083c8d7f14a79e70fcdefec597cb04ac03d2d. --- controller_manager/src/controller_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a6430a5fb5..c06338214d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2367,12 +2367,13 @@ controller_interface::return_type ControllerManager::update( : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); rclcpp::Time current_time = get_clock()->now(); + rclcpp::Duration controller_actual_period(0, 0); if ( current_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { // this can happen with sim_time until the /clock is received - RCLCPP_WARN(get_logger(), "No clock received, using default controller_period!"); + RCLCPP_INFO(get_logger(), "No clock received, use default controller_period"); current_time = time; } if ( @@ -2385,8 +2386,7 @@ controller_interface::return_type ControllerManager::update( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } - const auto controller_actual_period = - (current_time - *loaded_controller.last_update_cycle_time); + controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the /// jitter in the system sleep cycles. From 6b3587e18a6af851d682811590a1724497317794 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 2 Dec 2024 21:26:45 +0000 Subject: [PATCH 5/9] Improve log output --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c06338214d..dd1d4864d1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2373,7 +2373,7 @@ controller_interface::return_type ControllerManager::update( rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { // this can happen with sim_time until the /clock is received - RCLCPP_INFO(get_logger(), "No clock received, use default controller_period"); + RCLCPP_WARN(get_logger(), "No clock received, using default controller_period!"); current_time = time; } if ( From 5b697fa045b08343f35e2fb025cf7fe56749e926 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 2 Dec 2024 22:04:59 +0000 Subject: [PATCH 6/9] Return proper warnings and use controller_period if necessary --- controller_manager/src/controller_manager.cpp | 49 ++++++++++++++++--- 1 file changed, 43 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index dd1d4864d1..c967715ac6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2372,21 +2372,58 @@ controller_interface::return_type ControllerManager::update( current_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { - // this can happen with sim_time until the /clock is received - RCLCPP_WARN(get_logger(), "No clock received, using default controller_period!"); - current_time = time; + if ( + time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + throw std::runtime_error( + "No clock received, and time argument is zero. Check your controller_manager node's " + "clock configuration (use_sim_time parameter) and if a valid clock source is " + "available. Also pass a proper time argument to the update method."); + } + else + { + // this can happen with use_sim_time=true until the /clock is received + RCLCPP_WARN( + get_logger(), + "No clock received, using time argument instead! Check your node's clock " + "configuration (use_sim_time parameter) and if a valid clock source is available"); + current_time = time; + } } if ( *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { - // it is zero after activation - *loaded_controller.last_update_cycle_time = current_time - controller_period; + // last_update_cycle_time is zero after activation + if ( + current_time < + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()) + + controller_period) + { + // can't store negative time + *loaded_controller.last_update_cycle_time = current_time; + } + else + { + *loaded_controller.last_update_cycle_time = current_time - controller_period; + } RCLCPP_DEBUG( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } - controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); + if (current_time > *loaded_controller.last_update_cycle_time) + { + // the nominal case + controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); + } + else + { + controller_actual_period = controller_period; + RCLCPP_DEBUG( + get_logger(), "Using default controller_period %fs for the controller : %s", + controller_actual_period.seconds(), loaded_controller.info.name.c_str()); + } /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the /// jitter in the system sleep cycles. From 9fd84f5d75535639a169059a6a16f63e29b85b08 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 2 Dec 2024 22:29:38 +0000 Subject: [PATCH 7/9] Simplify logic --- controller_manager/src/controller_manager.cpp | 44 +++++++++---------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c967715ac6..b359de1c3b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2366,30 +2366,27 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - rclcpp::Time current_time = get_clock()->now(); - rclcpp::Duration controller_actual_period(0, 0); - if ( - current_time == - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + rclcpp::Time current_time; + if (get_clock()->started()) { - if ( - time == - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) - { - throw std::runtime_error( - "No clock received, and time argument is zero. Check your controller_manager node's " - "clock configuration (use_sim_time parameter) and if a valid clock source is " - "available. Also pass a proper time argument to the update method."); - } - else - { - // this can happen with use_sim_time=true until the /clock is received - RCLCPP_WARN( - get_logger(), - "No clock received, using time argument instead! Check your node's clock " - "configuration (use_sim_time parameter) and if a valid clock source is available"); - current_time = time; - } + current_time = get_clock()->now(); + } + else if ( + time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + throw std::runtime_error( + "No clock received, and time argument is zero. Check your controller_manager node's " + "clock configuration (use_sim_time parameter) and if a valid clock source is " + "available. Also pass a proper time argument to the update method."); + } + else + { + // this can happen with use_sim_time=true until the /clock is received + RCLCPP_WARN( + get_logger(), + "No clock received, using time argument instead! Check your node's clock " + "configuration (use_sim_time parameter) and if a valid clock source is available."); + current_time = time; } if ( *loaded_controller.last_update_cycle_time == @@ -2412,6 +2409,7 @@ controller_interface::return_type ControllerManager::update( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } + rclcpp::Duration controller_actual_period(0, 0); if (current_time > *loaded_controller.last_update_cycle_time) { // the nominal case From ec46577400623efa60dd5e1b6a12ce671910fbe8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 3 Dec 2024 16:00:56 +0100 Subject: [PATCH 8/9] Use throttled output Co-authored-by: Felix Exner (fexner) --- controller_manager/src/controller_manager.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b359de1c3b..24acce1f81 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2382,10 +2382,11 @@ controller_interface::return_type ControllerManager::update( else { // this can happen with use_sim_time=true until the /clock is received - RCLCPP_WARN( - get_logger(), + rclcpp::Clock clock = rclcpp::Clock(); + RCLCPP_WARN_THROTTLE( + get_logger(), clock, 1000, "No clock received, using time argument instead! Check your node's clock " - "configuration (use_sim_time parameter) and if a valid clock source is available."); + "configuration (use_sim_time parameter) and if a valid clock source is available"); current_time = time; } if ( From 6b400b901d86587acce610495c67054c6595056a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 3 Dec 2024 20:44:18 +0100 Subject: [PATCH 9/9] Merge latest diagnostics changes Co-authored-by: Sai Kishor Kothakota --- controller_manager/src/controller_manager.cpp | 28 ++----------------- 1 file changed, 3 insertions(+), 25 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 585649ba72..42542dec7c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2420,35 +2420,13 @@ controller_interface::return_type ControllerManager::update( { // last_update_cycle_time is zero after activation first_update_cycle = true; - if ( - current_time < - rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()) + - controller_period) - { - // can't store negative time - *loaded_controller.last_update_cycle_time = current_time; - } - else - { - *loaded_controller.last_update_cycle_time = current_time - controller_period; - } + *loaded_controller.last_update_cycle_time = current_time; RCLCPP_DEBUG( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); } - rclcpp::Duration controller_actual_period(0, 0); - if (current_time > *loaded_controller.last_update_cycle_time) - { - // the nominal case - controller_actual_period = (current_time - *loaded_controller.last_update_cycle_time); - } - else - { - controller_actual_period = controller_period; - RCLCPP_DEBUG( - get_logger(), "Using default controller_period %fs for the controller : %s", - controller_actual_period.seconds(), loaded_controller.info.name.c_str()); - } + const auto controller_actual_period = + (current_time - *loaded_controller.last_update_cycle_time); /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the /// jitter in the system sleep cycles.