Report inactive controllers as a diagnostics ok instead of an error (#…

…1184)

(cherry picked from commit c9bd3d8)

# Conflicts:
#	controller_manager/src/controller_manager.cpp

controller_manager/src/controller_manager.cpp

@@ -2372,4 +2372,33 @@ bool ControllerManager::controller_sorting(
   }
 };
 
+<<<<<<< HEAD
+=======
+void ControllerManager::controller_activity_diagnostic_callback(
+  diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  // lock controllers
+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+  const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);
+  bool all_active = true;
+  for (size_t i = 0; i < controllers.size(); ++i)
+  {
+    if (!is_controller_active(controllers[i].c))
+    {
+      all_active = false;
+    }
+    stat.add(controllers[i].info.name, controllers[i].c->get_state().label());
+  }
+
+  if (all_active)
+  {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "All controllers are active");
+  }
+  else
+  {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not all controllers are active");
+  }
+}
+
+>>>>>>> c9bd3d8 (Report inactive controllers as a diagnostics ok instead of an error (#1184))
 }  // namespace controller_manager