From d7b637df8b7e71a5207bb93cdb75e32bad820a1b Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 26 Feb 2026 13:50:55 -0800 Subject: [PATCH 1/5] Fix issue #23: prevent duplicate diagnostics when external node is already publishing Subscribe to /diagnostics in GreenwaveMonitor to track topic names already published externally. add_topic() now returns an error if a topic's diagnostics are already being published by an external node, preventing duplicate and potentially conflicting diagnostic entries. Also fix copyright-required pre-commit hook to only run at pre-commit stage, preventing it from incorrectly checking COMMIT_EDITMSG for a copyright header. Signed-off-by: Blake McHale Co-Authored-By: Claude Sonnet 4.6 --- .pre-commit-config.yaml | 1 + .../include/greenwave_monitor.hpp | 11 +++++ greenwave_monitor/src/greenwave_monitor.cpp | 36 ++++++++++++++ .../test/test_greenwave_monitor.py | 47 +++++++++++++++++++ 4 files changed, 95 insertions(+) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index aa2eafb..b3420c4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -38,6 +38,7 @@ repos: rev: 1.6.1 hooks: - id: copyright-required + stages: [pre-commit] exclude: '(^\.git/|(\.ini|\.json|\.service|__init__\.py|\.md|\.gitkeep|\.conf|LICENSE|\.toml|\.template|\.style\..*|\.gitattributes|\.gitignore|\.editorconfig|\.bash-completion|\.install|\.links|changelog|debian/source/format|.codespellrc|copyright|ament_code_style\.cfg|test_pep257\.py|test_flake8\.py|test_copyright\.py)$)' - repo: local hooks: diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 4302888..5d3389e 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -20,7 +20,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -48,6 +50,9 @@ class GreenwaveMonitor : public rclcpp::Node if (init_timer_) { init_timer_->cancel(); } + // Reset diagnostics subscription before clearing internal state to prevent + // callbacks from firing after greenwave_diagnostics_ is destroyed + diagnostics_subscription_.reset(); // Clear diagnostics before base Node destructor runs to avoid accessing invalid node state greenwave_diagnostics_.clear(); subscriptions_.clear(); @@ -88,6 +93,8 @@ class GreenwaveMonitor : public rclcpp::Node void add_topics_from_parameters(); + void diagnostics_callback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg); + std::map> greenwave_diagnostics_; std::vector> subscriptions_; @@ -98,4 +105,8 @@ class GreenwaveMonitor : public rclcpp::Node rclcpp::Service::SharedPtr set_expected_frequency_service_; greenwave_diagnostics::TimeCheckPreset time_check_preset_; + rclcpp::Subscription::SharedPtr + diagnostics_subscription_; + std::set externally_diagnosed_topics_; + std::mutex externally_diagnosed_topics_mutex_; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 48b26ef..75bab28 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -99,6 +99,14 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) timer_ = this->create_wall_timer( 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); + // Subscribe to /diagnostics early so we can detect external publishers before + // deferred_init() adds topics. This gives us the best chance of catching + // externally-published diagnostics before add_topic() is called. + diagnostics_subscription_ = + this->create_subscription( + "/diagnostics", 10, + std::bind(&GreenwaveMonitor::diagnostics_callback, this, std::placeholders::_1)); + // Defer topic discovery to allow the ROS graph to settle before querying other nodes init_timer_ = this->create_wall_timer( 100ms, [this]() { @@ -170,6 +178,19 @@ void GreenwaveMonitor::timer_callback() RCLCPP_INFO(this->get_logger(), "===================================================="); } +void GreenwaveMonitor::diagnostics_callback( + const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) +{ + std::lock_guard lock(externally_diagnosed_topics_mutex_); + for (const auto & status : msg->status) { + // Only track topic names that are not already monitored by us. This prevents + // our own published diagnostics from blocking re-adds after a remove_topic(). + if (greenwave_diagnostics_.find(status.name) == greenwave_diagnostics_.end()) { + externally_diagnosed_topics_.insert(status.name); + } + } +} + void GreenwaveMonitor::handle_manage_topic( const std::shared_ptr request, std::shared_ptr response) @@ -325,6 +346,21 @@ bool GreenwaveMonitor::add_topic( return false; } + // Check if an external node is already publishing diagnostics for this topic. + // Adding a duplicate would create redundant and potentially conflicting diagnostics. + { + std::lock_guard lock(externally_diagnosed_topics_mutex_); + if (externally_diagnosed_topics_.count(topic) > 0) { + message = "Topic '" + topic + + "' already has diagnostics published externally on /diagnostics"; + RCLCPP_ERROR( + this->get_logger(), + "Refusing to add topic '%s': diagnostics already published externally on /diagnostics", + topic.c_str()); + return false; + } + } + RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); auto maybe_type = find_topic_type(topic, max_retries, retry_wait_s); diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 4fd10c4..2e92b9c 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -23,6 +23,8 @@ import time import unittest +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus + from greenwave_monitor.test_utils import ( call_manage_topic_service, collect_diagnostics_for_topic, @@ -415,6 +417,51 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc f'Topic {NONEXISTENT_TOPIC} should not be monitored' ) + def test_reject_externally_diagnosed_topic( + self, expected_frequency, message_type, tolerance_hz): + """Test that add_topic() fails when the topic already has external diagnostics. + + Verifies the fix for issue #23: Greenwave Monitor should not create a duplicate + diagnostics object when an external node is already publishing diagnostics for + the same topic on /diagnostics. + """ + if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: + self.skipTest('Only running external diagnostics test once') + + service_client = self.check_node_launches_successfully() + + external_topic = '/external_diag_topic' + + # Publish several DiagnosticArray messages for the external topic so the + # Greenwave Monitor's /diagnostics subscriber can detect them. + diag_pub = self.test_node.create_publisher(DiagnosticArray, '/diagnostics', 10) + try: + end_time = time.time() + 3.0 + while time.time() < end_time: + msg = DiagnosticArray() + status = DiagnosticStatus() + status.name = external_topic + status.level = DiagnosticStatus.OK + status.message = 'External publisher' + msg.status = [status] + diag_pub.publish(msg) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + # Greenwave Monitor should refuse to add the topic because an external + # node is already publishing diagnostics for it. + response = self.call_manage_topic( + add=True, topic=external_topic, service_client=service_client) + self.assertFalse( + response.success, + 'add_topic() should fail when external diagnostics already exist for the topic' + ) + self.assertIn( + 'external', response.message.lower(), + 'Error message should mention external diagnostics' + ) + finally: + self.test_node.destroy_publisher(diag_pub) + if __name__ == '__main__': unittest.main() From 75329bf982dd6cd18fc184ad88faa52c86ca589f Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 27 Feb 2026 09:49:13 -0800 Subject: [PATCH 2/5] add protective guard Signed-off-by: Blake McHale --- .pre-commit-config.yaml | 1 - greenwave_monitor/src/greenwave_monitor.cpp | 12 ++++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b3420c4..aa2eafb 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -38,7 +38,6 @@ repos: rev: 1.6.1 hooks: - id: copyright-required - stages: [pre-commit] exclude: '(^\.git/|(\.ini|\.json|\.service|__init__\.py|\.md|\.gitkeep|\.conf|LICENSE|\.toml|\.template|\.style\..*|\.gitattributes|\.gitignore|\.editorconfig|\.bash-completion|\.install|\.links|changelog|debian/source/format|.codespellrc|copyright|ament_code_style\.cfg|test_pep257\.py|test_flake8\.py|test_copyright\.py)$)' - repo: local hooks: diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 75bab28..d6e2fc9 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -397,6 +397,18 @@ bool GreenwaveMonitor::add_topic( bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & message) { + { + std::lock_guard lock(externally_diagnosed_topics_mutex_); + if (externally_diagnosed_topics_.count(topic) > 0) { + message = "Topic '" + topic + "' is externally managed and cannot be removed"; + RCLCPP_ERROR( + this->get_logger(), + "Refusing to remove topic '%s': topic is externally managed", + topic.c_str()); + return false; + } + } + auto diag_it = greenwave_diagnostics_.find(topic); if (diag_it == greenwave_diagnostics_.end()) { message = "Topic not found"; From 93bdc068cc56b4432adb665067603852d336abf5 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 27 Feb 2026 10:00:22 -0800 Subject: [PATCH 3/5] Fix wording Signed-off-by: Blake McHale --- docs/DESIGN_AND_IMPLEMENTATION.md | 1 - greenwave_monitor/src/greenwave_monitor.cpp | 21 ++++---- .../test/test_greenwave_monitor.py | 53 ++++++++++++++----- 3 files changed, 50 insertions(+), 25 deletions(-) diff --git a/docs/DESIGN_AND_IMPLEMENTATION.md b/docs/DESIGN_AND_IMPLEMENTATION.md index 4bd4655..2ec4a67 100644 --- a/docs/DESIGN_AND_IMPLEMENTATION.md +++ b/docs/DESIGN_AND_IMPLEMENTATION.md @@ -161,7 +161,6 @@ Invalid values are ignored and the default `header_with_nodetime_fallback` is us `GreenwaveMonitor::has_header_from_type()`; unknown types fall back to no-header behavior. - `publishDiagnostics()` marks status as `STALE` if no fresh `updateDiagnostics()` happened since the previous publish. - `setExpectedDt()` requires `expected_hz > 0`; zero disables useful timing checks. -- Do not add externally monitored topics (those already using inline `GreenwaveDiagnostics`) via `manage_topic` or the terminal UI, as this will create a duplicate diagnostic subscription for the same topic. ## Where To Look In Code diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index d6e2fc9..d3b02fb 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -340,27 +340,26 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) bool GreenwaveMonitor::add_topic( const std::string & topic, std::string & message, int max_retries, double retry_wait_s) { - // Check if topic already exists - if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { - message = "Topic already being monitored"; - return false; - } - // Check if an external node is already publishing diagnostics for this topic. // Adding a duplicate would create redundant and potentially conflicting diagnostics. { std::lock_guard lock(externally_diagnosed_topics_mutex_); if (externally_diagnosed_topics_.count(topic) > 0) { - message = "Topic '" + topic + - "' already has diagnostics published externally on /diagnostics"; + message = "Topic already externally monitored"; RCLCPP_ERROR( this->get_logger(), - "Refusing to add topic '%s': diagnostics already published externally on /diagnostics", + "Refusing to add topic '%s': topic already externally monitored", topic.c_str()); return false; } } + // Check if topic already exists + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { + message = "Topic already being monitored"; + return false; + } + RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); auto maybe_type = find_topic_type(topic, max_retries, retry_wait_s); @@ -400,10 +399,10 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes { std::lock_guard lock(externally_diagnosed_topics_mutex_); if (externally_diagnosed_topics_.count(topic) > 0) { - message = "Topic '" + topic + "' is externally managed and cannot be removed"; + message = "Topic is externally monitored"; RCLCPP_ERROR( this->get_logger(), - "Refusing to remove topic '%s': topic is externally managed", + "Refusing to remove topic '%s': topic is externally monitored", topic.c_str()); return false; } diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 2e92b9c..2c8c8dd 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -417,23 +417,14 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc f'Topic {NONEXISTENT_TOPIC} should not be monitored' ) - def test_reject_externally_diagnosed_topic( + def test_reject_add_externally_diagnosed_topic( self, expected_frequency, message_type, tolerance_hz): - """Test that add_topic() fails when the topic already has external diagnostics. - - Verifies the fix for issue #23: Greenwave Monitor should not create a duplicate - diagnostics object when an external node is already publishing diagnostics for - the same topic on /diagnostics. - """ + """Test that add_topic() fails when the topic already has external diagnostics.""" if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: self.skipTest('Only running external diagnostics test once') service_client = self.check_node_launches_successfully() - external_topic = '/external_diag_topic' - - # Publish several DiagnosticArray messages for the external topic so the - # Greenwave Monitor's /diagnostics subscriber can detect them. diag_pub = self.test_node.create_publisher(DiagnosticArray, '/diagnostics', 10) try: end_time = time.time() + 3.0 @@ -456,8 +447,44 @@ def test_reject_externally_diagnosed_topic( 'add_topic() should fail when external diagnostics already exist for the topic' ) self.assertIn( - 'external', response.message.lower(), - 'Error message should mention external diagnostics' + 'externally monitored', response.message.lower(), + 'Error message should mention topic is externally monitored' + ) + finally: + self.test_node.destroy_publisher(diag_pub) + + def test_reject_remove_externally_diagnosed_topic( + self, expected_frequency, message_type, tolerance_hz): + """Test that remove_topic() fails when the topic is externally monitored.""" + if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: + self.skipTest('Only running external diagnostics removal test once') + + service_client = self.check_node_launches_successfully() + + external_topic = '/external_remove_topic' + + diag_pub = self.test_node.create_publisher(DiagnosticArray, '/diagnostics', 10) + try: + end_time = time.time() + 3.0 + while time.time() < end_time: + msg = DiagnosticArray() + status = DiagnosticStatus() + status.name = external_topic + status.level = DiagnosticStatus.OK + status.message = 'External publisher' + msg.status = [status] + diag_pub.publish(msg) + rclpy.spin_once(self.test_node, timeout_sec=0.1) + + response = self.call_manage_topic( + add=False, topic=external_topic, service_client=service_client) + self.assertFalse( + response.success, + 'remove_topic() should fail when topic is externally monitored' + ) + self.assertIn( + 'externally monitored', response.message.lower(), + 'Error message should mention topic is externally monitored' ) finally: self.test_node.destroy_publisher(diag_pub) From 5e968eb9b7e4eda17b6aa7d8d3bcba6dee5a164a Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 27 Feb 2026 10:54:07 -0800 Subject: [PATCH 4/5] Make warning and update wording Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index d3b02fb..16fb679 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -345,10 +345,10 @@ bool GreenwaveMonitor::add_topic( { std::lock_guard lock(externally_diagnosed_topics_mutex_); if (externally_diagnosed_topics_.count(topic) > 0) { - message = "Topic already externally monitored"; + message = "Topic is externally monitored"; RCLCPP_ERROR( this->get_logger(), - "Refusing to add topic '%s': topic already externally monitored", + "Refusing to add topic '%s': topic is externally monitored", topic.c_str()); return false; } @@ -364,7 +364,7 @@ bool GreenwaveMonitor::add_topic( auto maybe_type = find_topic_type(topic, max_retries, retry_wait_s); if (!maybe_type.has_value()) { - RCLCPP_ERROR(this->get_logger(), "Failed to find type for topic '%s'", topic.c_str()); + RCLCPP_WARN(this->get_logger(), "Failed to find type for topic '%s'", topic.c_str()); message = "Failed to find type for topic"; return false; } @@ -400,7 +400,7 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes std::lock_guard lock(externally_diagnosed_topics_mutex_); if (externally_diagnosed_topics_.count(topic) > 0) { message = "Topic is externally monitored"; - RCLCPP_ERROR( + RCLCPP_WARN( this->get_logger(), "Refusing to remove topic '%s': topic is externally monitored", topic.c_str()); From a25e74aa2f2c870e74e98bc85fc27c51ae991c39 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 27 Feb 2026 10:56:29 -0800 Subject: [PATCH 5/5] Fix Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 16fb679..28941f0 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -346,7 +346,7 @@ bool GreenwaveMonitor::add_topic( std::lock_guard lock(externally_diagnosed_topics_mutex_); if (externally_diagnosed_topics_.count(topic) > 0) { message = "Topic is externally monitored"; - RCLCPP_ERROR( + RCLCPP_WARN( this->get_logger(), "Refusing to add topic '%s': topic is externally monitored", topic.c_str()); @@ -364,7 +364,7 @@ bool GreenwaveMonitor::add_topic( auto maybe_type = find_topic_type(topic, max_retries, retry_wait_s); if (!maybe_type.has_value()) { - RCLCPP_WARN(this->get_logger(), "Failed to find type for topic '%s'", topic.c_str()); + RCLCPP_ERROR(this->get_logger(), "Failed to find type for topic '%s'", topic.c_str()); message = "Failed to find type for topic"; return false; }