From 55eb3a89d17ec7a7196695ab4e3b775c81a41990 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 9 Oct 2024 18:26:06 +0200 Subject: [PATCH] Improve diagnostics of Controllers, Hardware Components and Controller Manager (#1764) --- .../controller_manager/controller_manager.hpp | 4 + controller_manager/src/controller_manager.cpp | 104 +++++++++++++++++- 2 files changed, 105 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7c09377bb9..b8c05efcc0 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -443,6 +443,10 @@ class ControllerManager : public rclcpp::Node void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + 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 a0d7586308..ad0ed298d2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -281,6 +281,12 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + 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) @@ -2762,6 +2768,16 @@ controller_interface::return_type ControllerManager::check_preceeding_controller void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + atleast_one_hw_active = true; + break; + } + } // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); @@ -2775,13 +2791,95 @@ void ControllerManager::controller_activity_diagnostic_callback( stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); } - if (all_active) + if (!atleast_one_hw_active) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, + "No hardware components are currently active to activate controllers"); + } + else + { + if (controllers.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + } + } +} + +void ControllerManager::hardware_components_diagnostic_callback( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + bool all_active = true; + bool atleast_one_hw_active = false; + const auto hw_components_info = resource_manager_->get_components_status(); + for (const auto & [component_name, component_info] : hw_components_info) + { + stat.add(component_name, component_info.state.label()); + if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + all_active = false; + } + else + { + atleast_one_hw_active = true; + } + } + if (!is_resource_manager_initialized()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + } + else if (hw_components_info.empty()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + } + else + { + if (!atleast_one_hw_active) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + "No hardware components are currently active"); + } + else + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, all_active + ? "All hardware components are active" + : "Not all hardware components are active"); + } + } +} + +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, "All controllers are active"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not all controllers are active"); + 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!"); + } } }