From 7e891e06e0a1c22cc4c4c15307cf1ae22cf787a5 Mon Sep 17 00:00:00 2001 From: Natesh Narain Date: Sun, 4 Jan 2026 12:51:32 -0500 Subject: [PATCH 1/3] Resolves #321. Make aggregator composable --- diagnostic_aggregator/CMakeLists.txt | 34 ++++++++++++------ .../example/compose_example.launch.py.in | 35 +++++++++++++++++++ diagnostic_aggregator/example/example_pub.py | 7 ++-- .../diagnostic_aggregator/aggregator.hpp | 8 +++-- diagnostic_aggregator/package.xml | 3 ++ diagnostic_aggregator/src/aggregator.cpp | 27 ++++++++++++-- 6 files changed, 96 insertions(+), 18 deletions(-) create mode 100644 diagnostic_aggregator/example/compose_example.launch.py.in diff --git a/diagnostic_aggregator/CMakeLists.txt b/diagnostic_aggregator/CMakeLists.txt index 36ae42564..6df8a72c9 100644 --- a/diagnostic_aggregator/CMakeLists.txt +++ b/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 $ @@ -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 $ $) @@ -60,13 +62,20 @@ 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}) +# # 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) @@ -165,10 +174,10 @@ if(BUILD_TESTING) # ) endif() -install( - TARGETS aggregator_node - DESTINATION lib/${PROJECT_NAME} -) +# install( +# TARGETS aggregator_node +# DESTINATION lib/${PROJECT_NAME} +# ) install( TARGETS add_analyzer @@ -194,8 +203,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 diff --git a/diagnostic_aggregator/example/compose_example.launch.py.in b/diagnostic_aggregator/example/compose_example.launch.py.in new file mode 100644 index 000000000..81b4bf9ac --- /dev/null +++ b/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, + ]) diff --git a/diagnostic_aggregator/example/example_pub.py b/diagnostic_aggregator/example/example_pub.py index 0c1b10436..bb444fd4c 100755 --- a/diagnostic_aggregator/example/example_pub.py +++ b/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() diff --git a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp index 1d2c7e638..0201b7eb4 100644 --- a/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp +++ b/diagnostic_aggregator/include/diagnostic_aggregator/aggregator.hpp @@ -46,11 +46,12 @@ #include #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 analyzer_group_; + std::shared_ptr> analyzer_loader_; + std::shared_ptr analyzer_group_; std::unique_ptr other_analyzer_; std::string base_path_; /**< \brief Prepended to all status names of aggregator. */ diff --git a/diagnostic_aggregator/package.xml b/diagnostic_aggregator/package.xml index 3a57e1dc9..210a7103a 100644 --- a/diagnostic_aggregator/package.xml +++ b/diagnostic_aggregator/package.xml @@ -24,10 +24,13 @@ pluginlib rcl_interfaces rclcpp + rclcpp_components std_msgs rclpy + rclcpp_components + ament_cmake_gtest ament_cmake_pytest ament_lint_auto diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index 6b60eee32..6c20df775 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/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 lock(mutex_); - analyzer_group_ = std::make_unique(); - 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>( + "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) From 05ebfece45ad0a6c1f0ea82620d036134f8d0884 Mon Sep 17 00:00:00 2001 From: Natesh Narain Date: Sun, 4 Jan 2026 12:55:41 -0500 Subject: [PATCH 2/3] clean up --- diagnostic_aggregator/CMakeLists.txt | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/diagnostic_aggregator/CMakeLists.txt b/diagnostic_aggregator/CMakeLists.txt index 6df8a72c9..749814bb2 100644 --- a/diagnostic_aggregator/CMakeLists.txt +++ b/diagnostic_aggregator/CMakeLists.txt @@ -72,11 +72,6 @@ rclcpp_components_register_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) @@ -174,11 +169,6 @@ if(BUILD_TESTING) # ) endif() -# install( -# TARGETS aggregator_node -# DESTINATION lib/${PROJECT_NAME} -# ) - install( TARGETS add_analyzer DESTINATION lib/${PROJECT_NAME} From 7727e25b186c56d4b147abf244c1def353f339b6 Mon Sep 17 00:00:00 2001 From: Natesh Narain Date: Sun, 4 Jan 2026 13:05:32 -0500 Subject: [PATCH 3/3] lint --- diagnostic_aggregator/example/example_pub.py | 2 +- diagnostic_aggregator/src/aggregator.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/diagnostic_aggregator/example/example_pub.py b/diagnostic_aggregator/example/example_pub.py index bb444fd4c..2410f6952 100755 --- a/diagnostic_aggregator/example/example_pub.py +++ b/diagnostic_aggregator/example/example_pub.py @@ -117,7 +117,7 @@ def main(args=None): try: node = DiagnosticTalker() rclpy.spin(node) - except KeyboardInterrupt: + except KeyboardInterrupt: pass node.destroy_node() diff --git a/diagnostic_aggregator/src/aggregator.cpp b/diagnostic_aggregator/src/aggregator.cpp index 6c20df775..4fb181dbd 100644 --- a/diagnostic_aggregator/src/aggregator.cpp +++ b/diagnostic_aggregator/src/aggregator.cpp @@ -137,7 +137,7 @@ void Aggregator::initAnalyzers() { // lock the mutex while analyzer_group_ and other_analyzer_ are being updated std::lock_guard lock(mutex_); - + // Load analyzer_group as a plugin try { if (!analyzer_loader_) { @@ -146,7 +146,7 @@ void Aggregator::initAnalyzers() } 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!"); }