Added parameters to handle the overruns behaviour and prints (#2546)

(cherry picked from commit 1fcf10f)

Author: saikishor

controller_manager/doc/userdoc.rst

@@ -386,6 +386,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,23 @@ void ControllerManager::initialize_parameters()
   // Initialize parameters
   try
   {
+    use_sim_time_ = this->get_parameter("use_sim_time").as_bool();
+
+    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<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(
@@ -3198,7 +3210,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>(

doc/release_notes.rst

@@ -96,6 +96,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 <https://github.com/ros-controls/ros2_control/pull/2168>`_).
 * Parameter ``shutdown_on_initial_state_failure`` was added to avoid shutting down on hardware initial state failure  (`#2230 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/2546/files>`_).
 
 hardware_interface
 ******************