From b3e52d996b2de834815712ed532f0d7d30128a5d Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Tue, 16 Sep 2025 15:41:04 +0200 Subject: [PATCH 1/2] feat: adds enable_overrun parameter to CM Introduces a new parameter `enable_overrun` to the controller manager, allowing users to disable the overrun detection and warning mechanism. This parameter defaults to true, maintaining the existing behavior. --- controller_manager/src/controller_manager.cpp | 2 +- .../src/controller_manager_parameters.yaml | 6 +++ controller_manager/src/ros2_control_node.cpp | 42 ++++++++++++------- 3 files changed, 33 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 687a79512d..2d008dc843 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3181,7 +3181,7 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration execution_time_.total_time = execution_time_.write_time + execution_time_.update_time + execution_time_.read_time; const double expected_cycle_time = 1.e6 / static_cast(get_update_rate()); - if (execution_time_.total_time > expected_cycle_time && !use_sim_time_) + if (execution_time_.total_time > expected_cycle_time && !use_sim_time_ && params_->enable_overrun) { if (execution_time_.switch_time > 0.0) { diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 55536d0522..a4fd7a0604 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -6,6 +6,12 @@ controller_manager: description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." } + enable_overrun: { + type: bool, + default_value: true, + description: "If true, the controller manager will detect timing overruns, log warnings, and adjust the next iteration time to compensate for missed cycles." + } + enforce_command_limits: { type: bool, default_value: true, diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index c1fc38055f..e1fcf743b6 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -58,6 +58,7 @@ int main(int argc, char ** argv) executor, manager_node_name, "", cm_node_options); const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + const bool enable_overrun = cm->get_parameter_or("enable_overrun", true); const bool has_realtime = realtime_tools::has_realtime_kernel(); const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); @@ -75,13 +76,14 @@ int main(int argc, char ** argv) cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); + RCLCPP_INFO(cm->get_logger(), "Overrun logic: %s", enable_overrun ? "enabled " : "disabled"); const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); RCLCPP_INFO( cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(), thread_priority); std::thread cm_thread( - [cm, thread_priority, use_sim_time]() + [cm, thread_priority, use_sim_time, enable_overrun]() { rclcpp::Parameter cpu_affinity_param; if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) @@ -152,23 +154,31 @@ int main(int argc, char ** argv) else { next_iteration_time += period; - const auto time_now = std::chrono::steady_clock::now(); - if (next_iteration_time < time_now) + + if (enable_overrun) { - const double time_diff = - static_cast( - std::chrono::duration_cast(time_now - next_iteration_time) - .count()) / - 1.e6; - const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); - const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); - RCLCPP_WARN_THROTTLE( - cm->get_logger(), *cm->get_clock(), 1000, - "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " - "took %f ms (missed cycles : %d).", - cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); - next_iteration_time += (overrun_count * period); + const auto time_now = std::chrono::steady_clock::now(); + if (next_iteration_time < time_now) + { + const double time_diff = + static_cast(std::chrono::duration_cast( + time_now - next_iteration_time) + .count()) / + 1.e6; + const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); + const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); + + RCLCPP_WARN_THROTTLE( + cm->get_logger(), *cm->get_clock(), 1000, + "Overrun detected! The controller manager missed its desired rate of %d Hz. The " + "loop " + "took %f ms (missed cycles : %d).", + cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); + + next_iteration_time += (overrun_count * period); + } } + std::this_thread::sleep_until(next_iteration_time); } } From 55dffe7584bf01e097ff5a0c758d42ee76abddb3 Mon Sep 17 00:00:00 2001 From: Baris Yazici Date: Tue, 16 Sep 2025 15:43:33 +0200 Subject: [PATCH 2/2] disable the sleep with overrun disabled --- controller_manager/src/ros2_control_node.cpp | 43 +++++++++----------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index e1fcf743b6..fd88a15ec7 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -151,34 +151,31 @@ int main(int argc, char ** argv) { cm->get_clock()->sleep_until(current_time + period); } - else + else if(enable_overrun) { next_iteration_time += period; - if (enable_overrun) + const auto time_now = std::chrono::steady_clock::now(); + if (next_iteration_time < time_now) { - const auto time_now = std::chrono::steady_clock::now(); - if (next_iteration_time < time_now) - { - const double time_diff = - static_cast(std::chrono::duration_cast( - time_now - next_iteration_time) - .count()) / - 1.e6; - const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); - const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); - - RCLCPP_WARN_THROTTLE( - cm->get_logger(), *cm->get_clock(), 1000, - "Overrun detected! The controller manager missed its desired rate of %d Hz. The " - "loop " - "took %f ms (missed cycles : %d).", - cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); - - next_iteration_time += (overrun_count * period); - } + const double time_diff = + static_cast(std::chrono::duration_cast( + time_now - next_iteration_time) + .count()) / + 1.e6; + const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); + const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); + + RCLCPP_WARN_THROTTLE( + cm->get_logger(), *cm->get_clock(), 1000, + "Overrun detected! The controller manager missed its desired rate of %d Hz. The " + "loop " + "took %f ms (missed cycles : %d).", + cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); + + next_iteration_time += (overrun_count * period); } - + std::this_thread::sleep_until(next_iteration_time); } }