From f2c4286716183a22c380c6b2ca0b42116433012a Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Wed, 25 Sep 2024 16:07:01 +0200
Subject: [PATCH] add diagnostics of Controller Manager

---
 .../controller_manager/controller_manager.hpp |  2 ++
 controller_manager/src/controller_manager.cpp | 27 +++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 0a77738bc0..b8c05efcc0 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -445,6 +445,8 @@ class ControllerManager : public rclcpp::Node
 
   void hardware_components_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
+  void controller_manager_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
   /**
    * @brief determine_controller_node_options - A method that retrieves the controller defined node
    * options and adapts them, based on if there is a params file to be loaded or the use_sim_time
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 8e64fbfc40..bc605713e7 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -284,6 +284,9 @@ void ControllerManager::init_controller_manager()
   diagnostics_updater_.add(
     "Hardware Components Activity", this,
     &ControllerManager::hardware_components_diagnostic_callback);
+  diagnostics_updater_.add(
+    "Controller Manager Activity", this,
+    &ControllerManager::controller_manager_diagnostic_callback);
 }
 
 void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
@@ -2829,6 +2832,30 @@ void ControllerManager::hardware_components_diagnostic_callback(
   }
 }
 
+void ControllerManager::controller_manager_diagnostic_callback(
+  diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  stat.add("update_rate", std::to_string(get_update_rate()));
+  if (is_resource_manager_initialized())
+  {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running");
+  }
+  else
+  {
+    if (robot_description_.empty())
+    {
+      stat.summary(
+        diagnostic_msgs::msg::DiagnosticStatus::WARN, "Waiting for robot description....");
+    }
+    else
+    {
+      stat.summary(
+        diagnostic_msgs::msg::DiagnosticStatus::ERROR,
+        "Resource Manager is not initialized properly!");
+    }
+  }
+}
+
 void ControllerManager::update_list_with_controller_chain(
   const std::string & ctrl_name, std::vector<std::string>::iterator controller_iterator,
   bool append_to_controller)