From 91123d0fe695a75d849eaf9d1ff6132a6ebf652c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 1 Jun 2022 11:39:03 +0200 Subject: [PATCH 1/3] add diagnostics --- nav2_lifecycle_manager/CMakeLists.txt | 2 ++ .../nav2_lifecycle_manager/lifecycle_manager.hpp | 9 +++++++++ nav2_lifecycle_manager/package.xml | 2 ++ nav2_lifecycle_manager/src/lifecycle_manager.cpp | 16 +++++++++++++++- 4 files changed, 28 insertions(+), 1 deletion(-) diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index e0980df7b4..a05429558b 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(bondcpp REQUIRED) +find_package(diagnostics_updater REQUIRED) nav2_package() @@ -42,6 +43,7 @@ set(dependencies std_srvs tf2_geometry_msgs bondcpp + diagnostics_updater ) ament_target_dependencies(${library_name} diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 4ccac929b8..edd3454ffe 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -30,6 +30,8 @@ #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" #include "std_srvs/srv/trigger.hpp" #include "bondcpp/bond.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" + namespace nav2_lifecycle_manager { @@ -178,6 +180,12 @@ class LifecycleManager : public rclcpp::Node */ void message(const std::string & msg); + // Diagnostics functions + /** + * @brief function to check if the Nav2 system is active + */ + void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat); + // Timer thread to look at bond connections rclcpp::TimerBase::SharedPtr init_timer_; rclcpp::TimerBase::SharedPtr bond_timer_; @@ -203,6 +211,7 @@ class LifecycleManager : public rclcpp::Node bool attempt_respawn_reconnection_; bool system_active_{false}; + diagnostic_updater::Updater diagnostics_updater_; rclcpp::Time bond_respawn_start_time_{0}; rclcpp::Duration bond_respawn_max_duration_{10s}; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index e349e3af49..46ca334021 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -20,6 +20,7 @@ tf2_geometry_msgs bondcpp nav2_common + diagnostics_updater geometry_msgs lifecycle_msgs @@ -31,6 +32,7 @@ std_srvs bondcpp tf2_geometry_msgs + diagnostics_updater ament_lint_auto ament_lint_common diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 7a62fd5467..be49cbab00 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -33,7 +33,7 @@ namespace nav2_lifecycle_manager { LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) -: Node("lifecycle_manager", options) +: Node("lifecycle_manager", options), diagnostics_updater_(this) { RCLCPP_INFO(get_logger(), "Creating"); @@ -103,6 +103,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) executor->add_callback_group(callback_group_, get_node_base_interface()); service_thread_ = std::make_unique(executor); }); + diagnostics_updater_.setHardwareID("Nav2"); + diagnostics_updater_.add("Nav2 Health", this, &LifecycleManager::CreateActiveDiagnostic); } LifecycleManager::~LifecycleManager() @@ -145,6 +147,18 @@ LifecycleManager::isActiveCallback( response->success = system_active_; } +void +LifecycleManager::CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + + if (system_active_) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 is active"); + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 is inactive"); + } + +} + void LifecycleManager::createLifecycleServiceClients() { From 7971c3c2565cef5659105d50d8c54e854802c0c0 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 1 Jun 2022 12:06:30 +0200 Subject: [PATCH 2/3] fix --- nav2_lifecycle_manager/CMakeLists.txt | 4 ++-- nav2_lifecycle_manager/package.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index a05429558b..5f4b3f3c23 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(bondcpp REQUIRED) -find_package(diagnostics_updater REQUIRED) +find_package(diagnostic_updater REQUIRED) nav2_package() @@ -43,7 +43,7 @@ set(dependencies std_srvs tf2_geometry_msgs bondcpp - diagnostics_updater + diagnostic_updater ) ament_target_dependencies(${library_name} diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 46ca334021..ab5aa7fab2 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -20,7 +20,7 @@ tf2_geometry_msgs bondcpp nav2_common - diagnostics_updater + diagnostic_updater geometry_msgs lifecycle_msgs @@ -32,7 +32,7 @@ std_srvs bondcpp tf2_geometry_msgs - diagnostics_updater + diagnostic_updater ament_lint_auto ament_lint_common From 1d6f5b00574ae99f8a8ac12c09e8ebd47dbfbc1f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 1 Jun 2022 19:09:08 +0000 Subject: [PATCH 3/3] fix --- nav2_lifecycle_manager/src/lifecycle_manager.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index be49cbab00..f18436f21f 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -150,13 +150,11 @@ LifecycleManager::isActiveCallback( void LifecycleManager::CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat) { - if (system_active_) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 is active"); } else { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 is inactive"); } - } void