Added parameters to handle the overruns behaviour and prints

Author: saikishor

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
 -----------
 

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<controller_manager::ParamListener>(
       this->get_node_parameters_interface(), this->get_logger());
     params_ = std::make_shared<controller_manager::Params>(cm_param_listener_->get_params());
     update_rate_ = static_cast<unsigned int>(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<rclcpp::Clock>(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<double>(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)
     {

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.",
+    }

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<bool>("overruns.manage", true);
+  RCLCPP_INFO(
+    cm->get_logger(), "Overruns handling is : %s", manage_overruns ? "enabled" : "disabled");
   const int thread_priority = cm->get_parameter_or<int>("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<double>(