diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3e3e7e6ecd..823888b1d5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/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 guard(rt_controllers_wrapper_.controllers_lock_); + const std::vector & 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