Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve diagnostics of Controllers, Hardware Components and Controller Manager #1764

Merged
merged 4 commits into from
Oct 9, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
104 changes: 101 additions & 3 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -2735,6 +2741,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<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);
Expand All @@ -2748,13 +2764,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!");
}
}
}

Expand Down
Loading