Merge branch 'ros2' into fix/small_updater_fixes

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)
@@ -77,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"
@@ -124,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
@@ -140,6 +167,11 @@ install(
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(
+  TARGETS add_analyzer
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(
   TARGETS ${PROJECT_NAME} ${ANALYZERS}
   EXPORT ${PROJECT_NAME}Targets
@@ -157,6 +189,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
@@ -167,7 +200,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

@@ -135,6 +135,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,110 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Nobleo Technology
+ *  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 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.
+ *
+ *  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) {
+          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_;
+};
+
+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;
+}