From b0f0ac5bf6d33c7ae7084ce7449d16a817d6b6f4 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 12 Jan 2026 19:49:22 -0800 Subject: [PATCH 01/14] Add support for loading greenwave monitor parameters from a list Signed-off-by: Blake McHale --- greenwave_monitor/CMakeLists.txt | 2 +- greenwave_monitor/config/example.yaml | 11 ++ greenwave_monitor/examples/example.launch.py | 12 +- .../greenwave_monitor/ncurses_frontend.py | 7 +- .../greenwave_monitor/ui_adaptor.py | 12 ++ .../include/greenwave_monitor.hpp | 22 ++- .../include/message_diagnostics.hpp | 16 ++ .../include/minimal_publisher_node.hpp | 8 + greenwave_monitor/src/greenwave_monitor.cpp | 140 ++++++++++++++---- 9 files changed, 190 insertions(+), 40 deletions(-) create mode 100644 greenwave_monitor/config/example.yaml diff --git a/greenwave_monitor/CMakeLists.txt b/greenwave_monitor/CMakeLists.txt index 7c31f6d..bccd4a0 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -60,7 +60,7 @@ install(TARGETS minimal_publisher_node DESTINATION lib/${PROJECT_NAME}) install( - DIRECTORY launch examples + DIRECTORY launch examples config DESTINATION share/${PROJECT_NAME} ) diff --git a/greenwave_monitor/config/example.yaml b/greenwave_monitor/config/example.yaml new file mode 100644 index 0000000..371bd4f --- /dev/null +++ b/greenwave_monitor/config/example.yaml @@ -0,0 +1,11 @@ +# Example configuration for greenwave_monitor +# Defines topics to monitor with their expected frequencies and tolerances + +greenwave_monitor: + ros__parameters: + greenwave_diagnostics: + /imu_topic: + expected_frequency: 100.0 + tolerance: 10.0 + /image_topic: + expected_frequency: 0.0 # No initial monitoring for this topic diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index b7266c1..ff6bb88 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2025, NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026, NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,12 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import LogInfo from launch_ros.actions import Node def generate_launch_description(): + pkg_share = get_package_share_directory('greenwave_monitor') + config_file = os.path.join(pkg_share, 'config', 'example.yaml') + return LaunchDescription([ Node( package='greenwave_monitor', @@ -51,9 +57,7 @@ def generate_launch_description(): executable='greenwave_monitor', name='greenwave_monitor', output='log', - parameters=[ - {'topics': ['/imu_topic', '/image_topic', '/string_topic']} - ], + parameters=[config_file, {'topics': ['/string_topic']}], ), LogInfo( msg='Run `ros2 run r2s_gw r2s_gw` in another terminal to see the demo output ' diff --git a/greenwave_monitor/greenwave_monitor/ncurses_frontend.py b/greenwave_monitor/greenwave_monitor/ncurses_frontend.py index 77b76c1..d8b364b 100644 --- a/greenwave_monitor/greenwave_monitor/ncurses_frontend.py +++ b/greenwave_monitor/greenwave_monitor/ncurses_frontend.py @@ -337,8 +337,11 @@ def curses_main(stdscr, node): # Get expected frequency expected_hz, tolerance = node.ui_adaptor.get_expected_frequency(topic_name) - if expected_hz > 0: - expected_freq_display = f'{expected_hz:.1f}Hz'.ljust(12) + if expected_hz > 0.0: + expected_freq_display = f'{expected_hz:.1f}Hz' + if tolerance > 0.0: + expected_freq_display += f'±{tolerance:.0f}%' + expected_freq_display = expected_freq_display.ljust(12) # Color coding based on status if is_monitored: diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 09ed0f8..3a61630 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -57,6 +57,8 @@ class UiDiagnosticData: """ + expected_frequency: str = '-' + tolerance: str = '-' pub_rate: str = '-' msg_rate: str = '-' latency: str = '-' @@ -85,6 +87,10 @@ def from_status(cls, status: DiagnosticStatus) -> 'UiDiagnosticData': data.msg_rate = kv.value elif kv.key == 'current_delay_from_realtime_ms': data.latency = kv.value + elif kv.key == 'expected_frequency': + data.expected_frequency = kv.value + elif kv.key == 'tolerance': + data.tolerance = kv.value return data @@ -167,6 +173,12 @@ def _on_diagnostics(self, msg: DiagnosticArray): # Normalize the topic name to handle both NITROS and Greenwave formats topic_name = self._extract_topic_name(status.name) self.ui_diagnostics[topic_name] = ui_data + expected_frequency = float(ui_data.expected_frequency) + tolerance = float(ui_data.tolerance) + if expected_frequency > 0 and tolerance >= 0: + self.expected_frequencies[topic_name] = (expected_frequency, tolerance) + else: + self.expected_frequencies.pop(topic_name, None) def toggle_topic_monitoring(self, topic_name: str): """Toggle monitoring for a topic.""" diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index f9a39bb..742e952 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -39,9 +39,22 @@ class GreenwaveMonitor : public rclcpp::Node public: explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); + ~GreenwaveMonitor() + { + // Cancel timers first to stop callbacks from firing + if (timer_) { + timer_->cancel(); + } + if (init_timer_) { + init_timer_->cancel(); + } + // Clear diagnostics before base Node destructor runs to avoid accessing invalid node state + message_diagnostics_.clear(); + subscriptions_.clear(); + } + private: - std::optional find_topic_type_with_retry( - const std::string & topic, const int max_retries, const int retry_period_s); + std::optional find_topic_type(const std::string & topic); void topic_callback( const std::shared_ptr msg, @@ -49,6 +62,8 @@ class GreenwaveMonitor : public rclcpp::Node void timer_callback(); + void deferred_init(); + void handle_manage_topic( const std::shared_ptr request, std::shared_ptr response); @@ -68,10 +83,13 @@ class GreenwaveMonitor : public rclcpp::Node std::shared_ptr serialized_message_ptr, const std::string & type); + void add_topics_from_parameters(); + std::map> message_diagnostics_; std::vector> subscriptions_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr init_timer_; rclcpp::Service::SharedPtr manage_topic_service_; rclcpp::Service::SharedPtr diff --git a/greenwave_monitor/include/message_diagnostics.hpp b/greenwave_monitor/include/message_diagnostics.hpp index 324d37d..e686e6a 100644 --- a/greenwave_monitor/include/message_diagnostics.hpp +++ b/greenwave_monitor/include/message_diagnostics.hpp @@ -232,6 +232,14 @@ class MessageDiagnostics diagnostic_msgs::build() .key("total_dropped_frames") .value(std::to_string(msg_window_.outlier_count))); + values.push_back( + diagnostic_msgs::build() + .key("expected_frequency") + .value(std::to_string(frequency_))); + values.push_back( + diagnostic_msgs::build() + .key("tolerance") + .value(std::to_string(tolerance_))); } status_vec_[0].values = values; @@ -291,11 +299,13 @@ class MessageDiagnostics const int64_t expected_dt_us = static_cast(message_diagnostics::constants::kSecondsToMicroseconds / expected_hz); + frequency_ = expected_hz; diagnostics_config_.expected_dt_us = expected_dt_us; const int tolerance_us = static_cast((message_diagnostics::constants::kSecondsToMicroseconds / expected_hz) * (tolerance_percent / 100.0)); + tolerance_ = tolerance_percent; diagnostics_config_.jitter_tolerance_us = tolerance_us; } @@ -307,6 +317,9 @@ class MessageDiagnostics diagnostics_config_.expected_dt_us = 0; diagnostics_config_.jitter_tolerance_us = 0; + + frequency_ = 0.0; + tolerance_ = 0.0; } private: @@ -491,6 +504,9 @@ class MessageDiagnostics } return error_found; } + + double frequency_{0.0}; + double tolerance_{0.0}; }; } // namespace message_diagnostics diff --git a/greenwave_monitor/include/minimal_publisher_node.hpp b/greenwave_monitor/include/minimal_publisher_node.hpp index fb3a38c..9d1e94d 100644 --- a/greenwave_monitor/include/minimal_publisher_node.hpp +++ b/greenwave_monitor/include/minimal_publisher_node.hpp @@ -35,6 +35,14 @@ class MinimalPublisher : public rclcpp::Node public: explicit MinimalPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~MinimalPublisher() + { + if (timer_) { + timer_->cancel(); + } + message_diagnostics_.reset(); + } + private: void timer_callback(); diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e73fc65..307b615 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -20,37 +20,52 @@ #include #include #include +#include #include #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" using namespace std::chrono_literals; +namespace greenwave_monitor +{ + namespace constants + { + inline constexpr const char * kTopicParamPrefix = "greenwave_diagnostics."; + inline constexpr const char * kExpectedFrequencySuffix = ".expected_frequency"; + inline constexpr const char * kToleranceSuffix = ".tolerance"; + } +} + GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) -: Node("greenwave_monitor", options) +: Node("greenwave_monitor", + rclcpp::NodeOptions(options) + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { RCLCPP_INFO(this->get_logger(), "Starting GreenwaveMonitorNode"); - // Declare and get the topics parameter - this->declare_parameter>("topics", {""}); - auto topics = this->get_parameter("topics").as_string_array(); - - message_diagnostics::MessageDiagnosticsConfig diagnostics_config; - diagnostics_config.enable_all_topic_diagnostics = true; - - auto topic_names_and_types = this->get_topic_names_and_types(); - - for (const auto & topic : topics) { - if (!topic.empty()) { - std::string message; - add_topic(topic, message); - } + if (!this->has_parameter("topics")) { + this->declare_parameter>("topics", {""}); } timer_ = this->create_wall_timer( 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); - // Add service server + // Defer topic discovery to allow the ROS graph to settle before querying other nodes + init_timer_ = this->create_wall_timer( + 100ms, [this]() { + init_timer_->cancel(); + deferred_init(); + }); +} + +void GreenwaveMonitor::deferred_init() +{ + // Get all topics from YAML and parameters + add_topics_from_parameters(); + + // Add service servers after all topics are added to prevent race conditions manage_topic_service_ = this->create_service( "~/manage_topic", @@ -66,20 +81,14 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) std::placeholders::_1, std::placeholders::_2)); } -std::optional GreenwaveMonitor::find_topic_type_with_retry( - const std::string & topic, const int max_retries, const int retry_period_s) +std::optional GreenwaveMonitor::find_topic_type(const std::string & topic) { - for (int i = 0; i < max_retries; ++i) { - auto topic_names_and_types = this->get_topic_names_and_types(); - auto it = topic_names_and_types.find(topic); - if (it == topic_names_and_types.end() || it->second.empty()) { - std::this_thread::sleep_for(std::chrono::seconds(retry_period_s)); - continue; - } else { - return it->second[0]; - } + std::vector publishers; + publishers = this->get_publishers_info_by_topic(topic); + if (publishers.empty()) { + return std::nullopt; } - return std::nullopt; + return publishers[0].topic_type(); } void GreenwaveMonitor::topic_callback( @@ -264,9 +273,7 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); - const int max_retries = 5; - const int retry_period_s = 1; - auto maybe_type = find_topic_type_with_retry(topic, max_retries, retry_period_s); + auto maybe_type = find_topic_type(topic); if (!maybe_type.has_value()) { RCLCPP_ERROR(this->get_logger(), "Failed to find type for topic '%s'", topic.c_str()); message = "Failed to find type for topic"; @@ -348,3 +355,74 @@ GreenwaveMonitor::GetTimestampFromSerializedMessage( std::chrono::seconds(timestamp_sec) + std::chrono::nanoseconds(timestamp_nanosec)); return timestamp; } + +void GreenwaveMonitor::add_topics_from_parameters() +{ + using namespace greenwave_monitor::constants; + + std::set topics; + + // List all parameters with "greenwave_diagnostics." prefix + auto list_result = this->list_parameters({"greenwave_diagnostics"}, 10); + + // Loop over and find all unique topics with "greenwave_diagnostics." prefix + for (const auto & param_name : list_result.names) { + // Parameter names are like "greenwave_diagnostics./my_topic.tolerance" + // We need to extract the topic name (e.g., "/my_topic") + if (param_name.find(kTopicParamPrefix) != 0) { + continue; + } + + // Remove the "greenwave_diagnostics." prefix + std::string remainder = param_name.substr(std::strlen(kTopicParamPrefix)); + + // Find the last '.' to separate topic name from parameter suffix + size_t last_dot = remainder.rfind('.'); + if (last_dot == std::string::npos || last_dot == 0) { + continue; + } + + std::string topic_name = remainder.substr(0, last_dot); + if (!topic_name.empty() && topic_name[0] == '/') { + topics.insert(topic_name); + } + } + + // Add topics from "topics" parameter + auto topics_param = this->get_parameter("topics").as_string_array(); + topics.insert(topics_param.begin(), topics_param.end()); + + // Helper function to get double parameters from the node + auto get_double_param = [this](const std::string & name) -> double { + auto param = this->get_parameter(name); + if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { + return param.as_double(); + } else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { + return static_cast(param.as_int()); + } + return 0.0; + }; + + // For each topic, read parameters and add topic with expected frequency settings + for (const auto & topic : topics) { + std::string freq_param = std::string(kTopicParamPrefix) + topic + kExpectedFrequencySuffix; + std::string tol_param = std::string(kTopicParamPrefix) + topic + kToleranceSuffix; + + double expected_frequency = 0.0; + double tolerance = 0.0; + + if (this->has_parameter(freq_param)) { + expected_frequency = get_double_param(freq_param); + } + if (this->has_parameter(tol_param)) { + tolerance = get_double_param(tol_param); + } + + std::string message; + if (add_topic(topic, message)) { + if (expected_frequency > 0.0) { + message_diagnostics_[topic]->setExpectedDt(expected_frequency, tolerance); + } + } + } +} From 7815ede635dae078be9131e57ed8535689a0a4e3 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 12 Jan 2026 19:57:23 -0800 Subject: [PATCH 02/14] Rename message diagnostics to greenwave diagnostics Signed-off-by: Blake McHale --- greenwave_monitor/CMakeLists.txt | 22 +++--- .../greenwave_monitor/ui_adaptor.py | 2 +- ...gnostics.hpp => greenwave_diagnostics.hpp} | 57 +++++++-------- .../include/greenwave_monitor.hpp | 6 +- .../include/minimal_publisher_node.hpp | 6 +- greenwave_monitor/src/greenwave_monitor.cpp | 30 ++++---- .../src/minimal_publisher_node.cpp | 10 +-- ...ics.cpp => test_greenwave_diagnostics.cpp} | 69 ++++++++++--------- 8 files changed, 102 insertions(+), 100 deletions(-) rename greenwave_monitor/include/{message_diagnostics.hpp => greenwave_diagnostics.hpp} (90%) rename greenwave_monitor/test/{test_message_diagnostics.cpp => test_greenwave_diagnostics.cpp} (77%) diff --git a/greenwave_monitor/CMakeLists.txt b/greenwave_monitor/CMakeLists.txt index bccd4a0..e28c566 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -23,9 +23,9 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# Add message_diagnostics.hpp as a header-only library -add_library(message_diagnostics INTERFACE) -target_include_directories(message_diagnostics INTERFACE +# Add greenwave_diagnostics.hpp as a header-only library +add_library(greenwave_diagnostics INTERFACE) +target_include_directories(greenwave_diagnostics INTERFACE $ $) @@ -36,7 +36,7 @@ ament_target_dependencies(greenwave_monitor diagnostic_msgs greenwave_monitor_interfaces ) -target_link_libraries(greenwave_monitor message_diagnostics) +target_link_libraries(greenwave_monitor greenwave_diagnostics) target_include_directories(greenwave_monitor PUBLIC $ @@ -51,7 +51,7 @@ add_executable(minimal_publisher_node src/minimal_publisher_node.cpp src/minimal_publisher_main.cpp) ament_target_dependencies(minimal_publisher_node rclcpp std_msgs sensor_msgs diagnostic_msgs) -target_link_libraries(minimal_publisher_node message_diagnostics) +target_link_libraries(minimal_publisher_node greenwave_diagnostics) target_include_directories(minimal_publisher_node PUBLIC $ $) @@ -117,21 +117,21 @@ if(BUILD_TESTING) ) # Add gtests - ament_add_gtest(test_message_diagnostics test/test_message_diagnostics.cpp + ament_add_gtest(test_greenwave_diagnostics test/test_greenwave_diagnostics.cpp TIMEOUT 60 WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) - ament_target_dependencies(test_message_diagnostics + ament_target_dependencies(test_greenwave_diagnostics rclcpp std_msgs diagnostic_msgs ) - target_link_libraries(test_message_diagnostics message_diagnostics) - target_include_directories(test_message_diagnostics PUBLIC + target_link_libraries(test_greenwave_diagnostics greenwave_diagnostics) + target_include_directories(test_greenwave_diagnostics PUBLIC $ $ ) - target_compile_features(test_message_diagnostics PUBLIC c_std_99 cxx_std_17) + target_compile_features(test_greenwave_diagnostics PUBLIC c_std_99 cxx_std_17) ament_add_gtest(test_minimal_publisher test/test_minimal_publisher.cpp @@ -144,7 +144,7 @@ if(BUILD_TESTING) sensor_msgs diagnostic_msgs ) - target_link_libraries(test_minimal_publisher message_diagnostics) + target_link_libraries(test_minimal_publisher greenwave_diagnostics) target_include_directories(test_minimal_publisher PUBLIC $ $ diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 3a61630..6171c92 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -148,7 +148,7 @@ def _extract_topic_name(self, diagnostic_name: str) -> str: - NITROS: node_name + namespace + "/" + topic (e.g., "my_node/ns/camera/image") - Greenwave: topic_name only (e.g., "/ns/camera/image") - This is a temporary hack until NITROS migrates to message_diagnostics.hpp. + This is a temporary hack until NITROS migrates to greenwave_diagnostics.hpp. """ # If the name starts with '/', it's already just a topic name (Greenwave format) if diagnostic_name.startswith('/'): diff --git a/greenwave_monitor/include/message_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp similarity index 90% rename from greenwave_monitor/include/message_diagnostics.hpp rename to greenwave_monitor/include/greenwave_diagnostics.hpp index e686e6a..3c88a92 100644 --- a/greenwave_monitor/include/message_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -33,7 +33,7 @@ #include "builtin_interfaces/msg/time.hpp" #include "rclcpp/rclcpp.hpp" -namespace message_diagnostics +namespace greenwave_diagnostics { namespace constants { @@ -48,8 +48,8 @@ inline constexpr int64_t kDropWarnTimeoutSeconds = 5LL; inline constexpr int64_t kNonsenseLatencyMs = 365LL * 24LL * 60LL * 60LL * 1000LL; } // namespace constants -// Configurations for a message diagnostics -struct MessageDiagnosticsConfig +// Configurations for a greenwave diagnostics +struct GreenwaveDiagnosticsConfig { // diagnostics toggle bool enable_diagnostics{false}; @@ -73,13 +73,13 @@ struct MessageDiagnosticsConfig int64_t jitter_tolerance_us{0LL}; }; -class MessageDiagnostics +class GreenwaveDiagnostics { public: - MessageDiagnostics( + GreenwaveDiagnostics( rclcpp::Node & node, const std::string & topic_name, - const MessageDiagnosticsConfig & diagnostics_config) + const GreenwaveDiagnosticsConfig & diagnostics_config) : node_(node), topic_name_(topic_name), diagnostics_config_(diagnostics_config) { clock_ = node_.get_clock(); @@ -113,7 +113,7 @@ class MessageDiagnostics { // Mutex lock to prevent simultaneous access of common parameters // used by updateDiagnostics() and publishDiagnostics() - const std::lock_guard lock(message_diagnostics_mutex_); + const std::lock_guard lock(greenwave_diagnostics_mutex_); // Message diagnostics checks message intervals both using the node clock // and the message timestamp. // All variables name _node refers to the node timestamp checks. @@ -123,9 +123,9 @@ class MessageDiagnostics // Get the current timestamps in microseconds uint64_t current_timestamp_msg_us = - msg_timestamp_ns / message_diagnostics::constants::kMicrosecondsToNanoseconds; + msg_timestamp_ns / greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; uint64_t current_timestamp_node_us = static_cast(clock_->now().nanoseconds() / - message_diagnostics::constants::kMicrosecondsToNanoseconds); + greenwave_diagnostics::constants::kMicrosecondsToNanoseconds); // we can only calculate frame rate after 2 messages have been received if (prev_timestamp_node_us_ != std::numeric_limits::min()) { @@ -138,11 +138,11 @@ class MessageDiagnostics const rclcpp::Time time_from_node = node_.get_clock()->now(); uint64_t ros_node_system_time_us = time_from_node.nanoseconds() / - message_diagnostics::constants::kMicrosecondsToNanoseconds; + greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; const double latency_wrt_current_timestamp_node_ms = static_cast(ros_node_system_time_us - current_timestamp_msg_us) / - message_diagnostics::constants::kMillisecondsToMicroseconds; + greenwave_diagnostics::constants::kMillisecondsToMicroseconds; if (prev_timestamp_msg_us_ != std::numeric_limits::min()) { const int64_t timestamp_diff_msg_us = current_timestamp_msg_us - prev_timestamp_msg_us_; @@ -161,7 +161,7 @@ class MessageDiagnostics // calculate key values for diagnostics status (computed on publish/getters) message_latency_msg_ms_ = latency_wrt_current_timestamp_node_ms; - if (message_latency_msg_ms_ > message_diagnostics::constants::kNonsenseLatencyMs) { + if (message_latency_msg_ms_ > greenwave_diagnostics::constants::kNonsenseLatencyMs) { message_latency_msg_ms_ = std::numeric_limits::quiet_NaN(); } @@ -178,7 +178,7 @@ class MessageDiagnostics { // Mutex lock to prevent simultaneous access of common parameters // used by updateDiagnostics() and publishDiagnostics() - const std::lock_guard lock(message_diagnostics_mutex_); + const std::lock_guard lock(greenwave_diagnostics_mutex_); std::vector values; // publish diagnostics stale if message has not been updated since the last call @@ -246,9 +246,9 @@ class MessageDiagnostics // timestamp from std::chrono (steady clock); split into sec/nanosec correctly const uint64_t elapsed_ns = static_cast((clock_->now() - t_start_).nanoseconds()); const uint32_t time_seconds = static_cast( - elapsed_ns / static_cast(message_diagnostics::constants::kSecondsToNanoseconds)); + elapsed_ns / static_cast(greenwave_diagnostics::constants::kSecondsToNanoseconds)); const uint32_t time_ns = static_cast( - elapsed_ns % static_cast(message_diagnostics::constants::kSecondsToNanoseconds)); + elapsed_ns % static_cast(greenwave_diagnostics::constants::kSecondsToNanoseconds)); diagnostic_msgs::msg::DiagnosticArray diagnostic_msg; std_msgs::msg::Header header; @@ -280,7 +280,7 @@ class MessageDiagnostics void setExpectedDt(double expected_hz, double tolerance_percent) { - const std::lock_guard lock(message_diagnostics_mutex_); + const std::lock_guard lock(greenwave_diagnostics_mutex_); diagnostics_config_.enable_node_time_diagnostics = true; diagnostics_config_.enable_msg_time_diagnostics = true; @@ -298,12 +298,12 @@ class MessageDiagnostics } const int64_t expected_dt_us = - static_cast(message_diagnostics::constants::kSecondsToMicroseconds / expected_hz); + static_cast(greenwave_diagnostics::constants::kSecondsToMicroseconds / expected_hz); frequency_ = expected_hz; diagnostics_config_.expected_dt_us = expected_dt_us; const int tolerance_us = - static_cast((message_diagnostics::constants::kSecondsToMicroseconds / expected_hz) * + static_cast((greenwave_diagnostics::constants::kSecondsToMicroseconds / expected_hz) * (tolerance_percent / 100.0)); tolerance_ = tolerance_percent; diagnostics_config_.jitter_tolerance_us = tolerance_us; @@ -311,7 +311,7 @@ class MessageDiagnostics void clearExpectedDt() { - const std::lock_guard lock(message_diagnostics_mutex_); + const std::lock_guard lock(greenwave_diagnostics_mutex_); diagnostics_config_.enable_node_time_diagnostics = false; diagnostics_config_.enable_msg_time_diagnostics = false; @@ -365,7 +365,7 @@ class MessageDiagnostics if (sum_interarrival_us == 0 || interarrival_us.empty()) { return 0.0; } - return static_cast(message_diagnostics::constants::kSecondsToMicroseconds) / + return static_cast(greenwave_diagnostics::constants::kSecondsToMicroseconds) / (static_cast(sum_interarrival_us) / static_cast(interarrival_us.size())); } @@ -380,11 +380,11 @@ class MessageDiagnostics }; // Mutex lock to prevent simultaneous access of common parameters // used by updateDiagnostics() and publishDiagnostics() - std::mutex message_diagnostics_mutex_; + std::mutex greenwave_diagnostics_mutex_; rclcpp::Node & node_; std::string topic_name_; - MessageDiagnosticsConfig diagnostics_config_; + GreenwaveDiagnosticsConfig diagnostics_config_; std::vector status_vec_; rclcpp::Clock::SharedPtr clock_; rclcpp::Time t_start_; @@ -425,7 +425,7 @@ class MessageDiagnostics if (missed_deadline_node) { RCLCPP_DEBUG( node_.get_logger(), - "[MessageDiagnostics Node Time]" + "[GreenwaveDiagnostics Node Time]" " Difference of time between messages(%" PRId64 ") and expected time between" " messages(%" PRId64 ") is out of tolerance(%" PRId64 ") by %" PRId64 " for topic %s." " Units are microseconds.", @@ -455,7 +455,7 @@ class MessageDiagnostics prev_drop_ts_ = clock_->now(); RCLCPP_DEBUG( node_.get_logger(), - "[MessageDiagnostics Message Timestamp]" + "[GreenwaveDiagnostics Message Timestamp]" " Difference of time between messages(%" PRId64 ") and expected " "time between" " messages(%" PRId64 ") is out of tolerance(%" PRId64 ") by %" PRId64 " for topic %s. " @@ -468,7 +468,7 @@ class MessageDiagnostics if (prev_drop_ts_.nanoseconds() != 0) { auto time_since_drop = (clock_->now() - prev_drop_ts_).seconds(); - if (time_since_drop < message_diagnostics::constants::kDropWarnTimeoutSeconds) { + if (time_since_drop < greenwave_diagnostics::constants::kDropWarnTimeoutSeconds) { error_found = true; status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; deadlines_missed_since_last_pub_ = 0; @@ -488,14 +488,14 @@ class MessageDiagnostics prev_noninc_msg_ts_ = clock_->now(); RCLCPP_WARN( node_.get_logger(), - "[MessageDiagnostics Message Timestamp Non Increasing]" + "[GreenwaveDiagnostics Message Timestamp Non Increasing]" " Message timestamp is not increasing. Current timestamp: %" PRIu64 ", " " Previous timestamp: %" PRIu64 " for topic %s. Units are microseconds.", current_timestamp_msg_us, prev_timestamp_msg_us_, topic_name_.c_str()); } if (prev_noninc_msg_ts_.nanoseconds() != 0) { auto time_since_noninc = (clock_->now() - prev_noninc_msg_ts_).seconds(); - if (time_since_noninc < message_diagnostics::constants::kDropWarnTimeoutSeconds) { + if (time_since_noninc < greenwave_diagnostics::constants::kDropWarnTimeoutSeconds) { error_found = true; status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; deadlines_missed_since_last_pub_ = 0; @@ -509,4 +509,5 @@ class MessageDiagnostics double tolerance_{0.0}; }; -} // namespace message_diagnostics +} // namespace greenwave_diagnostics + diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 742e952..c45096e 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -30,7 +30,7 @@ #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" -#include "message_diagnostics.hpp" +#include "greenwave_diagnostics.hpp" #include "greenwave_monitor_interfaces/srv/manage_topic.hpp" #include "greenwave_monitor_interfaces/srv/set_expected_frequency.hpp" @@ -49,7 +49,7 @@ class GreenwaveMonitor : public rclcpp::Node init_timer_->cancel(); } // Clear diagnostics before base Node destructor runs to avoid accessing invalid node state - message_diagnostics_.clear(); + greenwave_diagnostics_.clear(); subscriptions_.clear(); } @@ -86,7 +86,7 @@ class GreenwaveMonitor : public rclcpp::Node void add_topics_from_parameters(); std::map> message_diagnostics_; + std::unique_ptr> greenwave_diagnostics_; std::vector> subscriptions_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr init_timer_; diff --git a/greenwave_monitor/include/minimal_publisher_node.hpp b/greenwave_monitor/include/minimal_publisher_node.hpp index 9d1e94d..f1fcbb3 100644 --- a/greenwave_monitor/include/minimal_publisher_node.hpp +++ b/greenwave_monitor/include/minimal_publisher_node.hpp @@ -25,7 +25,7 @@ #include "sensor_msgs/msg/image.hpp" #include "sensor_msgs/msg/imu.hpp" #include "std_msgs/msg/string.hpp" -#include "message_diagnostics.hpp" +#include "greenwave_diagnostics.hpp" #include "rclcpp/subscription_options.hpp" using std::chrono_literals::operator""ms; @@ -40,7 +40,7 @@ class MinimalPublisher : public rclcpp::Node if (timer_) { timer_->cancel(); } - message_diagnostics_.reset(); + greenwave_diagnostics_.reset(); } private: @@ -49,7 +49,7 @@ class MinimalPublisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; rclcpp::PublisherBase::SharedPtr publisher_; rclcpp::SubscriptionBase::SharedPtr subscription_; - std::unique_ptr message_diagnostics_; + std::unique_ptr greenwave_diagnostics_; size_t count_; std::string message_type_; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 307b615..f08fd39 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -96,16 +96,16 @@ void GreenwaveMonitor::topic_callback( const std::string & topic, const std::string & type) { auto msg_timestamp = GetTimestampFromSerializedMessage(msg, type); - message_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); + greenwave_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); } void GreenwaveMonitor::timer_callback() { RCLCPP_INFO(this->get_logger(), "===================================================="); - if (message_diagnostics_.empty()) { + if (greenwave_diagnostics_.empty()) { RCLCPP_INFO(this->get_logger(), "No topics to monitor"); } - for (auto & [topic, diagnostics] : message_diagnostics_) { + for (auto & [topic, diagnostics] : greenwave_diagnostics_) { diagnostics->publishDiagnostics(); RCLCPP_INFO( this->get_logger(), "Frame rate for topic %s: %.2f hz", @@ -132,9 +132,9 @@ void GreenwaveMonitor::handle_set_expected_frequency( const std::shared_ptr request, std::shared_ptr response) { - auto it = message_diagnostics_.find(request->topic_name); + auto it = greenwave_diagnostics_.find(request->topic_name); - if (it == message_diagnostics_.end()) { + if (it == greenwave_diagnostics_.end()) { if (!request->add_topic_if_missing) { response->success = false; response->message = "Failed to find topic"; @@ -145,10 +145,10 @@ void GreenwaveMonitor::handle_set_expected_frequency( response->success = false; return; } - it = message_diagnostics_.find(request->topic_name); + it = greenwave_diagnostics_.find(request->topic_name); } - message_diagnostics::MessageDiagnostics & msg_diagnostics_obj = *(it->second); + greenwave_diagnostics::GreenwaveDiagnostics & msg_diagnostics_obj = *(it->second); if (request->clear_expected) { msg_diagnostics_obj.clearExpectedDt(); @@ -266,7 +266,7 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & message) { // Check if topic already exists - if (message_diagnostics_.find(topic) != message_diagnostics_.end()) { + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { message = "Topic already being monitored"; return false; } @@ -290,13 +290,13 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag this->topic_callback(msg, topic, type); }); - message_diagnostics::MessageDiagnosticsConfig diagnostics_config; + greenwave_diagnostics::GreenwaveDiagnosticsConfig diagnostics_config; diagnostics_config.enable_all_topic_diagnostics = true; subscriptions_.push_back(sub); - message_diagnostics_.emplace( + greenwave_diagnostics_.emplace( topic, - std::make_unique(*this, topic, diagnostics_config)); + std::make_unique(*this, topic, diagnostics_config)); message = "Successfully added topic"; return true; @@ -304,8 +304,8 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & message) { - auto diag_it = message_diagnostics_.find(topic); - if (diag_it == message_diagnostics_.end()) { + auto diag_it = greenwave_diagnostics_.find(topic); + if (diag_it == greenwave_diagnostics_.end()) { message = "Topic not found"; return false; } @@ -321,7 +321,7 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes subscriptions_.erase(sub_it); } - message_diagnostics_.erase(diag_it); + greenwave_diagnostics_.erase(diag_it); message = "Successfully removed topic"; return true; } @@ -421,7 +421,7 @@ void GreenwaveMonitor::add_topics_from_parameters() std::string message; if (add_topic(topic, message)) { if (expected_frequency > 0.0) { - message_diagnostics_[topic]->setExpectedDt(expected_frequency, tolerance); + greenwave_diagnostics_[topic]->setExpectedDt(expected_frequency, tolerance); } } } diff --git a/greenwave_monitor/src/minimal_publisher_node.cpp b/greenwave_monitor/src/minimal_publisher_node.cpp index e7de807..0258b06 100644 --- a/greenwave_monitor/src/minimal_publisher_node.cpp +++ b/greenwave_monitor/src/minimal_publisher_node.cpp @@ -29,7 +29,7 @@ MinimalPublisher::MinimalPublisher(const rclcpp::NodeOptions & options) const auto topic = this->get_parameter("topic").as_string(); const auto frequency_hz = this->get_parameter("frequency_hz").as_double(); const auto period_ns = static_cast( - ::message_diagnostics::constants::kSecondsToNanoseconds / frequency_hz); + ::greenwave_diagnostics::constants::kSecondsToNanoseconds / frequency_hz); const auto create_subscriber = this->get_parameter("create_subscriber").as_bool(); message_type_ = this->get_parameter("message_type").as_string(); @@ -82,9 +82,9 @@ MinimalPublisher::MinimalPublisher(const rclcpp::NodeOptions & options) timer_ = this->create_wall_timer( std::chrono::nanoseconds(period_ns), std::bind(&MinimalPublisher::timer_callback, this)); - message_diagnostics::MessageDiagnosticsConfig diagnostics_config; + greenwave_diagnostics::GreenwaveDiagnosticsConfig diagnostics_config; diagnostics_config.enable_all_topic_diagnostics = true; - message_diagnostics_ = std::make_unique( + greenwave_diagnostics_ = std::make_unique( *this, topic, diagnostics_config); } @@ -138,6 +138,6 @@ void MinimalPublisher::timer_callback() } const auto msg_timestamp = this->now(); - message_diagnostics_->updateDiagnostics(msg_timestamp.nanoseconds()); - // message_diagnostics_->publishDiagnostics(); + greenwave_diagnostics_->updateDiagnostics(msg_timestamp.nanoseconds()); + // greenwave_diagnostics_->publishDiagnostics(); } diff --git a/greenwave_monitor/test/test_message_diagnostics.cpp b/greenwave_monitor/test/test_greenwave_diagnostics.cpp similarity index 77% rename from greenwave_monitor/test/test_message_diagnostics.cpp rename to greenwave_monitor/test/test_greenwave_diagnostics.cpp index df3e0dc..efddf65 100644 --- a/greenwave_monitor/test/test_message_diagnostics.cpp +++ b/greenwave_monitor/test/test_greenwave_diagnostics.cpp @@ -16,7 +16,7 @@ // SPDX-License-Identifier: Apache-2.0 /** -Unit tests for functionality in message_diagnostics.hpp, +Unit tests for functionality in greenwave_diagnostics.hpp, such as frame rate and latency calculation accuracy. **/ @@ -28,7 +28,7 @@ such as frame rate and latency calculation accuracy. #include #include -#include "message_diagnostics.hpp" +#include "greenwave_diagnostics.hpp" namespace test_constants { @@ -36,7 +36,7 @@ inline constexpr uint64_t kMillisecondsToSeconds = 1000ULL; inline constexpr uint64_t kStartTimestampNs = 10000000ULL; } // namespace test_constants -class MessageDiagnosticsTest : public ::testing::Test +class GreenwaveDiagnosticsTest : public ::testing::Test { protected: static void SetUpTestSuite() @@ -62,25 +62,25 @@ class MessageDiagnosticsTest : public ::testing::Test std::shared_ptr node_; }; -TEST_F(MessageDiagnosticsTest, FrameRateMsgTest) +TEST_F(GreenwaveDiagnosticsTest, FrameRateMsgTest) { - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); uint64_t timestamp = test_constants::kStartTimestampNs; // in nanoseconds for (int i = 0; i < 1000; i++) { - message_diagnostics.updateDiagnostics(timestamp); + greenwave_diagnostics.updateDiagnostics(timestamp); timestamp += 10000000; // 10 ms in nanoseconds } - EXPECT_EQ(message_diagnostics.getFrameRateMsg(), 100); // 100 Hz + EXPECT_EQ(greenwave_diagnostics.getFrameRateMsg(), 100); // 100 Hz } -TEST_F(MessageDiagnosticsTest, FrameRateNodeTest) +TEST_F(GreenwaveDiagnosticsTest, FrameRateNodeTest) { - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); // dummy timestamp, not used for node time calculation constexpr auto timestamp = test_constants::kStartTimestampNs; @@ -90,7 +90,7 @@ TEST_F(MessageDiagnosticsTest, FrameRateNodeTest) constexpr int interarrival_time_ms = 10; // 100 hz for (int i = 0; i < num_messages; i++) { - message_diagnostics.updateDiagnostics(timestamp); + greenwave_diagnostics.updateDiagnostics(timestamp); std::this_thread::sleep_for(std::chrono::milliseconds(interarrival_time_ms)); } @@ -100,14 +100,14 @@ TEST_F(MessageDiagnosticsTest, FrameRateNodeTest) const double expected_frame_rate = static_cast(num_messages) / total_duration.count(); // allow 2.0 Hz error - EXPECT_NEAR(message_diagnostics.getFrameRateNode(), expected_frame_rate, 2.0); + EXPECT_NEAR(greenwave_diagnostics.getFrameRateNode(), expected_frame_rate, 2.0); } -TEST_F(MessageDiagnosticsTest, MessageLatencyTest) +TEST_F(GreenwaveDiagnosticsTest, MessageLatencyTest) { - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); const rclcpp::Time current_time = node_->get_clock()->now(); // Make message timestamp a certain amount of time earlier than current time @@ -116,28 +116,28 @@ TEST_F(MessageDiagnosticsTest, MessageLatencyTest) current_time - rclcpp::Duration::from_seconds( expected_latency_ms / static_cast(test_constants::kMillisecondsToSeconds)); - message_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); + greenwave_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); - EXPECT_NEAR(message_diagnostics.getLatency(), expected_latency_ms, 1.0); // allow 1 ms tolerance + EXPECT_NEAR(greenwave_diagnostics.getLatency(), expected_latency_ms, 1.0); // allow 1 ms tolerance } -TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) +TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) { constexpr int input_frequency = 50; // 50 Hz // 20 ms in nanoseconds const int64_t interarrival_time_ns = static_cast( - ::message_diagnostics::constants::kSecondsToNanoseconds / input_frequency); + ::greenwave_diagnostics::constants::kSecondsToNanoseconds / input_frequency); - // Initialize MessageDiagnostics with diagnostics enabled - message_diagnostics::MessageDiagnosticsConfig config; + // Initialize GreenwaveDiagnostics with diagnostics enabled + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.enable_msg_time_diagnostics = true; config.enable_node_time_diagnostics = true; config.enable_increasing_msg_time_diagnostics = true; // in us config.expected_dt_us = interarrival_time_ns / - ::message_diagnostics::constants::kMicrosecondsToNanoseconds; + ::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; - message_diagnostics::MessageDiagnostics message_diagnostics(*node_, "test_topic", config); + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics(*node_, "test_topic", config); // Create a subscriber to receive diagnostic messages std::vector received_diagnostics; @@ -150,8 +150,8 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) // 50 ms delay constexpr int64_t delay_time_ns = 50 * - static_cast(::message_diagnostics::constants::kMillisecondsToMicroseconds) * - static_cast(::message_diagnostics::constants::kMicrosecondsToNanoseconds); + static_cast(::greenwave_diagnostics::constants::kMillisecondsToMicroseconds) * + static_cast(::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds); // Starting message timestamp in nanoseconds auto msg_timestamp = test_constants::kStartTimestampNs; @@ -167,8 +167,8 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) sent_count++; - message_diagnostics.updateDiagnostics(msg_timestamp); - message_diagnostics.publishDiagnostics(); + greenwave_diagnostics.updateDiagnostics(msg_timestamp); + greenwave_diagnostics.publishDiagnostics(); // Add a non-increasing timestamp at count 5 if (sent_count == 5) { @@ -196,7 +196,7 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) const auto sum_interarrival_time_msg_sec = static_cast( msg_timestamp - test_constants::kStartTimestampNs) / - static_cast(::message_diagnostics::constants::kSecondsToNanoseconds); + static_cast(::greenwave_diagnostics::constants::kSecondsToNanoseconds); const double expected_frame_rate_msg = static_cast(interarrival_time_count) / sum_interarrival_time_msg_sec; @@ -234,11 +234,11 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) // Sometimes diagnostics may arrive out of order, so we use getter methods instead of values from // the last diagnostic message to prevent flakiness - EXPECT_NEAR(message_diagnostics.getFrameRateNode(), expected_frame_rate_node, 1.0); + EXPECT_NEAR(greenwave_diagnostics.getFrameRateNode(), expected_frame_rate_node, 1.0); // Allow small floating point differences for frame rate msg constexpr double frame_rate_msg_tolerance = 0.001; EXPECT_NEAR( - message_diagnostics.getFrameRateMsg(), expected_frame_rate_msg, frame_rate_msg_tolerance); + greenwave_diagnostics.getFrameRateMsg(), expected_frame_rate_msg, frame_rate_msg_tolerance); // Sometimes diagnostics may arrive out of order, so we need to check all received diagnostics // to see if the expected msg frame rate is somewhere in there @@ -275,3 +275,4 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) EXPECT_GE(diagnostics_values["total_dropped_frames"], 1.0); EXPECT_GE(diagnostics_values["num_non_increasing_msg"], 1.0); } + From 63f6661a62a50944f815e5a05c3ff70745601b3f Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 12 Jan 2026 20:09:49 -0800 Subject: [PATCH 03/14] Fix linting Signed-off-by: Blake McHale --- .../include/greenwave_diagnostics.hpp | 1 - greenwave_monitor/src/greenwave_monitor.cpp | 43 ++++++++++--------- .../test/test_greenwave_diagnostics.cpp | 4 +- 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index 3c88a92..674d268 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -510,4 +510,3 @@ class GreenwaveDiagnostics }; } // namespace greenwave_diagnostics - diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index f08fd39..ad4c05c 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -29,19 +29,19 @@ using namespace std::chrono_literals; namespace greenwave_monitor { - namespace constants - { - inline constexpr const char * kTopicParamPrefix = "greenwave_diagnostics."; - inline constexpr const char * kExpectedFrequencySuffix = ".expected_frequency"; - inline constexpr const char * kToleranceSuffix = ".tolerance"; - } -} +namespace constants +{ +inline constexpr const char * kTopicParamPrefix = "greenwave_diagnostics."; +inline constexpr const char * kExpectedFrequencySuffix = ".expected_frequency"; +inline constexpr const char * kToleranceSuffix = ".tolerance"; +} // namespace constants +} // namespace greenwave_monitor GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) : Node("greenwave_monitor", - rclcpp::NodeOptions(options) - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)) + rclcpp::NodeOptions(options) + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { RCLCPP_INFO(this->get_logger(), "Starting GreenwaveMonitorNode"); @@ -296,7 +296,8 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag subscriptions_.push_back(sub); greenwave_diagnostics_.emplace( topic, - std::make_unique(*this, topic, diagnostics_config)); + std::make_unique( + *this, topic, diagnostics_config)); message = "Successfully added topic"; return true; @@ -358,7 +359,9 @@ GreenwaveMonitor::GetTimestampFromSerializedMessage( void GreenwaveMonitor::add_topics_from_parameters() { - using namespace greenwave_monitor::constants; + using greenwave_monitor::constants::kTopicParamPrefix; + using greenwave_monitor::constants::kExpectedFrequencySuffix; + using greenwave_monitor::constants::kToleranceSuffix; std::set topics; @@ -394,14 +397,14 @@ void GreenwaveMonitor::add_topics_from_parameters() // Helper function to get double parameters from the node auto get_double_param = [this](const std::string & name) -> double { - auto param = this->get_parameter(name); - if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { - return param.as_double(); - } else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { - return static_cast(param.as_int()); - } - return 0.0; - }; + auto param = this->get_parameter(name); + if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { + return param.as_double(); + } else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { + return static_cast(param.as_int()); + } + return 0.0; + }; // For each topic, read parameters and add topic with expected frequency settings for (const auto & topic : topics) { diff --git a/greenwave_monitor/test/test_greenwave_diagnostics.cpp b/greenwave_monitor/test/test_greenwave_diagnostics.cpp index efddf65..6b158ae 100644 --- a/greenwave_monitor/test/test_greenwave_diagnostics.cpp +++ b/greenwave_monitor/test/test_greenwave_diagnostics.cpp @@ -118,7 +118,8 @@ TEST_F(GreenwaveDiagnosticsTest, MessageLatencyTest) greenwave_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); - EXPECT_NEAR(greenwave_diagnostics.getLatency(), expected_latency_ms, 1.0); // allow 1 ms tolerance + // allow 1 ms tolerance + EXPECT_NEAR(greenwave_diagnostics.getLatency(), expected_latency_ms, 1.0); } TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) @@ -275,4 +276,3 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) EXPECT_GE(diagnostics_values["total_dropped_frames"], 1.0); EXPECT_GE(diagnostics_values["num_non_increasing_msg"], 1.0); } - From 8e5e61cac144e6acbca07938fa70d09b04c55644 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 12 Jan 2026 21:45:44 -0800 Subject: [PATCH 04/14] Add tests Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 12 ++- .../test/test_greenwave_monitor.py | 90 ++++++++++++++++++- .../test/test_topic_monitoring_integration.py | 69 +++++++++++++- 3 files changed, 159 insertions(+), 12 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 7434b1d..df4d3df 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -19,7 +19,7 @@ import math import time -from typing import List, Optional, Tuple +from typing import Any, List, Optional, Tuple from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency @@ -65,19 +65,17 @@ def create_minimal_publisher( def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE, node_name: str = MONITOR_NODE_NAME, - topics: List[str] = None): + parameters: List[dict[str, Any]] = None): """Create a greenwave_monitor node for testing.""" - if topics is None: - topics = ['/test_topic'] + if parameters is None: + parameters = [{'topics': ['/test_topic']}] return launch_ros.actions.Node( package='greenwave_monitor', executable='greenwave_monitor', name=node_name, namespace=namespace, - parameters=[{ - 'topics': topics - }], + parameters=parameters, output='screen' ) diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 2c49e2d..9b2a9b0 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -18,6 +18,7 @@ # SPDX-License-Identifier: Apache-2.0 +import tempfile import time import unittest @@ -44,15 +45,40 @@ from rclpy.node import Node +def create_test_yaml_config(): + """Create a temporary YAML config file for testing parameter loading.""" + # Use /** wildcard to match any namespace/node name + yaml_content = """\ +/**: + ros__parameters: + greenwave_diagnostics: + /test_topic_valid_expected_frequency: + expected_frequency: 100.0 + tolerance: 10.0 + /test_topic_invalid_expected_frequency: + expected_frequency: 0.0 + tolerance: 0.0 +""" + # Create temp file that persists until explicitly deleted + temp_file = tempfile.NamedTemporaryFile( + mode='w', suffix='.yaml', delete=False) + temp_file.write(yaml_content) + temp_file.flush() + return temp_file.name + + @pytest.mark.launch_test @launch_testing.parametrize('message_type, expected_frequency, tolerance_hz', TEST_CONFIGURATIONS) def generate_test_description(message_type, expected_frequency, tolerance_hz): """Generate launch description for greenwave monitor tests.""" + # Create temporary YAML config for testing parameter loading + yaml_config_file = create_test_yaml_config() + # Launch the greenwave_monitor ros2_monitor_node = create_monitor_node( namespace=MONITOR_NODE_NAMESPACE, node_name=MONITOR_NODE_NAME, - topics=['/test_topic'] + parameters=[yaml_config_file, {'topics': ['/test_topic']}] ) # Create publishers @@ -61,7 +87,14 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): create_minimal_publisher('/test_topic', expected_frequency, message_type), # Additional publishers for topic management tests create_minimal_publisher('/test_topic1', expected_frequency, message_type, '1'), - create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2') + create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2'), + # Publisher for YAML configuration tests + create_minimal_publisher( + '/test_topic_valid_expected_frequency', + expected_frequency, message_type, '_valid_expected_frequency'), + create_minimal_publisher( + '/test_topic_invalid_expected_frequency', + expected_frequency, message_type, '_invalid_expected_frequency') ] context = { @@ -248,6 +281,59 @@ def test_manage_multiple_topics(self, expected_frequency, message_type, toleranc add=False, topic=TEST_TOPIC1, service_client=service_client) self.assertTrue(response.success) + def test_yaml_parameter_loading(self, expected_frequency, message_type, tolerance_hz): + """Test that topics defined in YAML config are automatically monitored.""" + if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: + self.skipTest('Only running YAML parameter tests once') + + self.check_node_launches_successfully() + + # Topic defined in test_config.yaml with valid expected_frequency + YAML_TOPIC = '/test_topic_valid_expected_frequency' + + # Collect diagnostics - topic should already be monitored from YAML + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, YAML_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 1, + f'Topic {YAML_TOPIC} from YAML should be auto-monitored' + ) + + # Verify the expected_frequency from YAML (100.0) is applied + best_status, _ = find_best_diagnostic( + received_diagnostics, 100.0, message_type + ) + self.assertIsNotNone(best_status, f'Did not find diagnostics for {YAML_TOPIC}') + + # Extract values from diagnostic status + diag_values = {kv.key: kv.value for kv in best_status.values} + + # Check that expected_frequency was set from YAML + self.assertIn('expected_frequency', diag_values) + self.assertAlmostEqual( + float(diag_values['expected_frequency']), 100.0, places=1, + msg='Expected frequency from YAML should be 100.0' + ) + + # Verify the tolerance from YAML (10.0) is applied + self.assertIn('tolerance', diag_values) + self.assertAlmostEqual( + float(diag_values['tolerance']), 10.0, places=1, + msg='Tolerance from YAML should be 10.0' + ) + + # Verify that the invalid frequency topic is not monitored + INVALID_TOPIC = '/test_topic_invalid_expected_frequency' + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, INVALID_TOPIC, expected_count=0, timeout_sec=10.0 + ) + self.assertEqual( + len(received_diagnostics), 0, + f'Topic {INVALID_TOPIC} with invalid expected frequency ' + 'should not be monitored') + if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index 2864f97..7652d95 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -17,6 +17,7 @@ # # SPDX-License-Identifier: Apache-2.0 +import tempfile import threading import time import unittest @@ -41,14 +42,36 @@ from rclpy.node import Node +def create_test_yaml_config(): + """Create a temporary YAML config file for testing parameter loading.""" + # Use /** wildcard to match any namespace/node name + yaml_content = """\ +/**: + ros__parameters: + greenwave_diagnostics: + /yaml_config_topic: + expected_frequency: 75.0 + tolerance: 15.0 +""" + # Create temp file that persists until explicitly deleted + temp_file = tempfile.NamedTemporaryFile( + mode='w', suffix='.yaml', delete=False) + temp_file.write(yaml_content) + temp_file.flush() + return temp_file.name + + @pytest.mark.launch_test @launch_testing.parametrize('message_type, expected_frequency, tolerance_hz', TEST_CONFIGURATIONS) def generate_test_description(message_type, expected_frequency, tolerance_hz): """Generate launch description for topic monitoring tests.""" - # Launch the greenwave_monitor + # Create temporary YAML config for testing parameter loading + yaml_config_file = create_test_yaml_config() + + # Launch the greenwave_monitor with YAML config ros2_monitor_node = create_monitor_node( node_name=MONITOR_NODE_NAME, - topics=['/test_topic'] + parameters=[yaml_config_file, {'topics': ['/test_topic']}] ) # Create publishers for testing @@ -59,7 +82,9 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): create_minimal_publisher('/test_topic1', expected_frequency, message_type, '1'), create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2'), # Publisher for service discovery tests - create_minimal_publisher('/discovery_test_topic', 50.0, 'imu', '_discovery') + create_minimal_publisher('/discovery_test_topic', 50.0, 'imu', '_discovery'), + # Publisher for YAML config test (75 Hz as defined in YAML) + create_minimal_publisher('/yaml_config_topic', 75.0, 'imu', '_yaml_config') ] context = { @@ -364,6 +389,44 @@ def test_diagnostics_callback_processing(self, expected_frequency, message_type, # Check that timestamp was updated recently self.assertGreater(topic_data.last_update, time.time() - 10.0) + def test_yaml_sets_parameters_at_startup( + self, expected_frequency, message_type, tolerance_hz): + """Test that YAML config sets expected frequency and tolerance at startup.""" + if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: + self.skipTest('Only running YAML config tests once') + + yaml_topic = '/yaml_config_topic' + + # Spin to receive diagnostics from the YAML-configured topic + max_wait_time = 10.0 + start_time = time.time() + topic_data = None + + while time.time() - start_time < max_wait_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + topic_data = self.diagnostics_monitor.get_topic_diagnostics(yaml_topic) + if topic_data.status != '-': + break + time.sleep(0.1) + + # Topic should be auto-monitored from YAML config + self.assertIsNotNone(topic_data) + self.assertNotEqual( + topic_data.status, '-', + f'Topic {yaml_topic} from YAML should be auto-monitored') + + # Verify expected_frequency from YAML (75.0) is applied + self.assertNotEqual(topic_data.expected_frequency, '-') + self.assertAlmostEqual( + float(topic_data.expected_frequency), 75.0, places=1, + msg='Expected frequency from YAML should be 75.0') + + # Verify tolerance from YAML (15.0) is applied + self.assertNotEqual(topic_data.tolerance, '-') + self.assertAlmostEqual( + float(topic_data.tolerance), 15.0, places=1, + msg='Tolerance from YAML should be 15.0') + def test_service_timeout_handling(self, expected_frequency, message_type, tolerance_hz): """Test service call timeout handling.""" if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: From 6368e9abe86a730528f3d246848e857b2a23272a Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 12 Jan 2026 21:58:43 -0800 Subject: [PATCH 05/14] More tests Signed-off-by: Blake McHale --- .../test/test_greenwave_monitor.py | 35 +++++++++++++ .../test/test_topic_monitoring_integration.py | 50 ++++++++++++++++++- 2 files changed, 84 insertions(+), 1 deletion(-) diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 9b2a9b0..bee58c0 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -58,6 +58,9 @@ def create_test_yaml_config(): /test_topic_invalid_expected_frequency: expected_frequency: 0.0 tolerance: 0.0 + /test_topic_integer_params: + expected_frequency: 50 + tolerance: 5 """ # Create temp file that persists until explicitly deleted temp_file = tempfile.NamedTemporaryFile( @@ -92,6 +95,9 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): create_minimal_publisher( '/test_topic_valid_expected_frequency', expected_frequency, message_type, '_valid_expected_frequency'), + create_minimal_publisher( + '/test_topic_integer_params', + 50.0, message_type, '_integer_params'), create_minimal_publisher( '/test_topic_invalid_expected_frequency', expected_frequency, message_type, '_invalid_expected_frequency') @@ -324,6 +330,35 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc msg='Tolerance from YAML should be 10.0' ) + # Verify integer parameters are handled correctly + INT_TOPIC = '/test_topic_integer_params' + int_diagnostics = collect_diagnostics_for_topic( + self.test_node, INT_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(int_diagnostics), 1, + f'Topic {INT_TOPIC} with integer params should be monitored' + ) + + int_status, _ = find_best_diagnostic(int_diagnostics, 50.0, message_type) + self.assertIsNotNone(int_status, f'Did not find diagnostics for {INT_TOPIC}') + + int_values = {kv.key: kv.value for kv in int_status.values} + + # Check integer expected_frequency (50) is properly converted + self.assertIn('expected_frequency', int_values) + self.assertAlmostEqual( + float(int_values['expected_frequency']), 50.0, places=1, + msg='Integer expected frequency from YAML should be 50' + ) + + # Check integer tolerance (5) is properly converted + self.assertIn('tolerance', int_values) + self.assertAlmostEqual( + float(int_values['tolerance']), 5.0, places=1, + msg='Integer tolerance from YAML should be 5' + ) + # Verify that the invalid frequency topic is not monitored INVALID_TOPIC = '/test_topic_invalid_expected_frequency' received_diagnostics = collect_diagnostics_for_topic( diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index 7652d95..ae9ea0d 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -52,6 +52,12 @@ def create_test_yaml_config(): /yaml_config_topic: expected_frequency: 75.0 tolerance: 15.0 + /yaml_invalid_topic: + expected_frequency: 0.0 + tolerance: 0.0 + /yaml_integer_topic: + expected_frequency: 60 + tolerance: 12 """ # Create temp file that persists until explicitly deleted temp_file = tempfile.NamedTemporaryFile( @@ -84,7 +90,11 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): # Publisher for service discovery tests create_minimal_publisher('/discovery_test_topic', 50.0, 'imu', '_discovery'), # Publisher for YAML config test (75 Hz as defined in YAML) - create_minimal_publisher('/yaml_config_topic', 75.0, 'imu', '_yaml_config') + create_minimal_publisher('/yaml_config_topic', 75.0, 'imu', '_yaml_config'), + # Publisher for invalid YAML config test (0 Hz - should not be monitored) + create_minimal_publisher('/yaml_invalid_topic', 50.0, 'imu', '_yaml_invalid'), + # Publisher for integer YAML config test (60 Hz as integer in YAML) + create_minimal_publisher('/yaml_integer_topic', 60.0, 'imu', '_yaml_integer') ] context = { @@ -427,6 +437,44 @@ def test_yaml_sets_parameters_at_startup( float(topic_data.tolerance), 15.0, places=1, msg='Tolerance from YAML should be 15.0') + # Verify that the invalid topic (0.0 expected_frequency) is not monitored + invalid_topic = '/yaml_invalid_topic' + invalid_data = self.diagnostics_monitor.get_topic_diagnostics(invalid_topic) + self.assertEqual( + invalid_data.status, '-', + f'Topic {invalid_topic} with invalid expected frequency ' + 'should not be monitored') + + # Verify integer parameters are handled correctly + int_topic = '/yaml_integer_topic' + int_data = None + start_time = time.time() + + while time.time() - start_time < max_wait_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + int_data = self.diagnostics_monitor.get_topic_diagnostics(int_topic) + if int_data.status != '-': + break + time.sleep(0.1) + + # Topic with integer params should be monitored + self.assertIsNotNone(int_data) + self.assertNotEqual( + int_data.status, '-', + f'Topic {int_topic} with integer params should be monitored') + + # Check integer expected_frequency (60) is properly converted + self.assertNotEqual(int_data.expected_frequency, '-') + self.assertAlmostEqual( + float(int_data.expected_frequency), 60.0, places=1, + msg='Integer expected frequency from YAML should be 60') + + # Check integer tolerance (12) is properly converted + self.assertNotEqual(int_data.tolerance, '-') + self.assertAlmostEqual( + float(int_data.tolerance), 12.0, places=1, + msg='Integer tolerance from YAML should be 12') + def test_service_timeout_handling(self, expected_frequency, message_type, tolerance_hz): """Test service call timeout handling.""" if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: From c97ffc74e7186505d502b1c5a461c25227881bf9 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 12 Jan 2026 22:52:25 -0800 Subject: [PATCH 06/14] More tests and lint Signed-off-by: Blake McHale --- greenwave_monitor/config/example.yaml | 2 +- .../greenwave_monitor/ncurses_frontend.py | 5 +- .../greenwave_monitor/ui_adaptor.py | 14 +++-- greenwave_monitor/src/greenwave_monitor.cpp | 12 ++++ .../test/test_greenwave_monitor.py | 60 +++++++++++++------ .../test/test_topic_monitoring_integration.py | 51 ++++++++++++---- 6 files changed, 103 insertions(+), 41 deletions(-) diff --git a/greenwave_monitor/config/example.yaml b/greenwave_monitor/config/example.yaml index 371bd4f..76c9141 100644 --- a/greenwave_monitor/config/example.yaml +++ b/greenwave_monitor/config/example.yaml @@ -8,4 +8,4 @@ greenwave_monitor: expected_frequency: 100.0 tolerance: 10.0 /image_topic: - expected_frequency: 0.0 # No initial monitoring for this topic + expected_frequency: 0.0 # Invalid parameter settings are ignored diff --git a/greenwave_monitor/greenwave_monitor/ncurses_frontend.py b/greenwave_monitor/greenwave_monitor/ncurses_frontend.py index d8b364b..a50d231 100644 --- a/greenwave_monitor/greenwave_monitor/ncurses_frontend.py +++ b/greenwave_monitor/greenwave_monitor/ncurses_frontend.py @@ -338,10 +338,7 @@ def curses_main(stdscr, node): # Get expected frequency expected_hz, tolerance = node.ui_adaptor.get_expected_frequency(topic_name) if expected_hz > 0.0: - expected_freq_display = f'{expected_hz:.1f}Hz' - if tolerance > 0.0: - expected_freq_display += f'±{tolerance:.0f}%' - expected_freq_display = expected_freq_display.ljust(12) + expected_freq_display = f'{expected_hz:.1f}Hz'.ljust(12) # Color coding based on status if is_monitored: diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 6171c92..30d84ba 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -173,11 +173,15 @@ def _on_diagnostics(self, msg: DiagnosticArray): # Normalize the topic name to handle both NITROS and Greenwave formats topic_name = self._extract_topic_name(status.name) self.ui_diagnostics[topic_name] = ui_data - expected_frequency = float(ui_data.expected_frequency) - tolerance = float(ui_data.tolerance) - if expected_frequency > 0 and tolerance >= 0: - self.expected_frequencies[topic_name] = (expected_frequency, tolerance) - else: + try: + expected_frequency = float(ui_data.expected_frequency) + tolerance = float(ui_data.tolerance) + if expected_frequency > 0 and tolerance >= 0: + self.expected_frequencies[topic_name] = (expected_frequency, tolerance) + else: + self.expected_frequencies.pop(topic_name, None) + except (ValueError, TypeError): + # Skip updating expected_frequencies if values aren't numeric self.expected_frequencies.pop(topic_name, None) def toggle_topic_monitoring(self, topic_name: str): diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index ad4c05c..d5cb714 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -408,6 +408,9 @@ void GreenwaveMonitor::add_topics_from_parameters() // For each topic, read parameters and add topic with expected frequency settings for (const auto & topic : topics) { + if (topic.empty()) { + continue; + } std::string freq_param = std::string(kTopicParamPrefix) + topic + kExpectedFrequencySuffix; std::string tol_param = std::string(kTopicParamPrefix) + topic + kToleranceSuffix; @@ -419,12 +422,21 @@ void GreenwaveMonitor::add_topics_from_parameters() } if (this->has_parameter(tol_param)) { tolerance = get_double_param(tol_param); + // Default to 0 if tolerance is negative + if (tolerance < 0.0) { + tolerance = 0.0; + } } std::string message; if (add_topic(topic, message)) { if (expected_frequency > 0.0) { greenwave_diagnostics_[topic]->setExpectedDt(expected_frequency, tolerance); + } else { + RCLCPP_WARN( + this->get_logger(), + "Expected frequency is 0 for topic '%s', skipping parameter settings", + topic.c_str()); } } } diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index bee58c0..ced28be 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -18,6 +18,7 @@ # SPDX-License-Identifier: Apache-2.0 +import os import tempfile import time import unittest @@ -45,6 +46,10 @@ from rclpy.node import Node +# Temp directory auto-cleans when garbage collected or process exits +_temp_dir = tempfile.TemporaryDirectory() + + def create_test_yaml_config(): """Create a temporary YAML config file for testing parameter loading.""" # Use /** wildcard to match any namespace/node name @@ -55,19 +60,17 @@ def create_test_yaml_config(): /test_topic_valid_expected_frequency: expected_frequency: 100.0 tolerance: 10.0 - /test_topic_invalid_expected_frequency: - expected_frequency: 0.0 - tolerance: 0.0 + /test_topic_invalid_parameters: + expected_frequency: -10.0 + tolerance: -10.0 /test_topic_integer_params: expected_frequency: 50 tolerance: 5 """ - # Create temp file that persists until explicitly deleted - temp_file = tempfile.NamedTemporaryFile( - mode='w', suffix='.yaml', delete=False) - temp_file.write(yaml_content) - temp_file.flush() - return temp_file.name + filepath = os.path.join(_temp_dir.name, 'test_config.yaml') + with open(filepath, 'w') as f: + f.write(yaml_content) + return filepath @pytest.mark.launch_test @@ -99,7 +102,7 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): '/test_topic_integer_params', 50.0, message_type, '_integer_params'), create_minimal_publisher( - '/test_topic_invalid_expected_frequency', + '/test_topic_invalid_parameters', expected_frequency, message_type, '_invalid_expected_frequency') ] @@ -359,15 +362,36 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc msg='Integer tolerance from YAML should be 5' ) - # Verify that the invalid frequency topic is not monitored - INVALID_TOPIC = '/test_topic_invalid_expected_frequency' - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, INVALID_TOPIC, expected_count=0, timeout_sec=10.0 + # Verify topic with invalid (negative) params is monitored but with 0 values + invalid_topic = '/test_topic_invalid_parameters' + invalid_diagnostics = collect_diagnostics_for_topic( + self.test_node, invalid_topic, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(invalid_diagnostics), 1, + f'Topic {invalid_topic} should still be monitored' + ) + + invalid_status, _ = find_best_diagnostic( + invalid_diagnostics, 0.0, message_type) + self.assertIsNotNone( + invalid_status, f'Did not find diagnostics for {invalid_topic}') + + invalid_values = {kv.key: kv.value for kv in invalid_status.values} + + # Invalid (negative) expected_frequency should report as 0 + self.assertIn('expected_frequency', invalid_values) + self.assertAlmostEqual( + float(invalid_values['expected_frequency']), 0.0, places=1, + msg='Invalid expected frequency should report as 0' + ) + + # Invalid (negative) tolerance should report as 0 + self.assertIn('tolerance', invalid_values) + self.assertAlmostEqual( + float(invalid_values['tolerance']), 0.0, places=1, + msg='Invalid tolerance should report as 0' ) - self.assertEqual( - len(received_diagnostics), 0, - f'Topic {INVALID_TOPIC} with invalid expected frequency ' - 'should not be monitored') if __name__ == '__main__': diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index ae9ea0d..ca345c1 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -17,6 +17,7 @@ # # SPDX-License-Identifier: Apache-2.0 +import os import tempfile import threading import time @@ -42,6 +43,10 @@ from rclpy.node import Node +# Temp directory auto-cleans when garbage collected or process exits +_temp_dir = tempfile.TemporaryDirectory() + + def create_test_yaml_config(): """Create a temporary YAML config file for testing parameter loading.""" # Use /** wildcard to match any namespace/node name @@ -53,18 +58,16 @@ def create_test_yaml_config(): expected_frequency: 75.0 tolerance: 15.0 /yaml_invalid_topic: - expected_frequency: 0.0 - tolerance: 0.0 + expected_frequency: -10.0 + tolerance: -10.0 /yaml_integer_topic: expected_frequency: 60 tolerance: 12 """ - # Create temp file that persists until explicitly deleted - temp_file = tempfile.NamedTemporaryFile( - mode='w', suffix='.yaml', delete=False) - temp_file.write(yaml_content) - temp_file.flush() - return temp_file.name + filepath = os.path.join(_temp_dir.name, 'test_config.yaml') + with open(filepath, 'w') as f: + f.write(yaml_content) + return filepath @pytest.mark.launch_test @@ -437,13 +440,35 @@ def test_yaml_sets_parameters_at_startup( float(topic_data.tolerance), 15.0, places=1, msg='Tolerance from YAML should be 15.0') - # Verify that the invalid topic (0.0 expected_frequency) is not monitored + # Verify topic with invalid (negative) params is monitored but with 0 values invalid_topic = '/yaml_invalid_topic' - invalid_data = self.diagnostics_monitor.get_topic_diagnostics(invalid_topic) - self.assertEqual( + invalid_data = None + + while time.time() - start_time < max_wait_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + invalid_data = self.diagnostics_monitor.get_topic_diagnostics( + invalid_topic) + if invalid_data.status != '-': + break + time.sleep(0.1) + + # Topic should still be monitored + self.assertIsNotNone(invalid_data) + self.assertNotEqual( invalid_data.status, '-', - f'Topic {invalid_topic} with invalid expected frequency ' - 'should not be monitored') + f'Topic {invalid_topic} should still be monitored') + + # Invalid (negative) expected_frequency should report as 0 + self.assertNotEqual(invalid_data.expected_frequency, '-') + self.assertAlmostEqual( + float(invalid_data.expected_frequency), 0.0, places=1, + msg='Invalid expected frequency should report as 0') + + # Invalid (negative) tolerance should report as 0 + self.assertNotEqual(invalid_data.tolerance, '-') + self.assertAlmostEqual( + float(invalid_data.tolerance), 0.0, places=1, + msg='Invalid tolerance should report as 0') # Verify integer parameters are handled correctly int_topic = '/yaml_integer_topic' From b5fd5135fa1b55688f4d6341137aab8a0f25eb9e Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Tue, 13 Jan 2026 09:01:24 -0800 Subject: [PATCH 07/14] Add retry back to find topic Signed-off-by: Blake McHale --- .../include/greenwave_monitor.hpp | 7 +++-- greenwave_monitor/src/greenwave_monitor.cpp | 27 ++++++++++++------- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index c45096e..fdb1e0a 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -54,7 +54,8 @@ class GreenwaveMonitor : public rclcpp::Node } private: - std::optional find_topic_type(const std::string & topic); + std::optional find_topic_type( + const std::string & topic, int max_retries = 0, double retry_wait_s = 0.0); void topic_callback( const std::shared_ptr msg, @@ -72,7 +73,9 @@ class GreenwaveMonitor : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - bool add_topic(const std::string & topic, std::string & message); + bool add_topic( + const std::string & topic, std::string & message, + int max_retries = 0, double retry_wait_s = 0.0); bool remove_topic(const std::string & topic, std::string & message); diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index d5cb714..97dc30a 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -81,14 +81,20 @@ void GreenwaveMonitor::deferred_init() std::placeholders::_1, std::placeholders::_2)); } -std::optional GreenwaveMonitor::find_topic_type(const std::string & topic) +std::optional GreenwaveMonitor::find_topic_type( + const std::string & topic, int max_retries, double retry_wait_s) { - std::vector publishers; - publishers = this->get_publishers_info_by_topic(topic); - if (publishers.empty()) { - return std::nullopt; + for (int attempt = 0; attempt <= max_retries; ++attempt) { + auto publishers = this->get_publishers_info_by_topic(topic); + if (!publishers.empty()) { + return publishers[0].topic_type(); + } + if (attempt < max_retries && retry_wait_s > 0.0) { + std::this_thread::sleep_for( + std::chrono::duration(retry_wait_s)); + } } - return publishers[0].topic_type(); + return std::nullopt; } void GreenwaveMonitor::topic_callback( @@ -263,7 +269,8 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) return has_header; } -bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & message) +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()) { @@ -273,7 +280,7 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); - auto maybe_type = find_topic_type(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()); message = "Failed to find type for topic"; @@ -429,7 +436,9 @@ void GreenwaveMonitor::add_topics_from_parameters() } std::string message; - if (add_topic(topic, message)) { + static const int max_retries = 5; + static const double retry_wait_s = 0.5; + if (add_topic(topic, message, max_retries, retry_wait_s)) { if (expected_frequency > 0.0) { greenwave_diagnostics_[topic]->setExpectedDt(expected_frequency, tolerance); } else { From 4ad2656aa04d4bc18cd7cc5c4728b669c7d3c486 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Tue, 13 Jan 2026 09:41:03 -0800 Subject: [PATCH 08/14] Add catch for when context becomes invalid Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/greenwave_monitor/src/greenwave_monitor_main.cpp b/greenwave_monitor/src/greenwave_monitor_main.cpp index 5be5a58..95fc005 100644 --- a/greenwave_monitor/src/greenwave_monitor_main.cpp +++ b/greenwave_monitor/src/greenwave_monitor_main.cpp @@ -22,7 +22,13 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); rclcpp::NodeOptions options; auto node = std::make_shared(options); - rclcpp::spin(node); + try { + rclcpp::spin(node); + } catch (const rclcpp::exceptions::RCLError & e) { + RCLCPP_DEBUG( + node->get_logger(), + "Context became invalid during spin (expected during shutdown): %s", e.what()); + } rclcpp::shutdown(); return 0; } From e461081aa92900e3d9bee5dd1d13631f6943aa3e Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Tue, 13 Jan 2026 18:18:08 -0800 Subject: [PATCH 09/14] Add fixes for comments Signed-off-by: Blake McHale --- greenwave_monitor/config/example.yaml | 27 ++++- greenwave_monitor/examples/example.launch.py | 11 +- .../include/greenwave_diagnostics.hpp | 8 +- greenwave_monitor/src/greenwave_monitor.cpp | 4 + .../test/test_greenwave_monitor.py | 113 +++++++++++------- .../test/test_topic_monitoring_integration.py | 87 ++++++++------ 6 files changed, 159 insertions(+), 91 deletions(-) diff --git a/greenwave_monitor/config/example.yaml b/greenwave_monitor/config/example.yaml index 76c9141..3a6732f 100644 --- a/greenwave_monitor/config/example.yaml +++ b/greenwave_monitor/config/example.yaml @@ -1,11 +1,36 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 # Example configuration for greenwave_monitor # Defines topics to monitor with their expected frequencies and tolerances greenwave_monitor: ros__parameters: + # topics parameter specifies topics to monitor that do not require expected frequencies or + # tolerances. + topics: ['/string_topic'] + # greenwave_diagnostics parameter specifies the topics to monitor and their expected + # frequencies and tolerances. greenwave_diagnostics: /imu_topic: expected_frequency: 100.0 tolerance: 10.0 /image_topic: - expected_frequency: 0.0 # Invalid parameter settings are ignored + # If an invalid frequency is provided (e.g. 0.0), the topic will be monitored but no + # expected frequency will be set. + expected_frequency: 0.0 + # Invalid tolerance values are clamped to 0.0. + tolerance: -10.0 diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index ff6bb88..8760682 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -57,7 +57,16 @@ def generate_launch_description(): executable='greenwave_monitor', name='greenwave_monitor', output='log', - parameters=[config_file, {'topics': ['/string_topic']}], + # Example of inline parameter settings + # parameters=[{ + # 'topics': ['/string_topic'], + # 'greenwave_diagnostics': { + # '/imu_topic': {'expected_frequency': 100.0, 'tolerance': 10.0}, + # '/image_topic': {'expected_frequency': 0.0, 'tolerance': -10.0} + # } + # }], + # Example of using a config file + parameters=[config_file], ), LogInfo( msg='Run `ros2 run r2s_gw r2s_gw` in another terminal to see the demo output ' diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index 674d268..42ccafe 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -235,7 +235,7 @@ class GreenwaveDiagnostics values.push_back( diagnostic_msgs::build() .key("expected_frequency") - .value(std::to_string(frequency_))); + .value(std::to_string(expected_frequency_))); values.push_back( diagnostic_msgs::build() .key("tolerance") @@ -299,7 +299,7 @@ class GreenwaveDiagnostics const int64_t expected_dt_us = static_cast(greenwave_diagnostics::constants::kSecondsToMicroseconds / expected_hz); - frequency_ = expected_hz; + expected_frequency_ = expected_hz; diagnostics_config_.expected_dt_us = expected_dt_us; const int tolerance_us = @@ -318,7 +318,7 @@ class GreenwaveDiagnostics diagnostics_config_.expected_dt_us = 0; diagnostics_config_.jitter_tolerance_us = 0; - frequency_ = 0.0; + expected_frequency_ = 0.0; tolerance_ = 0.0; } @@ -505,7 +505,7 @@ class GreenwaveDiagnostics return error_found; } - double frequency_{0.0}; + double expected_frequency_{0.0}; double tolerance_{0.0}; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 97dc30a..e2cc2b4 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -431,6 +431,10 @@ void GreenwaveMonitor::add_topics_from_parameters() tolerance = get_double_param(tol_param); // Default to 0 if tolerance is negative if (tolerance < 0.0) { + RCLCPP_WARN( + this->get_logger(), + "Invalid tolerance for topic '%s', clamping to 0.0", + topic.c_str()); tolerance = 0.0; } } diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index ced28be..b72c913 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -49,23 +49,46 @@ # Temp directory auto-cleans when garbage collected or process exits _temp_dir = tempfile.TemporaryDirectory() +TEST_TOPIC = '/test_topic' +TEST_TOPIC1 = '/test_topic1' +TEST_TOPIC2 = '/test_topic2' + +VALID_PARAMS_TOPIC = '/test_topic_valid_parameters' +VALID_PARAMS_EXPECTED_FREQUENCY = 100.0 +VALID_PARAMS_TOLERANCE = 10.0 + +INVALID_PARAMS_TOPIC = '/test_topic_invalid_parameters' +INVALID_PARAMS_EXPECTED_FREQUENCY = -10.0 +INVALID_PARAMS_TOLERANCE = -10.0 + +INTEGER_PARAMS_TOPIC = '/test_topic_integer_params' +INTEGER_PARAMS_EXPECTED_FREQUENCY = 50 +INTEGER_PARAMS_TOLERANCE = 5 + +NONEXISTENT_TOPIC = '/test_topic_nonexistent' +NONEXISTENT_EXPECTED_FREQUENCY = 10.0 +NONEXISTENT_TOLERANCE = 1.0 + def create_test_yaml_config(): """Create a temporary YAML config file for testing parameter loading.""" # Use /** wildcard to match any namespace/node name - yaml_content = """\ + yaml_content = f"""\ /**: ros__parameters: greenwave_diagnostics: - /test_topic_valid_expected_frequency: - expected_frequency: 100.0 - tolerance: 10.0 - /test_topic_invalid_parameters: - expected_frequency: -10.0 - tolerance: -10.0 - /test_topic_integer_params: - expected_frequency: 50 - tolerance: 5 + {VALID_PARAMS_TOPIC}: + expected_frequency: {VALID_PARAMS_EXPECTED_FREQUENCY} + tolerance: {VALID_PARAMS_TOLERANCE} + {INVALID_PARAMS_TOPIC}: + expected_frequency: {INVALID_PARAMS_EXPECTED_FREQUENCY} + tolerance: {INVALID_PARAMS_TOLERANCE} + {INTEGER_PARAMS_TOPIC}: + expected_frequency: {INTEGER_PARAMS_EXPECTED_FREQUENCY} + tolerance: {INTEGER_PARAMS_TOLERANCE} + {NONEXISTENT_TOPIC}: + expected_frequency: {NONEXISTENT_EXPECTED_FREQUENCY} + tolerance: {NONEXISTENT_TOLERANCE} """ filepath = os.path.join(_temp_dir.name, 'test_config.yaml') with open(filepath, 'w') as f: @@ -90,19 +113,19 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): # Create publishers publishers = [ # Main test topic publisher with parametrized frequency - create_minimal_publisher('/test_topic', expected_frequency, message_type), + create_minimal_publisher(TEST_TOPIC, expected_frequency, message_type), # Additional publishers for topic management tests - create_minimal_publisher('/test_topic1', expected_frequency, message_type, '1'), - create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2'), + create_minimal_publisher(TEST_TOPIC1, expected_frequency, message_type, '1'), + create_minimal_publisher(TEST_TOPIC2, expected_frequency, message_type, '2'), # Publisher for YAML configuration tests create_minimal_publisher( - '/test_topic_valid_expected_frequency', + {VALID_PARAMS_TOPIC}, expected_frequency, message_type, '_valid_expected_frequency'), create_minimal_publisher( - '/test_topic_integer_params', - 50.0, message_type, '_integer_params'), + {INTEGER_PARAMS_TOPIC}, + expected_frequency, message_type, '_integer_params'), create_minimal_publisher( - '/test_topic_invalid_parameters', + {INVALID_PARAMS_TOPIC}, expected_frequency, message_type, '_invalid_expected_frequency') ] @@ -206,7 +229,7 @@ def test_frequency_monitoring(self, expected_frequency, message_type, tolerance_ """Test that the monitor node correctly tracks different frequencies.""" # This test runs for all configurations to verify frequency monitoring self.check_node_launches_successfully() - self.verify_diagnostics('/test_topic', expected_frequency, message_type, tolerance_hz) + self.verify_diagnostics(TEST_TOPIC, expected_frequency, message_type, tolerance_hz) def call_manage_topic(self, add, topic, service_client): """Service call helper.""" @@ -223,8 +246,6 @@ def test_manage_one_topic(self, expected_frequency, message_type, tolerance_hz): service_client = self.check_node_launches_successfully() - TEST_TOPIC = '/test_topic' - # 1. Remove an existing topic – should succeed on first attempt. response = self.call_manage_topic( add=False, topic=TEST_TOPIC, service_client=service_client) @@ -255,18 +276,14 @@ def test_manage_multiple_topics(self, expected_frequency, message_type, toleranc service_client = self.check_node_launches_successfully() - TEST_TOPIC1 = '/test_topic1' - TEST_TOPIC2 = '/test_topic2' - # Allow some time for topic discovery end_time = time.time() + 1.0 while time.time() < end_time: rclpy.spin_once(self.test_node, timeout_sec=0.1) # Try to add a non-existent topic - should fail - nonexistent_topic = '/test/nonexistent_topic' response = self.call_manage_topic( - add=True, topic=nonexistent_topic, service_client=service_client) + add=True, topic=NONEXISTENT_TOPIC, service_client=service_client) self.assertFalse(response.success) # 1. Add first topic – should succeed. @@ -297,24 +314,21 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc self.check_node_launches_successfully() - # Topic defined in test_config.yaml with valid expected_frequency - YAML_TOPIC = '/test_topic_valid_expected_frequency' - # Collect diagnostics - topic should already be monitored from YAML received_diagnostics = collect_diagnostics_for_topic( - self.test_node, YAML_TOPIC, expected_count=3, timeout_sec=10.0 + self.test_node, VALID_PARAMS_TOPIC, expected_count=3, timeout_sec=10.0 ) self.assertGreaterEqual( len(received_diagnostics), 1, - f'Topic {YAML_TOPIC} from YAML should be auto-monitored' + f'Topic {VALID_PARAMS_TOPIC} from YAML should be auto-monitored' ) # Verify the expected_frequency from YAML (100.0) is applied best_status, _ = find_best_diagnostic( - received_diagnostics, 100.0, message_type + received_diagnostics, VALID_PARAMS_EXPECTED_FREQUENCY, message_type ) - self.assertIsNotNone(best_status, f'Did not find diagnostics for {YAML_TOPIC}') + self.assertIsNotNone(best_status, f'Did not find diagnostics for {VALID_PARAMS_TOPIC}') # Extract values from diagnostic status diag_values = {kv.key: kv.value for kv in best_status.values} @@ -322,60 +336,58 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc # Check that expected_frequency was set from YAML self.assertIn('expected_frequency', diag_values) self.assertAlmostEqual( - float(diag_values['expected_frequency']), 100.0, places=1, + float(diag_values['expected_frequency']), VALID_PARAMS_EXPECTED_FREQUENCY, places=1, msg='Expected frequency from YAML should be 100.0' ) # Verify the tolerance from YAML (10.0) is applied self.assertIn('tolerance', diag_values) self.assertAlmostEqual( - float(diag_values['tolerance']), 10.0, places=1, + float(diag_values['tolerance']), VALID_PARAMS_TOLERANCE, places=1, msg='Tolerance from YAML should be 10.0' ) # Verify integer parameters are handled correctly - INT_TOPIC = '/test_topic_integer_params' int_diagnostics = collect_diagnostics_for_topic( - self.test_node, INT_TOPIC, expected_count=3, timeout_sec=10.0 + self.test_node, INTEGER_PARAMS_TOPIC, expected_count=3, timeout_sec=10.0 ) self.assertGreaterEqual( len(int_diagnostics), 1, - f'Topic {INT_TOPIC} with integer params should be monitored' + f'Topic {INTEGER_PARAMS_TOPIC} with integer params should be monitored' ) - int_status, _ = find_best_diagnostic(int_diagnostics, 50.0, message_type) - self.assertIsNotNone(int_status, f'Did not find diagnostics for {INT_TOPIC}') + int_status, _ = find_best_diagnostic(int_diagnostics, INTEGER_PARAMS_EXPECTED_FREQUENCY, message_type) + self.assertIsNotNone(int_status, f'Did not find diagnostics for {INTEGER_PARAMS_TOPIC}') int_values = {kv.key: kv.value for kv in int_status.values} # Check integer expected_frequency (50) is properly converted self.assertIn('expected_frequency', int_values) self.assertAlmostEqual( - float(int_values['expected_frequency']), 50.0, places=1, + float(int_values['expected_frequency']), INTEGER_PARAMS_EXPECTED_FREQUENCY, places=1, msg='Integer expected frequency from YAML should be 50' ) # Check integer tolerance (5) is properly converted self.assertIn('tolerance', int_values) self.assertAlmostEqual( - float(int_values['tolerance']), 5.0, places=1, + float(int_values['tolerance']), INTEGER_PARAMS_TOLERANCE, places=1, msg='Integer tolerance from YAML should be 5' ) # Verify topic with invalid (negative) params is monitored but with 0 values - invalid_topic = '/test_topic_invalid_parameters' invalid_diagnostics = collect_diagnostics_for_topic( - self.test_node, invalid_topic, expected_count=3, timeout_sec=10.0 + self.test_node, INVALID_PARAMS_TOPIC, expected_count=3, timeout_sec=10.0 ) self.assertGreaterEqual( len(invalid_diagnostics), 1, - f'Topic {invalid_topic} should still be monitored' + f'Topic {INVALID_PARAMS_TOPIC} should still be monitored' ) invalid_status, _ = find_best_diagnostic( - invalid_diagnostics, 0.0, message_type) + invalid_diagnostics, INVALID_PARAMS_EXPECTED_FREQUENCY, message_type) self.assertIsNotNone( - invalid_status, f'Did not find diagnostics for {invalid_topic}') + invalid_status, f'Did not find diagnostics for {INVALID_PARAMS_TOPIC}') invalid_values = {kv.key: kv.value for kv in invalid_status.values} @@ -393,6 +405,15 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc msg='Invalid tolerance should report as 0' ) + # Verify topic with nonexistent parameters is not monitored + nonexistent_diagnostics = collect_diagnostics_for_topic( + self.test_node, NONEXISTENT_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertEqual( + len(nonexistent_diagnostics), 0, + f'Topic {NONEXISTENT_TOPIC} should not be monitored' + ) + if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index ca345c1..e7f150a 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -46,23 +46,35 @@ # Temp directory auto-cleans when garbage collected or process exits _temp_dir = tempfile.TemporaryDirectory() +YAML_CONFIG_TOPIC = '/yaml_config_topic' +YAML_CONFIG_EXPECTED_FREQUENCY = 75.0 +YAML_CONFIG_TOLERANCE = 15.0 + +YAML_INVALID_TOPIC = '/yaml_invalid_topic' +YAML_INVALID_EXPECTED_FREQUENCY = -10.0 +YAML_INVALID_TOLERANCE = -10.0 + +YAML_INTEGER_TOPIC = '/yaml_integer_topic' +YAML_INTEGER_EXPECTED_FREQUENCY = 60 +YAML_INTEGER_TOLERANCE = 12 + def create_test_yaml_config(): """Create a temporary YAML config file for testing parameter loading.""" # Use /** wildcard to match any namespace/node name - yaml_content = """\ + yaml_content = f"""\ /**: ros__parameters: greenwave_diagnostics: - /yaml_config_topic: - expected_frequency: 75.0 - tolerance: 15.0 - /yaml_invalid_topic: - expected_frequency: -10.0 - tolerance: -10.0 - /yaml_integer_topic: - expected_frequency: 60 - tolerance: 12 + {YAML_CONFIG_TOPIC}: + expected_frequency: {YAML_CONFIG_EXPECTED_FREQUENCY} + tolerance: {YAML_CONFIG_TOLERANCE} + {YAML_INVALID_TOPIC}: + expected_frequency: {YAML_INVALID_EXPECTED_FREQUENCY} + tolerance: {YAML_INVALID_TOLERANCE} + {YAML_INTEGER_TOPIC}: + expected_frequency: {YAML_INTEGER_EXPECTED_FREQUENCY} + tolerance: {YAML_INTEGER_TOLERANCE} """ filepath = os.path.join(_temp_dir.name, 'test_config.yaml') with open(filepath, 'w') as f: @@ -92,12 +104,14 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2'), # Publisher for service discovery tests create_minimal_publisher('/discovery_test_topic', 50.0, 'imu', '_discovery'), - # Publisher for YAML config test (75 Hz as defined in YAML) - create_minimal_publisher('/yaml_config_topic', 75.0, 'imu', '_yaml_config'), - # Publisher for invalid YAML config test (0 Hz - should not be monitored) - create_minimal_publisher('/yaml_invalid_topic', 50.0, 'imu', '_yaml_invalid'), - # Publisher for integer YAML config test (60 Hz as integer in YAML) - create_minimal_publisher('/yaml_integer_topic', 60.0, 'imu', '_yaml_integer') + # Publisher for YAML config test + create_minimal_publisher( + YAML_CONFIG_TOPIC, YAML_CONFIG_EXPECTED_FREQUENCY, 'imu', '_yaml_config'), + # Publisher for invalid YAML config test (should still be monitored but with 0 values) + create_minimal_publisher(YAML_INVALID_TOPIC, 50.0, 'imu', '_yaml_invalid'), + # Publisher for integer YAML config test + create_minimal_publisher( + YAML_INTEGER_TOPIC, YAML_INTEGER_EXPECTED_FREQUENCY, 'imu', '_yaml_integer') ] context = { @@ -408,8 +422,6 @@ def test_yaml_sets_parameters_at_startup( if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: self.skipTest('Only running YAML config tests once') - yaml_topic = '/yaml_config_topic' - # Spin to receive diagnostics from the YAML-configured topic max_wait_time = 10.0 start_time = time.time() @@ -417,7 +429,7 @@ def test_yaml_sets_parameters_at_startup( while time.time() - start_time < max_wait_time: rclpy.spin_once(self.test_node, timeout_sec=0.1) - topic_data = self.diagnostics_monitor.get_topic_diagnostics(yaml_topic) + topic_data = self.diagnostics_monitor.get_topic_diagnostics(YAML_CONFIG_TOPIC) if topic_data.status != '-': break time.sleep(0.1) @@ -426,28 +438,26 @@ def test_yaml_sets_parameters_at_startup( self.assertIsNotNone(topic_data) self.assertNotEqual( topic_data.status, '-', - f'Topic {yaml_topic} from YAML should be auto-monitored') + f'Topic {YAML_CONFIG_TOPIC} from YAML should be auto-monitored') - # Verify expected_frequency from YAML (75.0) is applied + # Verify expected_frequency from YAML is applied self.assertNotEqual(topic_data.expected_frequency, '-') self.assertAlmostEqual( - float(topic_data.expected_frequency), 75.0, places=1, - msg='Expected frequency from YAML should be 75.0') + float(topic_data.expected_frequency), YAML_CONFIG_EXPECTED_FREQUENCY, places=1, + msg=f'Expected frequency from YAML should be {YAML_CONFIG_EXPECTED_FREQUENCY}') - # Verify tolerance from YAML (15.0) is applied + # Verify tolerance from YAML is applied self.assertNotEqual(topic_data.tolerance, '-') self.assertAlmostEqual( - float(topic_data.tolerance), 15.0, places=1, - msg='Tolerance from YAML should be 15.0') + float(topic_data.tolerance), YAML_CONFIG_TOLERANCE, places=1, + msg=f'Tolerance from YAML should be {YAML_CONFIG_TOLERANCE}') # Verify topic with invalid (negative) params is monitored but with 0 values - invalid_topic = '/yaml_invalid_topic' invalid_data = None while time.time() - start_time < max_wait_time: rclpy.spin_once(self.test_node, timeout_sec=0.1) - invalid_data = self.diagnostics_monitor.get_topic_diagnostics( - invalid_topic) + invalid_data = self.diagnostics_monitor.get_topic_diagnostics(YAML_INVALID_TOPIC) if invalid_data.status != '-': break time.sleep(0.1) @@ -456,7 +466,7 @@ def test_yaml_sets_parameters_at_startup( self.assertIsNotNone(invalid_data) self.assertNotEqual( invalid_data.status, '-', - f'Topic {invalid_topic} should still be monitored') + f'Topic {YAML_INVALID_TOPIC} should still be monitored') # Invalid (negative) expected_frequency should report as 0 self.assertNotEqual(invalid_data.expected_frequency, '-') @@ -471,13 +481,12 @@ def test_yaml_sets_parameters_at_startup( msg='Invalid tolerance should report as 0') # Verify integer parameters are handled correctly - int_topic = '/yaml_integer_topic' int_data = None start_time = time.time() while time.time() - start_time < max_wait_time: rclpy.spin_once(self.test_node, timeout_sec=0.1) - int_data = self.diagnostics_monitor.get_topic_diagnostics(int_topic) + int_data = self.diagnostics_monitor.get_topic_diagnostics(YAML_INTEGER_TOPIC) if int_data.status != '-': break time.sleep(0.1) @@ -486,19 +495,19 @@ def test_yaml_sets_parameters_at_startup( self.assertIsNotNone(int_data) self.assertNotEqual( int_data.status, '-', - f'Topic {int_topic} with integer params should be monitored') + f'Topic {YAML_INTEGER_TOPIC} with integer params should be monitored') - # Check integer expected_frequency (60) is properly converted + # Check integer expected_frequency is properly converted self.assertNotEqual(int_data.expected_frequency, '-') self.assertAlmostEqual( - float(int_data.expected_frequency), 60.0, places=1, - msg='Integer expected frequency from YAML should be 60') + float(int_data.expected_frequency), YAML_INTEGER_EXPECTED_FREQUENCY, places=1, + msg=f'Integer expected frequency from YAML should be {YAML_INTEGER_EXPECTED_FREQUENCY}') - # Check integer tolerance (12) is properly converted + # Check integer tolerance is properly converted self.assertNotEqual(int_data.tolerance, '-') self.assertAlmostEqual( - float(int_data.tolerance), 12.0, places=1, - msg='Integer tolerance from YAML should be 12') + float(int_data.tolerance), YAML_INTEGER_TOLERANCE, places=1, + msg=f'Integer tolerance from YAML should be {YAML_INTEGER_TOLERANCE}') def test_service_timeout_handling(self, expected_frequency, message_type, tolerance_hz): """Test service call timeout handling.""" From fc769b6bc443fbf24d22034c2e1aa882b84ffd76 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Tue, 13 Jan 2026 18:33:17 -0800 Subject: [PATCH 10/14] Add an or negative Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e2cc2b4..0edfeee 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -448,7 +448,7 @@ void GreenwaveMonitor::add_topics_from_parameters() } else { RCLCPP_WARN( this->get_logger(), - "Expected frequency is 0 for topic '%s', skipping parameter settings", + "Expected frequency is 0 or negative for topic '%s', skipping parameter settings", topic.c_str()); } } From 943b6a10c56c78889841785d4b1a7ce076ced4f8 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Tue, 13 Jan 2026 18:36:19 -0800 Subject: [PATCH 11/14] Fix lint Signed-off-by: Blake McHale --- greenwave_monitor/greenwave_monitor/test_utils.py | 5 +++-- greenwave_monitor/test/test_greenwave_monitor.py | 9 +++++---- .../test/test_topic_monitoring_integration.py | 9 +++++---- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index df4d3df..36b46bd 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -32,9 +32,10 @@ # (message_type, expected_frequency, tolerance_hz) # NOTE: Tolerances and frequencies are set conservatively for reliable operation # on slow/loaded CI systems such as the ROS buildfarm. The 30% tolerance standard -# ensures tests pass even under system load. +# ensures tests pass even under system load. Low frequencies (1 Hz) use 50% +# tolerance due to higher timing variability. TEST_CONFIGURATIONS = [ - ('imu', 1.0, 0.3), + ('imu', 1.0, 0.5), ('imu', 100.0, 30.0), ('imu', 500.0, 150.0), ('image', 10.0, 3.0), diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index b72c913..cd5506d 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -119,13 +119,13 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): create_minimal_publisher(TEST_TOPIC2, expected_frequency, message_type, '2'), # Publisher for YAML configuration tests create_minimal_publisher( - {VALID_PARAMS_TOPIC}, + VALID_PARAMS_TOPIC, expected_frequency, message_type, '_valid_expected_frequency'), create_minimal_publisher( - {INTEGER_PARAMS_TOPIC}, + INTEGER_PARAMS_TOPIC, expected_frequency, message_type, '_integer_params'), create_minimal_publisher( - {INVALID_PARAMS_TOPIC}, + INVALID_PARAMS_TOPIC, expected_frequency, message_type, '_invalid_expected_frequency') ] @@ -356,7 +356,8 @@ def test_yaml_parameter_loading(self, expected_frequency, message_type, toleranc f'Topic {INTEGER_PARAMS_TOPIC} with integer params should be monitored' ) - int_status, _ = find_best_diagnostic(int_diagnostics, INTEGER_PARAMS_EXPECTED_FREQUENCY, message_type) + int_status, _ = find_best_diagnostic( + int_diagnostics, INTEGER_PARAMS_EXPECTED_FREQUENCY, message_type) self.assertIsNotNone(int_status, f'Did not find diagnostics for {INTEGER_PARAMS_TOPIC}') int_values = {kv.key: kv.value for kv in int_status.values} diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index e7f150a..d8f9949 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -109,9 +109,9 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): YAML_CONFIG_TOPIC, YAML_CONFIG_EXPECTED_FREQUENCY, 'imu', '_yaml_config'), # Publisher for invalid YAML config test (should still be monitored but with 0 values) create_minimal_publisher(YAML_INVALID_TOPIC, 50.0, 'imu', '_yaml_invalid'), - # Publisher for integer YAML config test + # Publisher for integer YAML config test (use float for publisher, YAML uses int) create_minimal_publisher( - YAML_INTEGER_TOPIC, YAML_INTEGER_EXPECTED_FREQUENCY, 'imu', '_yaml_integer') + YAML_INTEGER_TOPIC, float(YAML_INTEGER_EXPECTED_FREQUENCY), 'imu', '_yaml_integer') ] context = { @@ -500,8 +500,9 @@ def test_yaml_sets_parameters_at_startup( # Check integer expected_frequency is properly converted self.assertNotEqual(int_data.expected_frequency, '-') self.assertAlmostEqual( - float(int_data.expected_frequency), YAML_INTEGER_EXPECTED_FREQUENCY, places=1, - msg=f'Integer expected frequency from YAML should be {YAML_INTEGER_EXPECTED_FREQUENCY}') + float(int_data.expected_frequency), YAML_INTEGER_EXPECTED_FREQUENCY, + places=1, msg='Integer expected frequency from YAML should be ' + f'{YAML_INTEGER_EXPECTED_FREQUENCY}') # Check integer tolerance is properly converted self.assertNotEqual(int_data.tolerance, '-') From 9e8b9fd3cc438c1098bf43d5da96c4ff8cfd671b Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Tue, 13 Jan 2026 22:49:16 -0800 Subject: [PATCH 12/14] Up wait times of services Signed-off-by: Blake McHale --- greenwave_monitor/test/test_greenwave_monitor.py | 2 +- greenwave_monitor/test/test_topic_monitoring_integration.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index cd5506d..5cb72ce 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -188,7 +188,7 @@ def check_node_launches_successfully(self): self.test_node, MONITOR_NODE_NAMESPACE, MONITOR_NODE_NAME ) service_available = wait_for_service_connection( - self.test_node, manage_client, timeout_sec=3.0, + self.test_node, manage_client, timeout_sec=10.0, service_name=f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}/manage_topic' ) self.assertTrue( diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index d8f9949..81f4f22 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -203,10 +203,10 @@ def test_service_discovery_default_namespace( # Verify services are available manage_available = self.diagnostics_monitor.manage_topic_client.wait_for_service( - timeout_sec=5.0) + timeout_sec=10.0) set_freq_available = ( self.diagnostics_monitor.set_expected_frequency_client - .wait_for_service(timeout_sec=5.0)) + .wait_for_service(timeout_sec=10.0)) self.assertTrue(manage_available, 'ManageTopic service should be available') self.assertTrue(set_freq_available, 'SetExpectedFrequency service should be available') From b4e265b25a58037d2d4c99cf0bce0e7c80a35295 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Wed, 14 Jan 2026 14:50:06 -0800 Subject: [PATCH 13/14] Rename parameters Signed-off-by: Blake McHale --- README.md | 2 +- greenwave_monitor/config/example.yaml | 12 +++++------ greenwave_monitor/examples/README.md | 2 +- greenwave_monitor/examples/example.launch.py | 4 ++-- .../greenwave_monitor/test_utils.py | 2 +- greenwave_monitor/launch/hz.launch.py | 4 ++-- greenwave_monitor/src/greenwave_monitor.cpp | 20 +++++++++---------- .../test/test_greenwave_monitor.py | 4 ++-- .../test/test_topic_monitoring_integration.py | 4 ++-- 9 files changed, 27 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index d50fc53..c926a89 100644 --- a/README.md +++ b/README.md @@ -108,5 +108,5 @@ You can of course also launch the node standalone, or incorporate it into your o If you want to use it as a command line tool, you can do so with the following launch file: ```bash -ros2 launch greenwave_monitor hz.launch.py topics:='["/topic1", "/topic2"]' +ros2 launch greenwave_monitor hz.launch.py gw_monitored_topics:='["/topic1", "/topic2"]' ``` \ No newline at end of file diff --git a/greenwave_monitor/config/example.yaml b/greenwave_monitor/config/example.yaml index 3a6732f..a143411 100644 --- a/greenwave_monitor/config/example.yaml +++ b/greenwave_monitor/config/example.yaml @@ -19,12 +19,12 @@ greenwave_monitor: ros__parameters: - # topics parameter specifies topics to monitor that do not require expected frequencies or - # tolerances. - topics: ['/string_topic'] - # greenwave_diagnostics parameter specifies the topics to monitor and their expected - # frequencies and tolerances. - greenwave_diagnostics: + # gw_monitored_topics parameter specifies topics to monitor that do not require expected + # frequencies or tolerances. + gw_monitored_topics: ['/string_topic'] + # gw_frequency_monitored_topics parameter specifies the topics to monitor and their + # expected frequencies and tolerances. + gw_frequency_monitored_topics: /imu_topic: expected_frequency: 100.0 tolerance: 10.0 diff --git a/greenwave_monitor/examples/README.md b/greenwave_monitor/examples/README.md index 1b33bca..cfa4518 100644 --- a/greenwave_monitor/examples/README.md +++ b/greenwave_monitor/examples/README.md @@ -26,7 +26,7 @@ Node( name='greenwave_monitor', output='screen', # or 'log' if you want to add monitoring without terminal output parameters=[ - {'topics': ['/your_topic_1', '/your_topic_2']} # List your topics to monitor + {'gw_monitored_topics': ['/your_topic_1', '/your_topic_2']} # List your topics to monitor ], ), ``` diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index 8760682..d479205 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -59,8 +59,8 @@ def generate_launch_description(): output='log', # Example of inline parameter settings # parameters=[{ - # 'topics': ['/string_topic'], - # 'greenwave_diagnostics': { + # 'gw_monitored_topics': ['/string_topic'], + # 'gw_frequency_monitored_topics': { # '/imu_topic': {'expected_frequency': 100.0, 'tolerance': 10.0}, # '/image_topic': {'expected_frequency': 0.0, 'tolerance': -10.0} # } diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 36b46bd..74acb68 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -69,7 +69,7 @@ def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE, parameters: List[dict[str, Any]] = None): """Create a greenwave_monitor node for testing.""" if parameters is None: - parameters = [{'topics': ['/test_topic']}] + parameters = [{'gw_monitored_topics': ['/test_topic']}] return launch_ros.actions.Node( package='greenwave_monitor', diff --git a/greenwave_monitor/launch/hz.launch.py b/greenwave_monitor/launch/hz.launch.py index e081a12..bb679df 100644 --- a/greenwave_monitor/launch/hz.launch.py +++ b/greenwave_monitor/launch/hz.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( - 'topics', + 'gw_monitored_topics', default_value='[""]', description='List of topics to monitor' ), @@ -33,7 +33,7 @@ def generate_launch_description(): name='greenwave_monitor', output='screen', parameters=[ - {'topics': LaunchConfiguration('topics'), + {'gw_monitored_topics': LaunchConfiguration('gw_monitored_topics'), 'use_sim_time': LaunchConfiguration('use_sim_time')} ], ), diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 0edfeee..9d59ff4 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -31,7 +31,7 @@ namespace greenwave_monitor { namespace constants { -inline constexpr const char * kTopicParamPrefix = "greenwave_diagnostics."; +inline constexpr const char * kTopicParamPrefix = "gw_frequency_monitored_topics."; inline constexpr const char * kExpectedFrequencySuffix = ".expected_frequency"; inline constexpr const char * kToleranceSuffix = ".tolerance"; } // namespace constants @@ -45,8 +45,8 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) { RCLCPP_INFO(this->get_logger(), "Starting GreenwaveMonitorNode"); - if (!this->has_parameter("topics")) { - this->declare_parameter>("topics", {""}); + if (!this->has_parameter("gw_monitored_topics")) { + this->declare_parameter>("gw_monitored_topics", {""}); } timer_ = this->create_wall_timer( @@ -372,18 +372,18 @@ void GreenwaveMonitor::add_topics_from_parameters() std::set topics; - // List all parameters with "greenwave_diagnostics." prefix - auto list_result = this->list_parameters({"greenwave_diagnostics"}, 10); + // List all parameters with "gw_frequency_monitored_topics." prefix + auto list_result = this->list_parameters({"gw_frequency_monitored_topics"}, 10); - // Loop over and find all unique topics with "greenwave_diagnostics." prefix + // Loop over and find all unique topics with "gw_frequency_monitored_topics." prefix for (const auto & param_name : list_result.names) { - // Parameter names are like "greenwave_diagnostics./my_topic.tolerance" + // Parameter names are like "gw_frequency_monitored_topics./my_topic.tolerance" // We need to extract the topic name (e.g., "/my_topic") if (param_name.find(kTopicParamPrefix) != 0) { continue; } - // Remove the "greenwave_diagnostics." prefix + // Remove the "gw_frequency_monitored_topics." prefix std::string remainder = param_name.substr(std::strlen(kTopicParamPrefix)); // Find the last '.' to separate topic name from parameter suffix @@ -398,8 +398,8 @@ void GreenwaveMonitor::add_topics_from_parameters() } } - // Add topics from "topics" parameter - auto topics_param = this->get_parameter("topics").as_string_array(); + // Add topics from "gw_monitored_topics" parameter + auto topics_param = this->get_parameter("gw_monitored_topics").as_string_array(); topics.insert(topics_param.begin(), topics_param.end()); // Helper function to get double parameters from the node diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 5cb72ce..82b58d4 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -76,7 +76,7 @@ def create_test_yaml_config(): yaml_content = f"""\ /**: ros__parameters: - greenwave_diagnostics: + gw_frequency_monitored_topics: {VALID_PARAMS_TOPIC}: expected_frequency: {VALID_PARAMS_EXPECTED_FREQUENCY} tolerance: {VALID_PARAMS_TOLERANCE} @@ -107,7 +107,7 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): ros2_monitor_node = create_monitor_node( namespace=MONITOR_NODE_NAMESPACE, node_name=MONITOR_NODE_NAME, - parameters=[yaml_config_file, {'topics': ['/test_topic']}] + parameters=[yaml_config_file, {'gw_monitored_topics': ['/test_topic']}] ) # Create publishers diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index 81f4f22..3641c64 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -65,7 +65,7 @@ def create_test_yaml_config(): yaml_content = f"""\ /**: ros__parameters: - greenwave_diagnostics: + gw_frequency_monitored_topics: {YAML_CONFIG_TOPIC}: expected_frequency: {YAML_CONFIG_EXPECTED_FREQUENCY} tolerance: {YAML_CONFIG_TOLERANCE} @@ -92,7 +92,7 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): # Launch the greenwave_monitor with YAML config ros2_monitor_node = create_monitor_node( node_name=MONITOR_NODE_NAME, - parameters=[yaml_config_file, {'topics': ['/test_topic']}] + parameters=[yaml_config_file, {'gw_monitored_topics': ['/test_topic']}] ) # Create publishers for testing From b72522a02285f53e2803d2804f6c45915d65cd2e Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Wed, 14 Jan 2026 14:58:34 -0800 Subject: [PATCH 14/14] Bump tolerance at low freq to 0.6 Signed-off-by: Blake McHale --- greenwave_monitor/greenwave_monitor/test_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 74acb68..49f5e0d 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -35,7 +35,7 @@ # ensures tests pass even under system load. Low frequencies (1 Hz) use 50% # tolerance due to higher timing variability. TEST_CONFIGURATIONS = [ - ('imu', 1.0, 0.5), + ('imu', 1.0, 0.6), ('imu', 100.0, 30.0), ('imu', 500.0, 150.0), ('image', 10.0, 3.0),