From 71f8f3e64e63fe3e6a8767c241795011bab52733 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Sep 2025 23:44:26 +0200 Subject: [PATCH 1/3] Added parameters to handle the overruns behaviour and prints --- controller_manager/doc/userdoc.rst | 6 ++++++ controller_manager/src/controller_manager.cpp | 5 +++-- controller_manager/src/controller_manager_parameters.yaml | 5 +++++ controller_manager/src/ros2_control_node.cpp | 7 +++++-- 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index c9932e6936..98d97d7cd9 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -383,6 +383,12 @@ thread_priority (optional; int; default: 50) use_sim_time (optional; bool; default: false) Enables the use of simulation time in the ``controller_manager`` node. +overruns.manage (optional; bool; default: true) + Enables or disables the handling of overruns in the real-time loop of the ``controller_manager`` node. + If set to true, the controller manager will detect overruns caused by system time changes or longer execution times of the controllers and hardware components. + If an overrun is detected, the controller manager will print a warning message to the console. + When used with ``use_sim_time`` set to true, this parameter is ignored and the overrun handling is disabled. + Concepts ----------- diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 687a79512d..dd9ba2c765 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -591,11 +591,12 @@ void ControllerManager::initialize_parameters() // Initialize parameters try { + use_sim_time_ = this->get_parameter("use_sim_time").as_bool(); + this->declare_parameter("overruns.print_warnings", !use_sim_time_); cm_param_listener_ = std::make_shared( this->get_node_parameters_interface(), this->get_logger()); params_ = std::make_shared(cm_param_listener_->get_params()); update_rate_ = static_cast(params_->update_rate); - use_sim_time_ = this->get_parameter("use_sim_time").as_bool(); trigger_clock_ = use_sim_time_ ? this->get_clock() : std::make_shared(RCL_STEADY_TIME); RCLCPP_INFO( @@ -3181,7 +3182,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 (params_->overruns.print_warnings && execution_time_.total_time > expected_cycle_time) { 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..44a3f4d9d0 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -234,3 +234,8 @@ controller_manager: gt<>: 0.0, } } + overruns: + print_warnings: { + type: bool, + description: "If true, the controller manager will print a warning message to the console if an overrun is detected in its real-time loop (``read``, ``update`` and ``write``). By default, it is set to true, except when used with ``use_sim_time`` parameter set to true.", + } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index c1fc38055f..271d91b4c9 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -75,13 +75,16 @@ 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()); + const bool manage_overruns = cm->get_parameter_or("overruns.manage", true); + RCLCPP_INFO( + cm->get_logger(), "Overruns handling is : %s", manage_overruns ? "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, manage_overruns]() { rclcpp::Parameter cpu_affinity_param; if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) @@ -153,7 +156,7 @@ int main(int argc, char ** argv) { next_iteration_time += period; const auto time_now = std::chrono::steady_clock::now(); - if (next_iteration_time < time_now) + if (manage_overruns && next_iteration_time < time_now) { const double time_diff = static_cast( From b52d21ae61c8d2b1d25473792f70dd0a339f7193 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Sep 2025 18:20:23 +0200 Subject: [PATCH 2/3] check before redeclaring the parameter --- controller_manager/src/controller_manager.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index dd9ba2c765..8ac9d5ab34 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -592,7 +592,18 @@ void ControllerManager::initialize_parameters() try { use_sim_time_ = this->get_parameter("use_sim_time").as_bool(); - this->declare_parameter("overruns.print_warnings", !use_sim_time_); + + if (!this->has_parameter("overruns.print_warnings")) + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = + "If true, the controller manager will print a warning message to the console if an overrun " + "is detected in its real-time loop (read, update and write). By default, it is set to " + "true, except when used with use_sim_time parameter set to true."; + descriptor.read_only = false; + auto parameter = rclcpp::ParameterValue(!use_sim_time_); + this->declare_parameter("overruns.print_warnings", parameter, descriptor); + } cm_param_listener_ = std::make_shared( this->get_node_parameters_interface(), this->get_logger()); params_ = std::make_shared(cm_param_listener_->get_params()); From 7363f61817c8e1d8fbebf9c662b9e90945158c7a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 2 Oct 2025 08:26:23 +0000 Subject: [PATCH 3/3] Update release_notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 3e4836e58e..355076c389 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -10,6 +10,7 @@ controller_manager * The default strictness of the ``switch_controllers`` can now we be chosen using ROS 2 parameters. The default behaviour is still left to ``BEST_EFFORT`` (`#2168 `_). * Parameter ``shutdown_on_initial_state_failure`` was added to avoid shutting down on hardware initial state failure (`#2230 `_). * The controller manager now publishes ``~/statistics/names`` and ``~/statistics/values`` topics to introspect the execution time and periodicity of the different entities running in the realtime loop (`#2449 `_). +* New parameters ``overruns.manage`` and ``overruns.print_warnings`` were added to control the behavior of the controller manager/ros2_control_node when overruns occur (`#2546 `_). hardware_interface ******************