Add add_analyzer functionality

diagnostic_aggregator/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
@@ -67,6 +68,10 @@ add_executable(aggregator_node src/aggregator_node.cpp)
 target_link_libraries(aggregator_node
   ${PROJECT_NAME})
 
+# Add analyzer
+add_executable(add_analyzer src/add_analyzer.cpp)
+ament_target_dependencies(add_analyzer rclcpp rcl_interfaces)
+
 # Testing macro
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -135,6 +140,11 @@ install(
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(
+  TARGETS add_analyzer
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(
   TARGETS ${PROJECT_NAME} ${ANALYZERS}
   EXPORT ${PROJECT_NAME}Targets
@@ -152,6 +162,7 @@ ament_python_install_package(${PROJECT_NAME})
 
 # Install Example
 set(ANALYZER_PARAMS_FILEPATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/example_analyzers.yaml")
+set(ADD_ANALYZER_PARAMS_FILEPATH "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/example_add_analyzers.yaml")
 configure_file(example/example.launch.py.in example.launch.py @ONLY)
 install( # launch descriptor
   FILES ${CMAKE_CURRENT_BINARY_DIR}/example.launch.py
@@ -162,7 +173,7 @@ install( # example publisher
   DESTINATION lib/${PROJECT_NAME}
 )
 install( # example aggregator configration
-  FILES example/example_analyzers.yaml
+  FILES example/example_analyzers.yaml example/example_add_analyzers.yaml
   DESTINATION share/${PROJECT_NAME}
 )
 

diagnostic_aggregator/README.md

@@ -132,6 +132,33 @@ You can launch the `aggregator_node` like this (see [example.launch.py.in](examp
     ])
 ```
 
+You can add analyzers at runtime using the `add_analyzer` node like this (see [example.launch.py.in](example/example.launch.py.in)):
+```
+    add_analyzer = launch_ros.actions.Node(
+        package='diagnostic_aggregator',
+        executable='add_analyzer',
+        output='screen',
+        parameters=[add_analyzer_params_filepath])
+    return launch.LaunchDescription([
+        add_analyzer,
+    ])
+```
+This node updates the parameters of the `aggregator_node` by calling the service `/analyzers/set_parameters_atomically`.
+The `aggregator_node` will detect when a `parameter-event` has introduced new parameters to it.
+When this happens the `aggregator_node` will reload all analyzers based on its new set of parameters.
+Adding analyzers this way can be done at runtime and can be made conditional.
+
+In the example, `add_analyzer` will add an analyzer for diagnostics that are marked optional:
+``` yaml
+/**:
+  ros__parameters:
+    optional:
+      type: diagnostic_aggregator/GenericAnalyzer
+      path: Optional
+      startswith: [ '/optional' ]
+```
+This will move the `/optional/runtime/analyzer` diagnostic from the "Other" to  "Aggregation" where it will not go stale after 5 seconds and will be taken into account for the toplevel state.
+
 # Basic analyzers
 The `diagnostic_aggregator` package provides a few basic analyzers that you can use to aggregate your diagnostics.
 

diagnostic_aggregator/example/README.md

@@ -1,5 +1,7 @@
 # Aggregator Example
 
-This is a simple example to show the diagnostic_aggregator in action. It involves one python script producing dummy diagnostic data ([example_pub.py](./example_pub.py)), and one diagnostic aggregator configuration ([example.yaml](./example.yaml)) that provides analyzers aggregating it.
+This is a simple example to show the diagnostic_aggregator and add_analyzer in action. It involves one python script producing dummy diagnostic data ([example_pub.py](./example_pub.py)), one diagnostic aggregator configuration ([example_analyzers.yaml](./example_analyzers.yaml)) and one add_analyzer configuration ([example_add_analyzers.yaml](./example_add_analyzers.yaml)).
+
+The aggregator will launch and load all the analyzers listed in ([example_analyzers.yaml](./example_analyzers.yaml)). Then the aggregator will be notified that there are additional analyzers that we also want to load in ([example_add_analyzers.yaml](./example_add_analyzers.yaml)). After this reload all analyzers will be active.
 
 Run the example with `ros2 launch diagnostic_aggregator example.launch.py`

diagnostic_aggregator/example/example.launch.py.in

@@ -4,6 +4,7 @@ import launch
 import launch_ros.actions
 
 analyzer_params_filepath = "@ANALYZER_PARAMS_FILEPATH@"
+add_analyzer_params_filepath = "@ADD_ANALYZER_PARAMS_FILEPATH@"
 
 
 def generate_launch_description():
@@ -12,11 +13,18 @@ def generate_launch_description():
         executable='aggregator_node',
         output='screen',
         parameters=[analyzer_params_filepath])
+    add_analyzer = launch_ros.actions.Node(
+        package='diagnostic_aggregator',
+        executable='add_analyzer',
+        output='screen',
+        parameters=[add_analyzer_params_filepath]
+    )
     diag_publisher = launch_ros.actions.Node(
         package='diagnostic_aggregator',
         executable='example_pub.py')
     return launch.LaunchDescription([
         aggregator,
+        add_analyzer,
         diag_publisher,
         launch.actions.RegisterEventHandler(
             event_handler=launch.event_handlers.OnProcessExit(

diagnostic_aggregator/example/example_add_analyzers.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    optional:
+      type: diagnostic_aggregator/GenericAnalyzer
+      path: Optional
+      contains: [ '/optional' ]

diagnostic_aggregator/example/example_pub.py

@@ -81,6 +81,10 @@ def __init__(self):
                              name='/sensors/front/cam', message='OK'),
             DiagnosticStatus(level=DiagnosticStatus.OK,
                              name='/sensors/rear/cam', message='OK'),
+
+            # Optional
+            DiagnosticStatus(level=DiagnosticStatus.OK,
+                             name='/optional/runtime/analyzer', message='OK'),
         ]
 
     def timer_callback(self):

diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp

@@ -133,6 +133,8 @@ class Aggregator
   rclcpp::Service<diagnostic_msgs::srv::AddDiagnostics>::SharedPtr add_srv_;
   /// DiagnosticArray, /diagnostics
   rclcpp::Subscription<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_sub_;
+  /// ParameterEvent, /parameter_events
+  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr param_sub_;
   /// DiagnosticArray, /diagnostics_agg
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr agg_pub_;
   /// DiagnosticStatus, /diagnostics_toplevel_state
@@ -165,6 +167,16 @@ class Aggregator
   /// Records all ROS warnings. No warnings are repeated.
   std::set<std::string> ros_warnings_;
 
+  /*
+   *!\brief Checks for new parameters to trigger reinitialization of the AnalyzerGroup and OtherAnalyzer
+   */
+  void parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr param_msg);
+
+  /*
+   *!\brief (re)initializes the AnalyzerGroup and OtherAnalyzer
+   */
+  void initAnalyzers();
+
   /*
    *!\brief Checks timestamp of message, and warns if timestamp is 0 (not set)
    */

diagnostic_aggregator/package.xml

@@ -12,7 +12,7 @@
   <license>BSD-3-Clause</license>
 
   <url type="website">http://www.ros.org/wiki/diagnostic_aggregator</url>
-  
+
   <author>Kevin Watts</author>
   <author email="[email protected]">Brice Rebsamen</author>
   <author email="[email protected]">Arne Nordmann</author>
@@ -22,6 +22,7 @@
 
   <build_depend>diagnostic_msgs</build_depend>
   <build_depend>pluginlib</build_depend>
+  <build_depend>rcl_interfaces</build_depend>
   <build_depend>rclcpp</build_depend>
   <build_depend>std_msgs</build_depend>
 

diagnostic_aggregator/src/add_analyzer.cpp

@@ -0,0 +1,112 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/**< \author Martin Cornelis */
+
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+#include "rcl_interfaces/msg/parameter.hpp"
+
+using namespace std::chrono_literals;
+
+class AddAnalyzer : public rclcpp::Node
+{
+public:
+  AddAnalyzer()
+  : Node("add_analyzer_node", "", rclcpp::NodeOptions().allow_undeclared_parameters(
+        true).automatically_declare_parameters_from_overrides(true))
+  {
+    client_ = this->create_client<rcl_interfaces::srv::SetParametersAtomically>(
+      "/analyzers/set_parameters_atomically");
+  }
+
+  void send_request()
+  {
+    while (!client_->wait_for_service(1s)) {
+      if (!rclcpp::ok()) {
+        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
+        return;
+      }
+      RCLCPP_INFO_ONCE(this->get_logger(), "service not available, waiting ...");
+    }
+    auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
+    std::map<std::string, rclcpp::Parameter> parameters;
+
+    if (!this->get_parameters("", parameters)) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to retrieve parameters");
+    }
+    for (const auto & [param_name, param] : parameters) {
+      // Find the suffix
+      size_t suffix_start = param_name.find_last_of('.');
+      // Remove suffix if it exists
+      if (suffix_start != std::string::npos) {
+        std::string stripped_param_name = param_name.substr(0, suffix_start);
+        // Check in map if the stripped param name with the added suffix "path" exists
+        // This indicates the parameter is part of an analyzer description
+        if (parameters.count(stripped_param_name + ".path") > 0) {
+          RCLCPP_INFO(this->get_logger(), param_name.c_str());
+          auto parameter_msg = param.to_parameter_msg();
+          request->parameters.push_back(parameter_msg);
+        }
+      }
+    }
+
+    auto result = client_->async_send_request(request);
+    // Wait for the result.
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      RCLCPP_INFO(this->get_logger(), "Parameters succesfully set");
+    } else {
+      RCLCPP_ERROR(this->get_logger(), "Failed to set parameters");
+    }
+  }
+
+private:
+  rclcpp::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr client_;
+  std::string analyzers_ns_ = "analyzers.";
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto add_analyzer = std::make_shared<AddAnalyzer>();
+  add_analyzer->send_request();
+  rclcpp::shutdown();
+
+  return 0;
+}

diagnostic_aggregator/src/aggregator.cpp

@@ -59,7 +59,8 @@ using diagnostic_msgs::msg::DiagnosticStatus;
 Aggregator::Aggregator()
 : n_(std::make_shared<rclcpp::Node>(
       "analyzers", "",
-      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))),
+      rclcpp::NodeOptions().allow_undeclared_parameters(true).
+      automatically_declare_parameters_from_overrides(true))),
   logger_(rclcpp::get_logger("Aggregator")),
   pub_rate_(1.0),
   history_depth_(1000),
@@ -69,6 +70,36 @@ Aggregator::Aggregator()
   last_top_level_state_(DiagnosticStatus::STALE)
 {
   RCLCPP_DEBUG(logger_, "constructor");
+  initAnalyzers();
+
+  diag_sub_ = n_->create_subscription<DiagnosticArray>(
+    "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_),
+    std::bind(&Aggregator::diagCallback, this, _1));
+  agg_pub_ = n_->create_publisher<DiagnosticArray>("/diagnostics_agg", 1);
+  toplevel_state_pub_ =
+    n_->create_publisher<DiagnosticStatus>("/diagnostics_toplevel_state", 1);
+
+  int publish_rate_ms = 1000 / pub_rate_;
+  publish_timer_ = n_->create_wall_timer(
+    std::chrono::milliseconds(publish_rate_ms),
+    std::bind(&Aggregator::publishData, this));
+
+  param_sub_ = n_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
+    "/parameter_events", 1, std::bind(&Aggregator::parameterCallback, this, _1));
+}
+
+void Aggregator::parameterCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg)
+{
+  if (msg->node == "/" + std::string(n_->get_name())) {
+    if (msg->new_parameters.size() != 0) {
+      base_path_ = "";
+      initAnalyzers();
+    }
+  }
+}
+
+void Aggregator::initAnalyzers()
+{
   bool other_as_errors = false;
 
   std::map<std::string, rclcpp::Parameter> parameters;
@@ -101,26 +132,17 @@ Aggregator::Aggregator()
   RCLCPP_DEBUG(
     logger_, "Aggregator critical publisher configured to: %s", (critical_ ? "true" : "false"));
 
-  analyzer_group_ = std::make_unique<AnalyzerGroup>();
-  if (!analyzer_group_->init(base_path_, "", n_)) {
-    RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!");
-  }
-
-  // Last analyzer handles remaining data
-  other_analyzer_ = std::make_unique<OtherAnalyzer>(other_as_errors);
-  other_analyzer_->init(base_path_);  // This always returns true
-
-  diag_sub_ = n_->create_subscription<DiagnosticArray>(
-    "/diagnostics", rclcpp::SystemDefaultsQoS().keep_last(history_depth_),
-    std::bind(&Aggregator::diagCallback, this, _1));
-  agg_pub_ = n_->create_publisher<DiagnosticArray>("/diagnostics_agg", 1);
-  toplevel_state_pub_ =
-    n_->create_publisher<DiagnosticStatus>("/diagnostics_toplevel_state", 1);
+  {  // lock the mutex while analyzer_group_ and other_analyzer_ are being updated
+    std::lock_guard<std::mutex> lock(mutex_);
+    analyzer_group_ = std::make_unique<AnalyzerGroup>();
+    if (!analyzer_group_->init(base_path_, "", n_)) {
+      RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!");
+    }
 
-  int publish_rate_ms = 1000 / pub_rate_;
-  publish_timer_ = n_->create_wall_timer(
-    std::chrono::milliseconds(publish_rate_ms),
-    std::bind(&Aggregator::publishData, this));
+    // Last analyzer handles remaining data
+    other_analyzer_ = std::make_unique<OtherAnalyzer>(other_as_errors);
+    other_analyzer_->init(base_path_);  // This always returns true
+  }
 }
 
 void Aggregator::checkTimestamp(const DiagnosticArray::SharedPtr diag_msg)