Merge branch 'feature/add-analyzers-through-parameters' into nobleo-ros2

diagnostic_aggregator/CMakeLists.txt

@@ -82,6 +82,7 @@ if(BUILD_TESTING)
   find_package(launch_testing_ament_cmake REQUIRED)
 
   file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/aggregator_node" AGGREGATOR_NODE)
+  file(TO_CMAKE_PATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/add_analyzer" ADD_ANALYZER)
   file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/test_listener.py" TEST_LISTENER)
   set(create_analyzers_tests
     "primitive_analyzers"
@@ -129,6 +130,27 @@ if(BUILD_TESTING)
     )
   endforeach()
 
+  set(add_analyzers_tests
+  "all_analyzers")
+
+  foreach(test_name ${add_analyzers_tests})
+    file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/default.yaml" PARAMETER_FILE)
+    file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/${test_name}.yaml" ADD_PARAMETER_FILE)
+    file(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/expected_output/add_${test_name}" EXPECTED_OUTPUT)
+
+    configure_file(
+      "test/add_analyzers.launch.py.in"
+      "test_add_${test_name}.launch.py"
+      @ONLY
+    )
+    add_launch_test(
+      "${CMAKE_CURRENT_BINARY_DIR}/test_add_${test_name}.launch.py"
+      TARGET "test_add_${test_name}"
+      TIMEOUT 30
+      ENV
+    )
+  endforeach()
+
   add_launch_test(
     test/test_critical_pub.py
     TIMEOUT 30
@@ -162,6 +184,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
@@ -172,7 +195,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/src/add_analyzer.cpp

@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2009, Willow Garage, Inc.
+ *  Copyright (c) 2024, Nobleo Technology
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     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
+ *   * Neither the name of the copyright holder nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -50,7 +50,7 @@ class AddAnalyzer : public rclcpp::Node
         true).automatically_declare_parameters_from_overrides(true))
   {
     client_ = this->create_client<rcl_interfaces::srv::SetParametersAtomically>(
-      "/diagnostics_agg/set_parameters_atomically");
+      "/analyzers/set_parameters_atomically");
   }
 
   void send_request()
@@ -64,14 +64,25 @@ class AddAnalyzer : public rclcpp::Node
     }
     auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
     std::map<std::string, rclcpp::Parameter> parameters;
-    if (this->get_parameters("", parameters)) {
-      for (const auto & param : parameters) {
-        if (param.first.substr(0, analyzers_ns_.length()).compare(analyzers_ns_) == 0) {
-          auto parameter_msg = param.second.to_parameter_msg();
+
+    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) {
+          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) ==
@@ -85,7 +96,6 @@ class AddAnalyzer : public rclcpp::Node
 
 private:
   rclcpp::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr client_;
-  std::string analyzers_ns_ = "analyzers.";
 };
 
 int main(int argc, char ** argv)

diagnostic_aggregator/src/aggregator.cpp

@@ -92,6 +92,7 @@ void Aggregator::parameterCallback(const rcl_interfaces::msg::ParameterEvent::Sh
 {
   if (msg->node == "/" + std::string(n_->get_name())) {
     if (msg->new_parameters.size() != 0) {
+      base_path_ = "";
       initAnalyzers();
     }
   }

diagnostic_aggregator/test/add_analyzers.launch.py.in

@@ -0,0 +1,74 @@
+import os
+
+import unittest
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch.events import matches_action
+from launch.events.process import ShutdownProcess
+
+import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing.util
+import launch_testing_ros
+
+
+def generate_test_description():
+    os.environ['OSPL_VERBOSITY'] = '8'
+    os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{message}'
+
+    aggregator_node = ExecuteProcess(
+        cmd=[
+            "@AGGREGATOR_NODE@",
+            "--ros-args",
+            "--params-file",
+            "@PARAMETER_FILE@"
+            ],
+        name='aggregator_node',
+        emulate_tty=True,
+        output='screen')
+
+    add_analyzer = ExecuteProcess(
+        cmd=[
+            "@ADD_ANALYZER@",
+            "--ros-args",
+            "--params-file",
+            "@ADD_PARAMETER_FILE@"
+            ],
+        name='add_analyzer',
+        emulate_tty=True,
+        output='screen')
+
+    launch_description = LaunchDescription()
+    launch_description.add_action(aggregator_node)
+    launch_description.add_action(add_analyzer)
+    launch_description.add_action(launch_testing.util.KeepAliveProc())
+    launch_description.add_action(launch_testing.actions.ReadyToTest())
+    return launch_description, locals()
+
+class TestAggregator(unittest.TestCase):
+
+    def test_processes_output(self, proc_output, aggregator_node):
+        """Check aggregator logging output for expected strings."""
+
+        from launch_testing.tools.output import get_default_filtered_prefixes
+        output_filter = launch_testing_ros.tools.basic_output_filter(
+            filtered_prefixes=get_default_filtered_prefixes() + ['service not available, waiting...'],
+            filtered_rmw_implementation='@rmw_implementation@'
+        )
+        proc_output.assertWaitFor(
+            expected_output=launch_testing.tools.expected_output_from_file(path="@EXPECTED_OUTPUT@"),
+            process=aggregator_node,
+            output_filter=output_filter,
+            timeout=15
+        )
+
+        import time
+        time.sleep(1)
+
+@launch_testing.post_shutdown_test()
+class TestAggregatorShutdown(unittest.TestCase):
+
+    def test_last_process_exit_code(self, proc_info, aggregator_node):
+        launch_testing.asserts.assertExitCodes(proc_info, process=aggregator_node)

diagnostic_aggregator/test/all_analyzers.yaml

@@ -1,4 +1,4 @@
-analyzers:
+/**:
   ros__parameters:
     path: BASIC
     prefix1:

diagnostic_aggregator/test/analyzer_group.yaml

@@ -1,4 +1,4 @@
-analyzers:
+/**:
   ros__parameters:
     path: TEST
     primary:

diagnostic_aggregator/test/default.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    path: BASIC
+    prefix0:
+      type: diagnostic_aggregator/GenericAnalyzer
+      path: Zeroth
+      contains: [
+        'contain0a',
+        'contain0b' ]

diagnostic_aggregator/test/empty_root_path.yaml

@@ -1,4 +1,4 @@
-analyzers:
+/**:
   ros__parameters:
     primary:
       type: 'diagnostic_aggregator/AnalyzerGroup'

diagnostic_aggregator/test/expected_output/add_all_analyzers.txt

@@ -0,0 +1,6 @@
+/BASIC/Zeroth
+prefix0
+/BASIC/First
+prefix1
+/BASIC/Third
+prefix3

diagnostic_aggregator/test/expected_stale_analyzers.yaml

@@ -1,4 +1,4 @@
-analyzers:
+/**:
   my_path:
     type: diagnostic_aggregator/GenericAnalyzer
     path: My Path

diagnostic_aggregator/test/multiple_match_analyzers.yaml

@@ -1,4 +1,4 @@
-analyzers:
+/**:
   my_path:
     type: diagnostic_aggregator/GenericAnalyzer
     path: Header1

diagnostic_aggregator/test/primitive_analyzers.yaml

@@ -1,4 +1,4 @@
-analyzers:
+/**:
   ros__parameters:
     log_level: debug
     primary: