Add controller update stats to print every 20 seconds when there is a…

… missed trigger
ros-controls · Oct 17, 2024 · 75754de · 75754de
1 parent 5a85a24
commit 75754de
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 10 deletions.
diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -60,6 +60,17 @@ struct InterfaceConfiguration
   std::vector<std::string> names = {};
 };
 
+struct ControllerUpdateStats
+{
+  void reset()
+  {
+    total_triggers = 0;
+    failed_triggers = 0;
+  }
+
+  unsigned int total_triggers;
+  unsigned int failed_triggers;
+};
 /**
  * Base interface class  for an controller. The interface may not be used to implement a controller.
  * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
@@ -312,6 +323,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   unsigned int update_rate_ = 0;
   bool is_async_ = false;
   std::string urdf_ = "";
+  ControllerUpdateStats trigger_stats_;
 };
 
 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;

diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
@@ -127,6 +127,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
       thread_priority);
     async_handler_->start_thread();
   }
+  trigger_stats_.reset();
 
   return get_node()->configure();
 }
@@ -153,9 +154,19 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
 std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  trigger_stats_.total_triggers++;
   if (is_async())
   {
-    return async_handler_->trigger_async_callback(time, period);
+    const auto result = async_handler_->trigger_async_callback(time, period);
+    if (!result.first)
+    {
+      trigger_stats_.failed_triggers++;
+      RCLCPP_WARN_THROTTLE(
+        get_node()->get_logger(), *get_node()->get_clock(), 20000,
+        "The controller missed %u update cycles out of %u total triggers.",
+        trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
+    }
+    return result;
   }
   else
   {

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -2409,15 +2409,6 @@ controller_interface::return_type ControllerManager::update(
           failed_controllers_list.push_back(loaded_controller.info.name);
           ret = controller_ret;
         }
-        else if (!trigger_status)
-        {
-          RCLCPP_WARN(
-            get_logger(),
-            "The controller '%s' missed an update cycle at time : '%f', will trigger next update "
-            "cycle at around : '%f'",
-            loaded_controller.info.name.c_str(), time.seconds(),
-            loaded_controller.next_update_cycle_time->seconds());
-        }
       }
     }
   }