add diagnostics of Controller Manager

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

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)