Make diagnostic aggregator composable

diagnostic_aggregator/CMakeLists.txt

@@ -15,12 +15,12 @@ find_package(ament_cmake REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
   src/status_item.cpp
-  src/analyzer_group.cpp
   src/aggregator.cpp)
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -30,6 +30,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
   ${std_msgs_TARGETS}
   pluginlib::pluginlib
   rclcpp::rclcpp
+  rclcpp_components::component
 )
 target_compile_definitions(${PROJECT_NAME}
   PRIVATE "DIAGNOSTIC_AGGREGATOR_BUILDING_DLL")
@@ -46,7 +47,8 @@ set(ANALYZERS "${PROJECT_NAME}_analyzers")
 add_library(${ANALYZERS} SHARED
   src/generic_analyzer.cpp
   src/discard_analyzer.cpp
-  src/ignore_analyzer.cpp)
+  src/ignore_analyzer.cpp
+  src/analyzer_group.cpp)
 target_include_directories(${ANALYZERS} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -60,14 +62,16 @@ target_link_libraries(${ANALYZERS} PUBLIC
 target_compile_definitions(${ANALYZERS}
   PRIVATE "DIAGNOSTIC_AGGREGATOR_BUILDING_DLL")
 
+# Register the aggregator node as a component
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "diagnostic_aggregator::Aggregator"
+  EXECUTABLE aggregator_node
+)
+
 # prevent pluginlib from using boost
 target_compile_definitions(${ANALYZERS} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
-# Aggregator node
-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)
 target_link_libraries(add_analyzer PUBLIC ${rcl_interfaces_TARGETS} rclcpp::rclcpp)
@@ -165,11 +169,6 @@ if(BUILD_TESTING)
   # )
 endif()
 
-install(
-  TARGETS aggregator_node
-  DESTINATION lib/${PROJECT_NAME}
-)
-
 install(
   TARGETS add_analyzer
   DESTINATION lib/${PROJECT_NAME}
@@ -194,8 +193,11 @@ ament_python_install_package(${PROJECT_NAME})
 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)
+configure_file(example/compose_example.launch.py.in compose_example.launch.py @ONLY)
 install( # launch descriptor
-  FILES ${CMAKE_CURRENT_BINARY_DIR}/example.launch.py
+  FILES
+    ${CMAKE_CURRENT_BINARY_DIR}/example.launch.py
+    ${CMAKE_CURRENT_BINARY_DIR}/compose_example.launch.py
   DESTINATION share/${PROJECT_NAME}
 )
 install( # example publisher

diagnostic_aggregator/example/compose_example.launch.py.in

@@ -0,0 +1,35 @@
+"""Launch analyzer loader with parameters from yaml."""
+
+import launch
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+analyzer_params_filepath = "@ANALYZER_PARAMS_FILEPATH@"
+add_analyzer_params_filepath = "@ADD_ANALYZER_PARAMS_FILEPATH@"
+
+
+def generate_launch_description():
+    container = ComposableNodeContainer(
+        name="diagnostics_container",
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='diagnostic_aggregator',
+                plugin='diagnostic_aggregator::Aggregator',
+                name='analyzers',
+                parameters=[analyzer_params_filepath]
+            )
+        ]
+    )
+
+    diag_publisher = Node(
+        package='diagnostic_aggregator',
+        executable='example_pub.py'
+    )
+    return launch.LaunchDescription([
+        container,
+        # add_analyzer,
+        diag_publisher,
+    ])

diagnostic_aggregator/example/example_pub.py

@@ -114,8 +114,11 @@ def timer_callback(self):
 def main(args=None):
     rclpy.init(args=args)
 
-    node = DiagnosticTalker()
-    rclpy.spin(node)
+    try:
+        node = DiagnosticTalker()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
 
     node.destroy_node()
     rclpy.try_shutdown()

diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp

@@ -46,11 +46,12 @@
 #include <vector>
 
 #include "diagnostic_aggregator/analyzer.hpp"
-#include "diagnostic_aggregator/analyzer_group.hpp"
 #include "diagnostic_aggregator/other_analyzer.hpp"
 #include "diagnostic_aggregator/status_item.hpp"
 #include "diagnostic_aggregator/visibility_control.hpp"
 
+#include "pluginlib/class_loader.hpp"
+
 #include "diagnostic_msgs/msg/diagnostic_array.hpp"
 #include "diagnostic_msgs/msg/diagnostic_status.hpp"
 #include "diagnostic_msgs/msg/key_value.hpp"
@@ -130,6 +131,8 @@ class Aggregator
   DIAGNOSTIC_AGGREGATOR_PUBLIC
   rclcpp::Node::SharedPtr get_node() const;
 
+  DIAGNOSTIC_AGGREGATOR_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
 private:
   rclcpp::Node::SharedPtr n_;
 
@@ -156,7 +159,8 @@ class Aggregator
    */
   void diagCallback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr diag_msg);
 
-  std::unique_ptr<AnalyzerGroup> analyzer_group_;
+  std::shared_ptr<pluginlib::ClassLoader<Analyzer>> analyzer_loader_;
+  std::shared_ptr<Analyzer> analyzer_group_;
   std::unique_ptr<OtherAnalyzer> other_analyzer_;
 
   std::string base_path_; /**< \brief Prepended to all status names of aggregator. */

diagnostic_aggregator/package.xml

@@ -24,10 +24,13 @@
   <build_depend>pluginlib</build_depend>
   <build_depend>rcl_interfaces</build_depend>
   <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_components</build_depend>
   <build_depend>std_msgs</build_depend>
 
   <depend>rclpy</depend>
 
+  <exec_depend>rclcpp_components</exec_depend>
+
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diagnostic_aggregator/src/aggregator.cpp

@@ -137,9 +137,21 @@ void Aggregator::initAnalyzers()
 
   {  // 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!");
+
+    // Load analyzer_group as a plugin
+    try {
+      if (!analyzer_loader_) {
+        analyzer_loader_ = std::make_shared<pluginlib::ClassLoader<Analyzer>>(
+          "diagnostic_aggregator", "diagnostic_aggregator::Analyzer");
+      }
+      analyzer_group_ = analyzer_loader_->createSharedInstance(
+        "diagnostic_aggregator/AnalyzerGroup");
+
+      if (!analyzer_group_->init(base_path_, "", n_)) {
+        RCLCPP_ERROR(logger_, "Analyzer group for diagnostic aggregator failed to initialize!");
+      }
+    } catch (pluginlib::PluginlibException & e) {
+      RCLCPP_ERROR(logger_, "Failed to load AnalyzerGroup plugin: %s", e.what());
     }
 
     // Last analyzer handles remaining data
@@ -269,4 +281,13 @@ rclcpp::Node::SharedPtr Aggregator::get_node() const
   return this->n_;
 }
 
+rclcpp::node_interfaces::NodeBaseInterface::SharedPtr Aggregator::get_node_base_interface() const
+{
+  RCLCPP_DEBUG(logger_, "get_node_base_interface()");
+  return this->n_->get_node_base_interface();
+}
+
 }  // namespace diagnostic_aggregator
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_aggregator::Aggregator)