From 256a121442a361863b2b8937317786e111a58f7a Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 09:35:40 -0800 Subject: [PATCH 01/11] Add controlling topic expected frequencies and tolerances through ROS 2 parameters Signed-off-by: Blake McHale --- greenwave_monitor/CMakeLists.txt | 11 + greenwave_monitor/examples/example.launch.py | 17 +- .../greenwave_monitor/test_utils.py | 26 +- .../greenwave_monitor/ui_adaptor.py | 181 +++++++++ .../include/greenwave_monitor.hpp | 33 ++ greenwave_monitor/src/greenwave_monitor.cpp | 365 +++++++++++++++++- .../test/parameters/test_param_dynamic.py | 199 ++++++++++ .../test/parameters/test_param_freq_only.py | 118 ++++++ .../test/parameters/test_param_new_topic.py | 131 +++++++ .../test/parameters/test_param_tol_only.py | 107 +++++ .../test/parameters/test_param_yaml.py | 132 +++++++ .../test/parameters/test_topic_parameters.py | 162 ++++++++ .../test/test_topic_monitoring_integration.py | 8 + 13 files changed, 1462 insertions(+), 28 deletions(-) create mode 100644 greenwave_monitor/test/parameters/test_param_dynamic.py create mode 100644 greenwave_monitor/test/parameters/test_param_freq_only.py create mode 100644 greenwave_monitor/test/parameters/test_param_new_topic.py create mode 100644 greenwave_monitor/test/parameters/test_param_tol_only.py create mode 100644 greenwave_monitor/test/parameters/test_param_yaml.py create mode 100644 greenwave_monitor/test/parameters/test_topic_parameters.py diff --git a/greenwave_monitor/CMakeLists.txt b/greenwave_monitor/CMakeLists.txt index 7c31f6d..29c1a56 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -116,6 +116,17 @@ if(BUILD_TESTING) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) + # Add parameter-based topic configuration tests (in test/parameters/) + # Automatically discover and add all test_*.py files in the parameters directory + file(GLOB PARAM_TEST_FILES "${CMAKE_SOURCE_DIR}/test/parameters/test_*.py") + foreach(TEST_FILE ${PARAM_TEST_FILES}) + get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) + ament_add_pytest_test(${TEST_NAME} test/parameters/${TEST_NAME}.py + TIMEOUT 120 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + # Add gtests ament_add_gtest(test_message_diagnostics test/test_message_diagnostics.cpp TIMEOUT 60 diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index b7266c1..6f05710 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -13,7 +13,6 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import LogInfo from launch_ros.actions import Node @@ -52,11 +51,13 @@ def generate_launch_description(): name='greenwave_monitor', output='log', parameters=[ - {'topics': ['/imu_topic', '/image_topic', '/string_topic']} - ], - ), - LogInfo( - msg='Run `ros2 run r2s_gw r2s_gw` in another terminal to see the demo output ' - 'with the r2s dashboard.' - ), + { + 'topics': { + '/imu_topic': {'expected_frequency': 100.0}, + '/image_topic': {'expected_frequency': 30.0}, + '/string_topic': {'expected_frequency': 1000.0} + }, + } + ] + ) ]) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 7434b1d..67d2d87 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -22,6 +22,11 @@ from typing import List, Optional, Tuple from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from greenwave_monitor.ui_adaptor import ( + FREQ_SUFFIX, + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency import launch_ros import rclpy @@ -65,19 +70,32 @@ def create_minimal_publisher( def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE, node_name: str = MONITOR_NODE_NAME, - topics: List[str] = None): + topics: List[str] = None, + topic_configs: dict = None): """Create a greenwave_monitor node for testing.""" if topics is None: topics = ['/test_topic'] + # Ensure topics list has at least one element (even if empty string) + if not topics: + topics = [''] + + params = {'topics': topics} + + if topic_configs: + for topic, config in topic_configs.items(): + if 'expected_frequency' in config: + params[f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}'] = float( + config['expected_frequency']) + if 'tolerance' in config: + params[f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}'] = float(config['tolerance']) + return launch_ros.actions.Node( package='greenwave_monitor', executable='greenwave_monitor', name=node_name, namespace=namespace, - parameters=[{ - 'topics': topics - }], + parameters=[params], output='screen' ) diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 09ed0f8..2e7ba8d 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -36,15 +36,58 @@ """ from dataclasses import dataclass +from enum import Enum import threading import time from typing import Dict from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency +from rcl_interfaces.msg import ParameterEvent, ParameterType, ParameterValue +from rcl_interfaces.srv import GetParameters, ListParameters import rclpy from rclpy.node import Node +# Parameter name constants +TOPIC_PARAM_PREFIX = 'topics.' +FREQ_SUFFIX = '.expected_frequency' +TOL_SUFFIX = '.tolerance' +DEFAULT_TOLERANCE_PERCENT = 5.0 + + +class TopicParamField(Enum): + """Type of topic parameter field.""" + + NONE = 0 + FREQUENCY = 1 + TOLERANCE = 2 + + +def parse_topic_param_name(param_name: str) -> tuple[str, TopicParamField]: + """Parse parameter name to extract topic name and field type.""" + if not param_name.startswith(TOPIC_PARAM_PREFIX): + return ('', TopicParamField.NONE) + + topic_and_field = param_name[len(TOPIC_PARAM_PREFIX):] + + if topic_and_field.endswith(FREQ_SUFFIX): + topic_name = topic_and_field[:-len(FREQ_SUFFIX)] + return (topic_name, TopicParamField.FREQUENCY) + elif topic_and_field.endswith(TOL_SUFFIX): + topic_name = topic_and_field[:-len(TOL_SUFFIX)] + return (topic_name, TopicParamField.TOLERANCE) + + return ('', TopicParamField.NONE) + + +def param_value_to_float(value: ParameterValue) -> float | None: + """Convert a ParameterValue to float if it's a numeric type.""" + if value.type == ParameterType.PARAMETER_DOUBLE: + return value.double_value + elif value.type == ParameterType.PARAMETER_INTEGER: + return float(value.integer_value) + return None + @dataclass class UiDiagnosticData: @@ -119,6 +162,13 @@ def _setup_ros_components(self): 100 ) + self.param_events_subscription = self.node.create_subscription( + ParameterEvent, + '/parameter_events', + self._on_parameter_event, + 10 + ) + manage_service_name = f'{self.monitor_node_name}/manage_topic' set_freq_service_name = f'{self.monitor_node_name}/set_expected_frequency' @@ -134,6 +184,108 @@ def _setup_ros_components(self): set_freq_service_name ) + # Parameter service clients for querying initial state + self.list_params_client = self.node.create_client( + ListParameters, + f'{self.monitor_node_name}/list_parameters' + ) + self.get_params_client = self.node.create_client( + GetParameters, + f'{self.monitor_node_name}/get_parameters' + ) + + # Query initial parameters after a short delay to let services come up + self._initial_params_timer = self.node.create_timer( + 0.1, self._fetch_initial_parameters_callback) + + def _fetch_initial_parameters_callback(self): + """Timer callback to fetch initial parameters (retries until services ready).""" + # Check if services are available (non-blocking) + if not self.list_params_client.service_is_ready(): + return # Timer will retry + + if not self.get_params_client.service_is_ready(): + return # Timer will retry + + # Cancel timer now that services are ready + if self._initial_params_timer is not None: + self._initial_params_timer.cancel() + self._initial_params_timer = None + + # List all parameters (prefixes filter is unreliable with nested names) + list_request = ListParameters.Request() + list_request.prefixes = ['topics'] + list_request.depth = 10 + + list_future = self.list_params_client.call_async(list_request) + list_future.add_done_callback(self._on_list_parameters_response) + + def _on_list_parameters_response(self, future): + """Handle response from list_parameters service.""" + try: + if future.result() is None: + return + + all_param_names = future.result().result.names + + # Filter to only topic parameters (prefixes filter is unreliable) + param_names = [n for n in all_param_names if n.startswith(TOPIC_PARAM_PREFIX)] + + if not param_names: + return + + # Store for use in get callback + self._pending_param_names = param_names + + # Get values for topic parameters only + get_request = GetParameters.Request() + get_request.names = param_names + + get_future = self.get_params_client.call_async(get_request) + get_future.add_done_callback(self._on_get_parameters_response) + except Exception as e: + self.node.get_logger().debug(f'Error listing parameters: {e}') + + def _on_get_parameters_response(self, future): + """Handle response from get_parameters service.""" + try: + if future.result() is None: + return + + param_names = getattr(self, '_pending_param_names', []) + values = future.result().values + + # Parse parameters into expected_frequencies + topic_configs: Dict[str, Dict[str, float]] = {} + + for name, value in zip(param_names, values): + numeric_value = param_value_to_float(value) + if numeric_value is None: + continue + + topic_name, field = parse_topic_param_name(name) + if not topic_name or field == TopicParamField.NONE: + continue + + if topic_name not in topic_configs: + topic_configs[topic_name] = {} + + if field == TopicParamField.FREQUENCY: + topic_configs[topic_name]['freq'] = numeric_value + elif field == TopicParamField.TOLERANCE: + topic_configs[topic_name]['tol'] = numeric_value + + # Update expected_frequencies with valid configs + with self.data_lock: + for topic_name, config in topic_configs.items(): + freq = config.get('freq', 0.0) + tol = config.get('tol', DEFAULT_TOLERANCE_PERCENT) + if freq > 0: + self.expected_frequencies[topic_name] = (freq, tol) + + except Exception as e: + self.node.get_logger().debug(f'Error fetching parameters: {e}') + def _extract_topic_name(self, diagnostic_name: str) -> str: """ Extract topic name from diagnostic status name. @@ -168,6 +320,35 @@ def _on_diagnostics(self, msg: DiagnosticArray): topic_name = self._extract_topic_name(status.name) self.ui_diagnostics[topic_name] = ui_data + def _on_parameter_event(self, msg: ParameterEvent): + """Process parameter change events from the monitor node.""" + # Only process events from the monitor node + if self.monitor_node_name not in msg.node: + return + + # Process changed and new parameters + for param in msg.changed_parameters + msg.new_parameters: + value = param_value_to_float(param.value) + if value is None: + continue + + topic_name, field = parse_topic_param_name(param.name) + if not topic_name or field == TopicParamField.NONE: + continue + + with self.data_lock: + current = self.expected_frequencies.get(topic_name, (0.0, 0.0)) + + if field == TopicParamField.FREQUENCY: + if value > 0: + self.expected_frequencies[topic_name] = (value, current[1]) + elif topic_name in self.expected_frequencies: + del self.expected_frequencies[topic_name] + + elif field == TopicParamField.TOLERANCE: + if current[0] > 0: # Only update if frequency is set + self.expected_frequencies[topic_name] = (current[0], value) + def toggle_topic_monitoring(self, topic_name: str): """Toggle monitoring for a topic.""" if not self.manage_topic_client.wait_for_service(timeout_sec=1.0): diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index f9a39bb..c736639 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -26,6 +26,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" #include "std_msgs/msg/string.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" @@ -40,6 +41,12 @@ class GreenwaveMonitor : public rclcpp::Node explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); private: + struct TopicConfig + { + std::optional expected_frequency; + std::optional tolerance; + }; + std::optional find_topic_type_with_retry( const std::string & topic, const int max_retries, const int retry_period_s); @@ -57,6 +64,29 @@ class GreenwaveMonitor : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + bool set_topic_expected_frequency( + const std::string & topic_name, + double expected_hz, + double tolerance_percent, + bool add_topic_if_missing, + std::string & message, + bool update_parameters = true); + + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters); + + void apply_topic_config_if_complete(const std::string & topic_name); + + void load_topic_parameters_from_overrides(); + + std::optional get_numeric_parameter(const std::string & param_name); + + void try_undeclare_parameter(const std::string & param_name); + + void declare_or_set_parameter(const std::string & param_name, double value); + + void undeclare_topic_parameters(const std::string & topic_name); + bool add_topic(const std::string & topic, std::string & message); bool remove_topic(const std::string & topic, std::string & message); @@ -76,4 +106,7 @@ class GreenwaveMonitor : public rclcpp::Node manage_topic_service_; rclcpp::Service::SharedPtr set_expected_frequency_service_; + + std::map pending_topic_configs_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e73fc65..86183d9 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -22,17 +22,113 @@ #include #include +#include "rcl_interfaces/srv/list_parameters.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" using namespace std::chrono_literals; +namespace +{ +constexpr const char * kTopicParamPrefix = "topics."; +constexpr const char * kFreqSuffix = ".expected_frequency"; +constexpr const char * kTolSuffix = ".tolerance"; +constexpr double kDefaultTolerancePercent = 5.0; + +std::string make_freq_param_name(const std::string & topic_name) +{ + return std::string(kTopicParamPrefix) + topic_name + kFreqSuffix; +} + +std::string make_tol_param_name(const std::string & topic_name) +{ + return std::string(kTopicParamPrefix) + topic_name + kTolSuffix; +} + +enum class TopicParamField { kNone, kFrequency, kTolerance }; + +struct TopicParamInfo +{ + std::string topic_name; + TopicParamField field = TopicParamField::kNone; +}; + +// Parse a parameter name like "topics./my_topic.expected_frequency" into topic name and field type +TopicParamInfo parse_topic_param_name(const std::string & param_name) +{ + TopicParamInfo info; + + if (param_name.rfind(kTopicParamPrefix, 0) != 0) { + return info; + } + + std::string topic_and_field = param_name.substr(strlen(kTopicParamPrefix)); + + const size_t freq_suffix_len = strlen(kFreqSuffix); + const size_t tol_suffix_len = strlen(kTolSuffix); + + if (topic_and_field.length() > freq_suffix_len && + topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) + { + info.topic_name = topic_and_field.substr(0, topic_and_field.length() - freq_suffix_len); + info.field = TopicParamField::kFrequency; + } else if (topic_and_field.length() > tol_suffix_len && + topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) + { + info.topic_name = topic_and_field.substr(0, topic_and_field.length() - tol_suffix_len); + info.field = TopicParamField::kTolerance; + } + + return info; +} + +// Convert a parameter to double if it's a numeric type +std::optional param_to_double(const rclcpp::Parameter & param) +{ + 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 std::nullopt; +} + +const char * get_field_name(TopicParamField field) +{ + if (field == TopicParamField::kNone) { + return "none"; + } else if (field == TopicParamField::kFrequency) { + return "expected_frequency"; + } else if (field == TopicParamField::kTolerance) { + return "tolerance"; + } + return "unknown"; +} + +const char * get_field_unit(TopicParamField field) +{ + if (field == TopicParamField::kNone) { + return ""; + } else if (field == TopicParamField::kFrequency) { + return "Hz"; + } else if (field == TopicParamField::kTolerance) { + return "%"; + } + return "unknown"; +} +} // namespace + 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", {""}); + // Get the topics parameter (declare only if not already declared from overrides) + if (!this->has_parameter("topics")) { + this->declare_parameter>("topics", {""}); + } auto topics = this->get_parameter("topics").as_string_array(); message_diagnostics::MessageDiagnosticsConfig diagnostics_config; @@ -47,6 +143,13 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) } } + // Register parameter callback for dynamic topic configuration + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&GreenwaveMonitor::on_parameter_change, this, std::placeholders::_1)); + + // Process any topic parameters passed at startup + load_topic_parameters_from_overrides(); + timer_ = this->create_wall_timer( 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); @@ -143,30 +246,256 @@ void GreenwaveMonitor::handle_set_expected_frequency( if (request->clear_expected) { msg_diagnostics_obj.clearExpectedDt(); + undeclare_topic_parameters(request->topic_name); + response->success = true; response->message = "Successfully cleared expected frequency for topic '" + request->topic_name + "'"; return; } - if (request->expected_hz <= 0.0) { - response->success = false; - response->message = "Invalid expected frequency, must be set to a positive value"; - return; + response->success = set_topic_expected_frequency( + request->topic_name, + request->expected_hz, + request->tolerance_percent, + false, // topic already exists at this point + response->message); +} + +bool GreenwaveMonitor::set_topic_expected_frequency( + const std::string & topic_name, + double expected_hz, + double tolerance_percent, + bool add_topic_if_missing, + std::string & message, + bool update_parameters) +{ + auto it = message_diagnostics_.find(topic_name); + + if (it == message_diagnostics_.end()) { + if (!add_topic_if_missing) { + message = "Failed to find topic '" + topic_name + "'"; + return false; + } + + if (!add_topic(topic_name, message)) { + return false; + } + it = message_diagnostics_.find(topic_name); + } + + if (expected_hz <= 0.0) { + message = "Invalid expected frequency, must be set to a positive value"; + return false; + } + if (tolerance_percent < 0.0) { + message = "Invalid tolerance, must be a non-negative percentage"; + return false; + } + + message_diagnostics::MessageDiagnostics & msg_diagnostics_obj = *(it->second); + msg_diagnostics_obj.setExpectedDt(expected_hz, tolerance_percent); + + // Sync parameters with the new values + if (update_parameters) { + declare_or_set_parameter(make_freq_param_name(topic_name), expected_hz); + declare_or_set_parameter(make_tol_param_name(topic_name), tolerance_percent); } - if (request->tolerance_percent < 0.0) { - response->success = false; - response->message = - "Invalid tolerance, must be a non-negative percentage"; + + message = "Successfully set expected frequency for topic '" + + topic_name + "' to " + std::to_string(expected_hz) + + " hz with tolerance " + std::to_string(tolerance_percent) + "%"; + return true; +} + +rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + auto info = parse_topic_param_name(param.get_name()); + if (info.field == TopicParamField::kNone || info.topic_name.empty()) { + continue; + } + + auto value_opt = param_to_double(param); + if (!value_opt.has_value()) { + RCLCPP_WARN( + this->get_logger(), + "Parameter '%s' is not a numeric type, skipping", + param.get_name().c_str()); + continue; + } + + double value = value_opt.value(); + TopicConfig & config = pending_topic_configs_[info.topic_name]; + + if (info.field == TopicParamField::kFrequency) { + config.expected_frequency = value; + } else { + config.tolerance = value; + } + + RCLCPP_INFO( + this->get_logger(), + "Parameter set: %s for topic '%s' = %.2f %s", + get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); + + apply_topic_config_if_complete(info.topic_name); + } + + return result; +} + +void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_name) +{ + auto it = pending_topic_configs_.find(topic_name); + if (it == pending_topic_configs_.end()) { return; } - msg_diagnostics_obj.setExpectedDt(request->expected_hz, request->tolerance_percent); + const TopicConfig & config = it->second; + + // Get expected frequency from pending config or existing parameter + double expected_freq = 0.0; + if (config.expected_frequency.has_value()) { + expected_freq = config.expected_frequency.value(); + } else { + auto freq_opt = get_numeric_parameter(make_freq_param_name(topic_name)); + if (freq_opt.has_value()) { + expected_freq = freq_opt.value(); + } else { + // No frequency available, nothing to do + return; + } + } + + // Get tolerance from pending config, existing parameter, or default + double tolerance = config.tolerance.value_or( + get_numeric_parameter(make_tol_param_name(topic_name)).value_or(kDefaultTolerancePercent) + ); + + std::string message; + bool success = set_topic_expected_frequency( + topic_name, + expected_freq, + tolerance, + true, + message, + false); // don't update parameters - called from parameter change + + if (success) { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } else { + RCLCPP_WARN( + this->get_logger(), + "Could not apply config for topic '%s': %s. " + "Use manage_topic service to add the topic first.", + topic_name.c_str(), message.c_str()); + } + + pending_topic_configs_.erase(it); +} + +void GreenwaveMonitor::load_topic_parameters_from_overrides() +{ + // Parameters are automatically declared from overrides due to NodeOptions setting. + // List all parameters and filter by prefix manually (list_parameters prefix matching + // can be unreliable with deeply nested parameter names). + auto all_params = this->list_parameters( + {}, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); + + for (const auto & name : all_params.names) { + auto info = parse_topic_param_name(name); + if (info.field == TopicParamField::kNone || info.topic_name.empty()) { + continue; + } + + auto value_opt = get_numeric_parameter(name); + if (!value_opt.has_value()) { + continue; + } + + double value = value_opt.value(); + TopicConfig & config = pending_topic_configs_[info.topic_name]; + + if (info.field == TopicParamField::kFrequency) { + config.expected_frequency = value; + } else { + config.tolerance = value; + } + + RCLCPP_INFO( + this->get_logger(), + "Initial parameter: %s for topic '%s' = %.2f %s", + get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); + } + + // Apply all complete configs - at startup we can add topics if needed + std::vector topics_to_apply; + for (const auto & [topic, config] : pending_topic_configs_) { + if (config.expected_frequency.has_value()) { + topics_to_apply.push_back(topic); + } + } + for (const auto & topic : topics_to_apply) { + const TopicConfig & config = pending_topic_configs_[topic]; + double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); + + std::string message; + bool success = set_topic_expected_frequency( + topic, + config.expected_frequency.value(), + tolerance, + true, // add topic if missing - safe at startup + message, + false); // don't update parameters + + if (success) { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } else { + RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); + } + pending_topic_configs_.erase(topic); + } +} + +std::optional GreenwaveMonitor::get_numeric_parameter(const std::string & param_name) +{ + if (!this->has_parameter(param_name)) { + return std::nullopt; + } + return param_to_double(this->get_parameter(param_name)); +} + +void GreenwaveMonitor::try_undeclare_parameter(const std::string & param_name) +{ + try { + if (this->has_parameter(param_name)) { + this->undeclare_parameter(param_name); + } + } catch (const std::exception & e) { + RCLCPP_WARN( + this->get_logger(), "Could not undeclare %s: %s", + param_name.c_str(), e.what()); + } +} + +void GreenwaveMonitor::declare_or_set_parameter(const std::string & param_name, double value) +{ + if (!this->has_parameter(param_name)) { + this->declare_parameter(param_name, value); + } else { + this->set_parameter(rclcpp::Parameter(param_name, value)); + } +} - response->success = true; - response->message = "Successfully set expected frequency for topic '" + - request->topic_name + "' to " + std::to_string(request->expected_hz) + - " hz with tolerance " + std::to_string(request->tolerance_percent) + "%"; +void GreenwaveMonitor::undeclare_topic_parameters(const std::string & topic_name) +{ + try_undeclare_parameter(make_freq_param_name(topic_name)); + try_undeclare_parameter(make_tol_param_name(topic_name)); } bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) @@ -315,6 +644,10 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes } message_diagnostics_.erase(diag_it); + + // Clear any associated parameters + undeclare_topic_parameters(topic); + message = "Successfully removed topic"; return true; } diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py new file mode 100644 index 0000000..392b950 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -0,0 +1,199 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 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 + +"""Test: dynamic parameter changes via ros2 param set.""" + +import subprocess +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +from greenwave_monitor.ui_adaptor import ( + FREQ_SUFFIX, + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) +import launch +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/dynamic_param_topic' +TEST_FREQUENCY = 30.0 + + +def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: + """Run ros2 param set command and return success status.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] + try: + result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) + return result.returncode == 0 + except subprocess.TimeoutExpired: + return False + + +def run_ros2_param_get(node_name: str, param_name: str) -> tuple[bool, float | None]: + """Run ros2 param get command and return (success, value).""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + cmd = ['ros2', 'param', 'get', full_node_name, param_name] + try: + result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) + if result.returncode != 0: + return False, None + # Parse output like "Double value is: 30.0" or "Integer value is: 30" + output = result.stdout.strip() + if 'value is:' in output: + value_str = output.split('value is:')[1].strip() + return True, float(value_str) + return False, None + except (subprocess.TimeoutExpired, ValueError): + return False, None + + +def make_freq_param(topic: str) -> str: + """Build frequency parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' + + +def make_tol_param(topic: str) -> str: + """Build tolerance parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' + + +@pytest.mark.launch_test +def generate_test_description(): + """Test dynamic parameter changes via ros2 param set.""" + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[TEST_TOPIC], # Topic exists but no expected frequency + topic_configs={} + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestDynamicParameterChanges(unittest.TestCase): + """Test changing parameters dynamically via ros2 param set.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('dynamic_param_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_set_expected_frequency_via_param(self): + """Test setting expected frequency via ros2 param set.""" + time.sleep(2.0) + + freq_param = make_freq_param(TEST_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + time.sleep(1.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Expected diagnostics after setting frequency param' + ) + + def test_change_tolerance_via_param(self): + """Test changing tolerance via ros2 param set.""" + time.sleep(1.0) + + tol_param = make_tol_param(TEST_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, 20.0) + self.assertTrue(success, f'Failed to set {tol_param}') + + time.sleep(0.5) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=2, timeout_sec=5.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 2, + 'Topic should still be monitored after tolerance change' + ) + + def test_verify_params_with_get(self): + """Test that ros2 param get returns the values we set.""" + time.sleep(1.0) + + # Set specific values + freq_param = make_freq_param(TEST_TOPIC) + tol_param = make_tol_param(TEST_TOPIC) + expected_freq = 42.5 + expected_tol = 15.0 + + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, expected_freq) + self.assertTrue(success, f'Failed to set {freq_param}') + + success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, expected_tol) + self.assertTrue(success, f'Failed to set {tol_param}') + + time.sleep(0.5) + + # Verify with ros2 param get + success, actual_freq = run_ros2_param_get(MONITOR_NODE_NAME, freq_param) + self.assertTrue(success, f'Failed to get {freq_param}') + self.assertAlmostEqual( + actual_freq, expected_freq, places=1, + msg=f'Frequency mismatch: expected {expected_freq}, got {actual_freq}' + ) + + success, actual_tol = run_ros2_param_get(MONITOR_NODE_NAME, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, expected_tol, places=1, + msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py new file mode 100644 index 0000000..df1d89a --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 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 + +"""Test: only expected_frequency specified, tolerance defaults to 5%.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +import launch +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/freq_only_topic' +TEST_FREQUENCY = 50.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test with only expected_frequency specified.""" + topic_configs = { + TEST_TOPIC: { + 'expected_frequency': TEST_FREQUENCY + # No tolerance - should default to 5% + } + } + + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[], + topic_configs=topic_configs + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_freq_only' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestFrequencyOnlyParameter(unittest.TestCase): + """Test that only specifying frequency works (tolerance defaults).""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('freq_only_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_frequency_only_uses_default_tolerance(self): + """Test that specifying only frequency uses default tolerance.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + f'Expected at least 3 diagnostics, got {len(received_diagnostics)}' + ) + + has_valid_rate = False + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + has_valid_rate = True + break + except ValueError: + continue + if has_valid_rate: + break + + self.assertTrue(has_valid_rate, 'Should have valid frame rate with default tolerance') + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py new file mode 100644 index 0000000..1875516 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 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 + +"""Test: add new topic to monitoring via ros2 param set.""" + +import subprocess +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +from greenwave_monitor.ui_adaptor import ( + FREQ_SUFFIX, + TOPIC_PARAM_PREFIX, +) +import launch +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +NEW_TOPIC = '/new_dynamic_topic' +TEST_FREQUENCY = 50.0 + + +def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: + """Run ros2 param set command and return success status.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] + try: + result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) + return result.returncode == 0 + except subprocess.TimeoutExpired: + return False + + +def make_freq_param(topic: str) -> str: + """Build frequency parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' + + +@pytest.mark.launch_test +def generate_test_description(): + """Test adding a new topic via ros2 param set.""" + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[''], # Empty - no initial topics + topic_configs={} + ) + + publisher = create_minimal_publisher( + NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestAddNewTopicViaParam(unittest.TestCase): + """Test adding a new topic to monitoring via ros2 param set.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('new_topic_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_add_new_topic_via_frequency_param(self): + """Test that setting frequency param for new topic starts monitoring.""" + time.sleep(2.0) + + initial_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(initial_diagnostics), 0, + 'Topic should not be monitored initially' + ) + + freq_param = make_freq_param(NEW_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Should monitor new topic after setting frequency param' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py new file mode 100644 index 0000000..ddc8f08 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 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 + +"""Test: only tolerance specified - should NOT start monitoring.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +from greenwave_monitor.ui_adaptor import ( + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) +import launch +import launch_ros.actions +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/tol_only_topic' +TEST_FREQUENCY = 50.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test with only tolerance specified (should not monitor).""" + params = { + 'topics': [''], + f'{TOPIC_PARAM_PREFIX}{TEST_TOPIC}{TOL_SUFFIX}': 15.0 + } + + ros2_monitor_node = launch_ros.actions.Node( + package='greenwave_monitor', + executable='greenwave_monitor', + name=MONITOR_NODE_NAME, + namespace=MONITOR_NODE_NAMESPACE, + parameters=[params], + output='screen' + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_tol_only' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestToleranceOnlyParameter(unittest.TestCase): + """Test that only specifying tolerance does NOT start monitoring.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('tol_only_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_tolerance_only_does_not_monitor(self): + """Test that specifying only tolerance does not start monitoring.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=3.0 + ) + + self.assertEqual( + len(received_diagnostics), 0, + f'Should not monitor topic with only tolerance set, got {len(received_diagnostics)}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py new file mode 100644 index 0000000..6c1d7c5 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 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 + +"""Test: load topic configuration from YAML parameter file.""" + +import os +import tempfile +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +import launch +import launch_ros.actions +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +YAML_TOPIC = '/yaml_config_topic' +TEST_FREQUENCY = 50.0 +TEST_TOLERANCE = 10.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test loading parameters from a YAML file.""" + # Write YAML manually with proper quoting (yaml.dump doesn't quote keys with dots) + # Use /** to apply to all nodes, or use full namespace path + yaml_content = ( + f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n' + f' ros__parameters:\n' + f' topics:\n' + f" - ''\n" + f' "topics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n' + f' "topics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n' + ) + + yaml_dir = tempfile.mkdtemp() + yaml_path = os.path.join(yaml_dir, 'test_params.yaml') + with open(yaml_path, 'w') as f: + f.write(yaml_content) + + ros2_monitor_node = launch_ros.actions.Node( + package='greenwave_monitor', + executable='greenwave_monitor', + name=MONITOR_NODE_NAME, + namespace=MONITOR_NODE_NAMESPACE, + parameters=[yaml_path], + output='screen' + ) + + publisher = create_minimal_publisher( + YAML_TOPIC, TEST_FREQUENCY, 'imu', '_yaml' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestYamlParameterFile(unittest.TestCase): + """Test loading topic configuration from YAML parameter file.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('yaml_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_topic_configured_via_yaml(self): + """Test that topic is monitored when configured via YAML file.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, YAML_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Expected diagnostics from YAML-configured topic' + ) + + has_valid_rate = False + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + has_valid_rate = True + break + except ValueError: + continue + if has_valid_rate: + break + + self.assertTrue(has_valid_rate, 'Should have valid frame rate from YAML config') + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py new file mode 100644 index 0000000..4add93f --- /dev/null +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 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 + +"""Tests for parameter-based topic configuration - both frequency and tolerance.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +import launch +import launch_testing +from launch_testing import post_shutdown_test +from launch_testing.asserts import assertExitCodes +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/param_test_topic' +TEST_FREQUENCY = 50.0 +TEST_TOLERANCE = 10.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Generate launch description with both frequency and tolerance set.""" + topic_configs = { + TEST_TOPIC: { + 'expected_frequency': TEST_FREQUENCY, + 'tolerance': TEST_TOLERANCE + } + } + + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[], + topic_configs=topic_configs + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_param_test' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +@post_shutdown_test() +class TestTopicParametersPostShutdown(unittest.TestCase): + """Post-shutdown tests.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('shutdown_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_node_shutdown(self, proc_info): + """Test that the node shuts down correctly.""" + available_nodes = self.test_node.get_node_names() + self.assertNotIn(MONITOR_NODE_NAME, available_nodes) + assertExitCodes(proc_info, allowable_exit_codes=[0]) + + +class TestTopicParameters(unittest.TestCase): + """Tests for parameter-based topic configuration.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('topic_params_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_topic_configured_via_parameters(self): + """Test that topic is monitored when configured via parameters.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + f'Expected at least 3 diagnostics for {TEST_TOPIC}, got {len(received_diagnostics)}' + ) + + # Verify valid frame rate exists + best_status = None + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + frame_rate = float(kv.value) + if frame_rate > 0: + best_status = status + break + except ValueError: + continue + if best_status: + break + + self.assertIsNotNone( + best_status, + 'Should have received diagnostics with valid frame_rate_node' + ) + + frame_rate_node = None + for kv in best_status.values: + if kv.key == 'frame_rate_node': + frame_rate_node = float(kv.value) + break + + self.assertIsNotNone(frame_rate_node) + tolerance = TEST_FREQUENCY * 0.5 + self.assertAlmostEqual( + frame_rate_node, TEST_FREQUENCY, delta=tolerance, + msg=f'Frame rate {frame_rate_node} not within {tolerance} of {TEST_FREQUENCY}' + ) + + +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..3aca226 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -132,10 +132,18 @@ def tearDown(self): if hasattr(self, 'diagnostics_monitor'): # Clean up ROS components try: + timer = self.diagnostics_monitor._initial_params_timer + if timer is not None: + timer.cancel() + self.test_node.destroy_timer(timer) self.test_node.destroy_subscription(self.diagnostics_monitor.subscription) + self.test_node.destroy_subscription( + self.diagnostics_monitor.param_events_subscription) self.test_node.destroy_client(self.diagnostics_monitor.manage_topic_client) self.test_node.destroy_client( self.diagnostics_monitor.set_expected_frequency_client) + self.test_node.destroy_client(self.diagnostics_monitor.list_params_client) + self.test_node.destroy_client(self.diagnostics_monitor.get_params_client) except Exception: pass # Ignore cleanup errors From 596cc53868700eaa04e0231e3e10963deed74021 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 10:28:53 -0800 Subject: [PATCH 02/11] Add tests for nested YAML and don't specify the topics parameter every time Signed-off-by: Blake McHale --- greenwave_monitor/examples/example.launch.py | 6 +-- .../greenwave_monitor/test_utils.py | 17 +++---- .../test/parameters/test_param_dynamic.py | 42 +++++++++++++++-- .../test/parameters/test_param_freq_only.py | 4 -- .../test/parameters/test_param_new_topic.py | 7 +-- .../test/parameters/test_param_tol_only.py | 1 - .../test/parameters/test_param_yaml.py | 47 +++++++++++++++++-- .../test/parameters/test_topic_parameters.py | 3 -- 8 files changed, 94 insertions(+), 33 deletions(-) diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index 6f05710..c84b13a 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -53,9 +53,9 @@ def generate_launch_description(): parameters=[ { 'topics': { - '/imu_topic': {'expected_frequency': 100.0}, - '/image_topic': {'expected_frequency': 30.0}, - '/string_topic': {'expected_frequency': 1000.0} + '/imu_topic': {'expected_frequency': 100.0, 'tolerance': 5.0}, + '/image_topic': {'expected_frequency': 30.0, 'tolerance': 5.0}, + '/string_topic': {'expected_frequency': 1000.0, 'tolerance': 5.0} }, } ] diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 67d2d87..ac79bdd 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -73,14 +73,15 @@ def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE, topics: List[str] = None, topic_configs: dict = None): """Create a greenwave_monitor node for testing.""" - if topics is None: - topics = ['/test_topic'] - - # Ensure topics list has at least one element (even if empty string) - if not topics: - topics = [''] - - params = {'topics': topics} + params = {} + + # Only add topics param if explicitly provided or no topic_configs + if topics is not None: + if not topics: + topics = [''] + params['topics'] = topics + elif not topic_configs: + params['topics'] = ['/test_topic'] if topic_configs: for topic, config in topic_configs.items(): diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 392b950..6b130de 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -43,6 +43,7 @@ TEST_TOPIC = '/dynamic_param_topic' +NEW_TOPIC = '/new_param_topic' TEST_FREQUENCY = 30.0 @@ -89,20 +90,23 @@ def make_tol_param(topic: str) -> str: def generate_test_description(): """Test dynamic parameter changes via ros2 param set.""" ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[TEST_TOPIC], # Topic exists but no expected frequency - topic_configs={} + topics=[TEST_TOPIC] # Topic exists but no expected frequency ) publisher = create_minimal_publisher( TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' ) + # Publisher for topic not initially monitored + new_topic_publisher = create_minimal_publisher( + NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' + ) + return ( launch.LaunchDescription([ ros2_monitor_node, publisher, + new_topic_publisher, launch_testing.actions.ReadyToTest() ]), {} ) @@ -194,6 +198,36 @@ def test_verify_params_with_get(self): msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' ) + def test_add_new_topic_via_param(self): + """Test that setting frequency param for unmonitored topic starts monitoring.""" + time.sleep(1.0) + + # Verify topic is not initially monitored + initial_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(initial_diagnostics), 0, + f'{NEW_TOPIC} should not be monitored initially' + ) + + # Set expected frequency for the new topic + freq_param = make_freq_param(NEW_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + time.sleep(2.0) + + # Verify topic is now monitored + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + f'{NEW_TOPIC} should be monitored after setting frequency param' + ) + if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index df1d89a..805708a 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -26,7 +26,6 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE ) import launch @@ -51,9 +50,6 @@ def generate_test_description(): } ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[], topic_configs=topic_configs ) diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 1875516..6d8ac15 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -64,12 +64,7 @@ def make_freq_param(topic: str) -> str: @pytest.mark.launch_test def generate_test_description(): """Test adding a new topic via ros2 param set.""" - ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[''], # Empty - no initial topics - topic_configs={} - ) + ros2_monitor_node = create_monitor_node() publisher = create_minimal_publisher( NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index ddc8f08..f99e7b3 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -48,7 +48,6 @@ def generate_test_description(): """Test with only tolerance specified (should not monitor).""" params = { - 'topics': [''], f'{TOPIC_PARAM_PREFIX}{TEST_TOPIC}{TOL_SUFFIX}': 15.0 } diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 6c1d7c5..08d070e 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -39,22 +39,28 @@ YAML_TOPIC = '/yaml_config_topic' +NESTED_TOPIC = '/nested_yaml_topic' TEST_FREQUENCY = 50.0 +NESTED_FREQUENCY = 25.0 TEST_TOLERANCE = 10.0 @pytest.mark.launch_test def generate_test_description(): """Test loading parameters from a YAML file.""" - # Write YAML manually with proper quoting (yaml.dump doesn't quote keys with dots) - # Use /** to apply to all nodes, or use full namespace path + # Write YAML manually - demonstrates both flat dotted keys and nested dict formats + # Use full namespace path for node parameters yaml_content = ( f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n' f' ros__parameters:\n' - f' topics:\n' - f" - ''\n" + f' # Flat dotted key format (requires quotes)\n' f' "topics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n' f' "topics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n' + f' # Nested dictionary format\n' + f' topics:\n' + f' {NESTED_TOPIC}:\n' + f' expected_frequency: {NESTED_FREQUENCY}\n' + f' tolerance: {TEST_TOLERANCE}\n' ) yaml_dir = tempfile.mkdtemp() @@ -75,10 +81,15 @@ def generate_test_description(): YAML_TOPIC, TEST_FREQUENCY, 'imu', '_yaml' ) + nested_publisher = create_minimal_publisher( + NESTED_TOPIC, NESTED_FREQUENCY, 'imu', '_nested_yaml' + ) + return ( launch.LaunchDescription([ ros2_monitor_node, publisher, + nested_publisher, launch_testing.actions.ReadyToTest() ]), {} ) @@ -127,6 +138,34 @@ def test_topic_configured_via_yaml(self): self.assertTrue(has_valid_rate, 'Should have valid frame rate from YAML config') + def test_nested_dict_topic_configured_via_yaml(self): + """Test that topic configured via nested YAML dict is monitored.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, NESTED_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Expected diagnostics from nested YAML-configured topic' + ) + + has_valid_rate = False + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + has_valid_rate = True + break + except ValueError: + continue + if has_valid_rate: + break + + self.assertTrue(has_valid_rate, 'Should have valid frame rate from nested YAML config') + if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 4add93f..7c9c0cf 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -54,9 +54,6 @@ def generate_test_description(): } ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[], topic_configs=topic_configs ) From 2f319690435d86ebb38cbbf2d2baf5f22e1eaf0a Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 11:14:29 -0800 Subject: [PATCH 03/11] More common functions and use rclpy instead of subprocess for interfacing with parameters Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 83 ++++++++++++++ .../test/parameters/test_param_dynamic.py | 103 ++---------------- .../test/parameters/test_param_freq_only.py | 22 +--- .../test/parameters/test_param_new_topic.py | 28 +---- .../test/parameters/test_param_tol_only.py | 9 +- .../test/parameters/test_param_yaml.py | 41 ++----- .../test/parameters/test_topic_parameters.py | 2 +- 7 files changed, 119 insertions(+), 169 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index ac79bdd..1155357 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -29,6 +29,8 @@ ) from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency import launch_ros +from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue +from rcl_interfaces.srv import GetParameters, SetParameters import rclpy from rclpy.node import Node @@ -52,6 +54,87 @@ MONITOR_NODE_NAMESPACE = 'test_namespace' +def make_freq_param(topic: str) -> str: + """Build frequency parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' + + +def make_tol_param(topic: str) -> str: + """Build tolerance parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' + + +def set_parameter(test_node: Node, param_name: str, value: float, + node_name: str = MONITOR_NODE_NAME) -> bool: + """Set a parameter on the monitor node using rclpy service client.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + service_name = f'{full_node_name}/set_parameters' + + client = test_node.create_client(SetParameters, service_name) + if not client.wait_for_service(timeout_sec=5.0): + return False + + param = Parameter() + param.name = param_name + param.value = ParameterValue() + param.value.type = ParameterType.PARAMETER_DOUBLE + param.value.double_value = float(value) + + request = SetParameters.Request() + request.parameters = [param] + + future = client.call_async(request) + rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0) + + test_node.destroy_client(client) + + if future.result() is None: + return False + return all(r.successful for r in future.result().results) + + +def get_parameter(test_node: Node, param_name: str, + node_name: str = MONITOR_NODE_NAME) -> Tuple[bool, Optional[float]]: + """Get a parameter from the monitor node using rclpy service client.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + service_name = f'{full_node_name}/get_parameters' + + client = test_node.create_client(GetParameters, service_name) + if not client.wait_for_service(timeout_sec=5.0): + return False, None + + request = GetParameters.Request() + request.names = [param_name] + + future = client.call_async(request) + rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0) + + test_node.destroy_client(client) + + if future.result() is None or not future.result().values: + return False, None + + param_value = future.result().values[0] + if param_value.type == ParameterType.PARAMETER_DOUBLE: + return True, param_value.double_value + elif param_value.type == ParameterType.PARAMETER_INTEGER: + return True, float(param_value.integer_value) + return False, None + + +def has_valid_frame_rate(diagnostics: List[DiagnosticStatus]) -> bool: + """Check if any diagnostic has a valid (positive) frame_rate_node value.""" + for status in diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + return True + except ValueError: + continue + return False + + def create_minimal_publisher( topic: str, frequency_hz: float, message_type: str, id_suffix: str = ''): """Create a minimal publisher node with the given parameters.""" diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 6b130de..c625519 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -19,7 +19,6 @@ """Test: dynamic parameter changes via ros2 param set.""" -import subprocess import time import unittest @@ -27,13 +26,11 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE -) -from greenwave_monitor.ui_adaptor import ( - FREQ_SUFFIX, - TOL_SUFFIX, - TOPIC_PARAM_PREFIX, + get_parameter, + make_freq_param, + make_tol_param, + MONITOR_NODE_NAMESPACE, + set_parameter, ) import launch import launch_testing @@ -43,49 +40,9 @@ TEST_TOPIC = '/dynamic_param_topic' -NEW_TOPIC = '/new_param_topic' TEST_FREQUENCY = 30.0 -def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: - """Run ros2 param set command and return success status.""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' - cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] - try: - result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) - return result.returncode == 0 - except subprocess.TimeoutExpired: - return False - - -def run_ros2_param_get(node_name: str, param_name: str) -> tuple[bool, float | None]: - """Run ros2 param get command and return (success, value).""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' - cmd = ['ros2', 'param', 'get', full_node_name, param_name] - try: - result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) - if result.returncode != 0: - return False, None - # Parse output like "Double value is: 30.0" or "Integer value is: 30" - output = result.stdout.strip() - if 'value is:' in output: - value_str = output.split('value is:')[1].strip() - return True, float(value_str) - return False, None - except (subprocess.TimeoutExpired, ValueError): - return False, None - - -def make_freq_param(topic: str) -> str: - """Build frequency parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' - - -def make_tol_param(topic: str) -> str: - """Build tolerance parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' - - @pytest.mark.launch_test def generate_test_description(): """Test dynamic parameter changes via ros2 param set.""" @@ -97,16 +54,10 @@ def generate_test_description(): TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' ) - # Publisher for topic not initially monitored - new_topic_publisher = create_minimal_publisher( - NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' - ) - return ( launch.LaunchDescription([ ros2_monitor_node, publisher, - new_topic_publisher, launch_testing.actions.ReadyToTest() ]), {} ) @@ -132,7 +83,7 @@ def test_set_expected_frequency_via_param(self): time.sleep(2.0) freq_param = make_freq_param(TEST_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') time.sleep(1.0) @@ -151,7 +102,7 @@ def test_change_tolerance_via_param(self): time.sleep(1.0) tol_param = make_tol_param(TEST_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, 20.0) + success = set_parameter(self.test_node, tol_param, 20.0) self.assertTrue(success, f'Failed to set {tol_param}') time.sleep(0.5) @@ -175,59 +126,29 @@ def test_verify_params_with_get(self): expected_freq = 42.5 expected_tol = 15.0 - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, expected_freq) + success = set_parameter(self.test_node, freq_param, expected_freq) self.assertTrue(success, f'Failed to set {freq_param}') - success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, expected_tol) + success = set_parameter(self.test_node, tol_param, expected_tol) self.assertTrue(success, f'Failed to set {tol_param}') time.sleep(0.5) - # Verify with ros2 param get - success, actual_freq = run_ros2_param_get(MONITOR_NODE_NAME, freq_param) + # Verify with get_parameter + success, actual_freq = get_parameter(self.test_node, freq_param) self.assertTrue(success, f'Failed to get {freq_param}') self.assertAlmostEqual( actual_freq, expected_freq, places=1, msg=f'Frequency mismatch: expected {expected_freq}, got {actual_freq}' ) - success, actual_tol = run_ros2_param_get(MONITOR_NODE_NAME, tol_param) + success, actual_tol = get_parameter(self.test_node, tol_param) self.assertTrue(success, f'Failed to get {tol_param}') self.assertAlmostEqual( actual_tol, expected_tol, places=1, msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' ) - def test_add_new_topic_via_param(self): - """Test that setting frequency param for unmonitored topic starts monitoring.""" - time.sleep(1.0) - - # Verify topic is not initially monitored - initial_diagnostics = collect_diagnostics_for_topic( - self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0 - ) - self.assertEqual( - len(initial_diagnostics), 0, - f'{NEW_TOPIC} should not be monitored initially' - ) - - # Set expected frequency for the new topic - freq_param = make_freq_param(NEW_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) - self.assertTrue(success, f'Failed to set {freq_param}') - - time.sleep(2.0) - - # Verify topic is now monitored - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - f'{NEW_TOPIC} should be monitored after setting frequency param' - ) - if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index 805708a..c770b92 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -26,7 +26,8 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAMESPACE + has_valid_frame_rate, + MONITOR_NODE_NAMESPACE, ) import launch import launch_testing @@ -93,21 +94,10 @@ def test_frequency_only_uses_default_tolerance(self): len(received_diagnostics), 3, f'Expected at least 3 diagnostics, got {len(received_diagnostics)}' ) - - has_valid_rate = False - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - has_valid_rate = True - break - except ValueError: - continue - if has_valid_rate: - break - - self.assertTrue(has_valid_rate, 'Should have valid frame rate with default tolerance') + self.assertTrue( + has_valid_frame_rate(received_diagnostics), + 'Should have valid frame rate with default tolerance' + ) if __name__ == '__main__': diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 6d8ac15..297a448 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -19,7 +19,6 @@ """Test: add new topic to monitoring via ros2 param set.""" -import subprocess import time import unittest @@ -27,12 +26,9 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE -) -from greenwave_monitor.ui_adaptor import ( - FREQ_SUFFIX, - TOPIC_PARAM_PREFIX, + make_freq_param, + MONITOR_NODE_NAMESPACE, + set_parameter, ) import launch import launch_testing @@ -45,22 +41,6 @@ TEST_FREQUENCY = 50.0 -def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: - """Run ros2 param set command and return success status.""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' - cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] - try: - result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) - return result.returncode == 0 - except subprocess.TimeoutExpired: - return False - - -def make_freq_param(topic: str) -> str: - """Build frequency parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' - - @pytest.mark.launch_test def generate_test_description(): """Test adding a new topic via ros2 param set.""" @@ -107,7 +87,7 @@ def test_add_new_topic_via_frequency_param(self): ) freq_param = make_freq_param(NEW_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') time.sleep(2.0) diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index f99e7b3..4e87d9d 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -25,12 +25,9 @@ from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, + make_tol_param, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE -) -from greenwave_monitor.ui_adaptor import ( - TOL_SUFFIX, - TOPIC_PARAM_PREFIX, + MONITOR_NODE_NAMESPACE, ) import launch import launch_ros.actions @@ -48,7 +45,7 @@ def generate_test_description(): """Test with only tolerance specified (should not monitor).""" params = { - f'{TOPIC_PARAM_PREFIX}{TEST_TOPIC}{TOL_SUFFIX}': 15.0 + make_tol_param(TEST_TOPIC): 15.0 } ros2_monitor_node = launch_ros.actions.Node( diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 08d070e..b295d45 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -27,8 +27,9 @@ from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, + has_valid_frame_rate, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE + MONITOR_NODE_NAMESPACE, ) import launch import launch_ros.actions @@ -122,21 +123,10 @@ def test_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from YAML-configured topic' ) - - has_valid_rate = False - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - has_valid_rate = True - break - except ValueError: - continue - if has_valid_rate: - break - - self.assertTrue(has_valid_rate, 'Should have valid frame rate from YAML config') + self.assertTrue( + has_valid_frame_rate(received_diagnostics), + 'Should have valid frame rate from YAML config' + ) def test_nested_dict_topic_configured_via_yaml(self): """Test that topic configured via nested YAML dict is monitored.""" @@ -150,21 +140,10 @@ def test_nested_dict_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from nested YAML-configured topic' ) - - has_valid_rate = False - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - has_valid_rate = True - break - except ValueError: - continue - if has_valid_rate: - break - - self.assertTrue(has_valid_rate, 'Should have valid frame rate from nested YAML config') + self.assertTrue( + has_valid_frame_rate(received_diagnostics), + 'Should have valid frame rate from nested YAML config' + ) if __name__ == '__main__': diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 7c9c0cf..ce2fece 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -27,7 +27,7 @@ create_minimal_publisher, create_monitor_node, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE + MONITOR_NODE_NAMESPACE, ) import launch import launch_testing From e78aca32e37f23d6aaf3923eb0ea5d0dfb4b5bab Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 11:25:23 -0800 Subject: [PATCH 04/11] Fix cpplint Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 86183d9..b15da24 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -67,13 +67,11 @@ TopicParamInfo parse_topic_param_name(const std::string & param_name) const size_t tol_suffix_len = strlen(kTolSuffix); if (topic_and_field.length() > freq_suffix_len && - topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) - { + topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) { info.topic_name = topic_and_field.substr(0, topic_and_field.length() - freq_suffix_len); info.field = TopicParamField::kFrequency; } else if (topic_and_field.length() > tol_suffix_len && - topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) - { + topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) { info.topic_name = topic_and_field.substr(0, topic_and_field.length() - tol_suffix_len); info.field = TopicParamField::kTolerance; } From b891a6b8fb11a69f04b109e97b20dada04c7fc16 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 13:00:25 -0800 Subject: [PATCH 05/11] Fix lint Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 16 +++-- .../test/parameters/test_param_dynamic.py | 65 ++++++++++++------- 2 files changed, 52 insertions(+), 29 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index b15da24..20dd4ff 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -65,14 +65,18 @@ TopicParamInfo parse_topic_param_name(const std::string & param_name) const size_t freq_suffix_len = strlen(kFreqSuffix); const size_t tol_suffix_len = strlen(kTolSuffix); + const size_t len = topic_and_field.length(); - if (topic_and_field.length() > freq_suffix_len && - topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) { - info.topic_name = topic_and_field.substr(0, topic_and_field.length() - freq_suffix_len); + bool is_freq = len > freq_suffix_len && + topic_and_field.rfind(kFreqSuffix) == len - freq_suffix_len; + bool is_tol = len > tol_suffix_len && + topic_and_field.rfind(kTolSuffix) == len - tol_suffix_len; + + if (is_freq) { + info.topic_name = topic_and_field.substr(0, len - freq_suffix_len); info.field = TopicParamField::kFrequency; - } else if (topic_and_field.length() > tol_suffix_len && - topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) { - info.topic_name = topic_and_field.substr(0, topic_and_field.length() - tol_suffix_len); + } else if (is_tol) { + info.topic_name = topic_and_field.substr(0, len - tol_suffix_len); info.field = TopicParamField::kTolerance; } diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index c625519..4739edb 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -41,6 +41,8 @@ TEST_TOPIC = '/dynamic_param_topic' TEST_FREQUENCY = 30.0 +TEST_TOLERANCE = 20.0 +NONEXISTENT_TOPIC = '/topic_that_does_not_exist' @pytest.mark.launch_test @@ -82,6 +84,15 @@ def test_set_expected_frequency_via_param(self): """Test setting expected frequency via ros2 param set.""" time.sleep(2.0) + # Verify topic is not monitored before setting frequency + initial_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(initial_diagnostics), 0, + f'{TEST_TOPIC} should not be monitored before setting frequency' + ) + freq_param = make_freq_param(TEST_TOPIC) success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') @@ -97,12 +108,20 @@ def test_set_expected_frequency_via_param(self): 'Expected diagnostics after setting frequency param' ) + # Verify parameter value + success, actual_freq = get_parameter(self.test_node, freq_param) + self.assertTrue(success, f'Failed to get {freq_param}') + self.assertAlmostEqual( + actual_freq, TEST_FREQUENCY, places=1, + msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' + ) + def test_change_tolerance_via_param(self): """Test changing tolerance via ros2 param set.""" time.sleep(1.0) tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter(self.test_node, tol_param, 20.0) + success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) self.assertTrue(success, f'Failed to set {tol_param}') time.sleep(0.5) @@ -116,37 +135,37 @@ def test_change_tolerance_via_param(self): 'Topic should still be monitored after tolerance change' ) - def test_verify_params_with_get(self): - """Test that ros2 param get returns the values we set.""" - time.sleep(1.0) + # Verify parameter value + success, actual_tol = get_parameter(self.test_node, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, TEST_TOLERANCE, places=1, + msg=f'Tolerance mismatch: expected {TEST_TOLERANCE}, got {actual_tol}' + ) - # Set specific values - freq_param = make_freq_param(TEST_TOPIC) - tol_param = make_tol_param(TEST_TOPIC) - expected_freq = 42.5 - expected_tol = 15.0 + def test_set_frequency_for_nonexistent_topic(self): + """Test setting expected frequency for a topic that does not exist.""" + time.sleep(1.0) - success = set_parameter(self.test_node, freq_param, expected_freq) + freq_param = make_freq_param(NONEXISTENT_TOPIC) + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') - success = set_parameter(self.test_node, tol_param, expected_tol) - self.assertTrue(success, f'Failed to set {tol_param}') - - time.sleep(0.5) - - # Verify with get_parameter + # Verify parameter was set success, actual_freq = get_parameter(self.test_node, freq_param) self.assertTrue(success, f'Failed to get {freq_param}') self.assertAlmostEqual( - actual_freq, expected_freq, places=1, - msg=f'Frequency mismatch: expected {expected_freq}, got {actual_freq}' + actual_freq, TEST_FREQUENCY, places=1, + msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' ) - success, actual_tol = get_parameter(self.test_node, tol_param) - self.assertTrue(success, f'Failed to get {tol_param}') - self.assertAlmostEqual( - actual_tol, expected_tol, places=1, - msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' + # Topic should not appear in diagnostics since it doesn't exist + diagnostics = collect_diagnostics_for_topic( + self.test_node, NONEXISTENT_TOPIC, expected_count=1, timeout_sec=3.0 + ) + self.assertEqual( + len(diagnostics), 0, + f'{NONEXISTENT_TOPIC} should not appear in diagnostics' ) From eb488e384f39e62e4daebb7f33f33fc2443814d7 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 14:01:02 -0800 Subject: [PATCH 06/11] Fix tests Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 18 +--- .../include/greenwave_monitor.hpp | 6 ++ greenwave_monitor/src/greenwave_monitor.cpp | 15 ++++ .../test/parameters/test_param_dynamic.py | 88 ++++++++++++------- .../test/parameters/test_param_freq_only.py | 9 +- .../test/parameters/test_param_yaml.py | 16 ++-- .../test/parameters/test_topic_parameters.py | 29 ++---- 7 files changed, 100 insertions(+), 81 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 1155357..e9c4324 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -65,7 +65,8 @@ def make_tol_param(topic: str) -> str: def set_parameter(test_node: Node, param_name: str, value: float, - node_name: str = MONITOR_NODE_NAME) -> bool: + node_name: str = MONITOR_NODE_NAME, + timeout_sec: float = 10.0) -> bool: """Set a parameter on the monitor node using rclpy service client.""" full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' service_name = f'{full_node_name}/set_parameters' @@ -84,7 +85,7 @@ def set_parameter(test_node: Node, param_name: str, value: float, request.parameters = [param] future = client.call_async(request) - rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0) + rclpy.spin_until_future_complete(test_node, future, timeout_sec=timeout_sec) test_node.destroy_client(client) @@ -122,19 +123,6 @@ def get_parameter(test_node: Node, param_name: str, return False, None -def has_valid_frame_rate(diagnostics: List[DiagnosticStatus]) -> bool: - """Check if any diagnostic has a valid (positive) frame_rate_node value.""" - for status in diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - return True - except ValueError: - continue - return False - - def create_minimal_publisher( topic: str, frequency_hz: float, message_type: str, id_suffix: str = ''): """Create a minimal publisher node with the given parameters.""" diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index c736639..496fe9e 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -109,4 +110,9 @@ class GreenwaveMonitor : public rclcpp::Node std::map pending_topic_configs_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // Mutex protecting message_diagnostics_, subscriptions_, and pending_topic_configs_ + mutable std::mutex state_mutex_; + // Flag to skip parameter callback when updating params internally (avoids redundant work) + bool updating_params_internally_ = false; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 20dd4ff..0e4a005 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -192,11 +192,13 @@ void GreenwaveMonitor::topic_callback( const std::string & topic, const std::string & type) { auto msg_timestamp = GetTimestampFromSerializedMessage(msg, type); + std::lock_guard lock(state_mutex_); message_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); } void GreenwaveMonitor::timer_callback() { + std::lock_guard lock(state_mutex_); RCLCPP_INFO(this->get_logger(), "===================================================="); if (message_diagnostics_.empty()) { RCLCPP_INFO(this->get_logger(), "No topics to monitor"); @@ -217,6 +219,7 @@ void GreenwaveMonitor::handle_manage_topic( const std::shared_ptr request, std::shared_ptr response) { + std::lock_guard lock(state_mutex_); if (request->add_topic) { response->success = add_topic(request->topic_name, response->message); } else { @@ -228,6 +231,7 @@ void GreenwaveMonitor::handle_set_expected_frequency( const std::shared_ptr request, std::shared_ptr response) { + std::lock_guard lock(state_mutex_); auto it = message_diagnostics_.find(request->topic_name); if (it == message_diagnostics_.end()) { @@ -300,8 +304,10 @@ bool GreenwaveMonitor::set_topic_expected_frequency( // Sync parameters with the new values if (update_parameters) { + updating_params_internally_ = true; declare_or_set_parameter(make_freq_param_name(topic_name), expected_hz); declare_or_set_parameter(make_tol_param_name(topic_name), tolerance_percent); + updating_params_internally_ = false; } message = "Successfully set expected frequency for topic '" + @@ -316,6 +322,13 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( rcl_interfaces::msg::SetParametersResult result; result.successful = true; + // Skip if updating from within the node (avoids redundant work and deadlock) + if (updating_params_internally_) { + return result; + } + + std::lock_guard lock(state_mutex_); + for (const auto & param : parameters) { auto info = parse_topic_param_name(param.get_name()); if (info.field == TopicParamField::kNone || info.topic_name.empty()) { @@ -403,6 +416,8 @@ void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_ void GreenwaveMonitor::load_topic_parameters_from_overrides() { + std::lock_guard lock(state_mutex_); + // Parameters are automatically declared from overrides due to NodeOptions setting. // List all parameters and filter by prefix manually (list_parameters prefix matching // can be unreliable with deeply nested parameter names). diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 4739edb..7b7f2a0 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -48,9 +48,7 @@ @pytest.mark.launch_test def generate_test_description(): """Test dynamic parameter changes via ros2 param set.""" - ros2_monitor_node = create_monitor_node( - topics=[TEST_TOPIC] # Topic exists but no expected frequency - ) + ros2_monitor_node = create_monitor_node() publisher = create_minimal_publisher( TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' @@ -80,35 +78,46 @@ def tearDownClass(cls): cls.test_node.destroy_node() rclpy.shutdown() - def test_set_expected_frequency_via_param(self): - """Test setting expected frequency via ros2 param set.""" + def test_set_parameters(self): + """Test setting frequency and tolerance parameters in sequence.""" time.sleep(2.0) - # Verify topic is not monitored before setting frequency + freq_param = make_freq_param(TEST_TOPIC) + tol_param = make_tol_param(TEST_TOPIC) + + # 1. Verify topic is not monitored initially initial_diagnostics = collect_diagnostics_for_topic( self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 ) self.assertEqual( len(initial_diagnostics), 0, - f'{TEST_TOPIC} should not be monitored before setting frequency' + f'{TEST_TOPIC} should not be monitored initially' ) - freq_param = make_freq_param(TEST_TOPIC) - success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) - self.assertTrue(success, f'Failed to set {freq_param}') - - time.sleep(1.0) + # 2. Set tolerance before frequency - topic should remain unmonitored + success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) + self.assertTrue(success, f'Failed to set {tol_param}') - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + success, actual_tol = get_parameter(self.test_node, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, TEST_TOLERANCE, places=1, + msg=f'Tolerance mismatch: expected {TEST_TOLERANCE}, got {actual_tol}' ) - self.assertGreaterEqual( - len(received_diagnostics), 3, - 'Expected diagnostics after setting frequency param' + time.sleep(1.0) + diagnostics_after_tol = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(diagnostics_after_tol), 0, + f'{TEST_TOPIC} should remain unmonitored after setting only tolerance' ) - # Verify parameter value + # 3. Set frequency - topic should become monitored + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + success, actual_freq = get_parameter(self.test_node, freq_param) self.assertTrue(success, f'Failed to get {freq_param}') self.assertAlmostEqual( @@ -116,31 +125,42 @@ def test_set_expected_frequency_via_param(self): msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' ) - def test_change_tolerance_via_param(self): - """Test changing tolerance via ros2 param set.""" time.sleep(1.0) + diagnostics_after_freq = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics_after_freq), 3, + 'Expected diagnostics after setting frequency param' + ) - tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) - self.assertTrue(success, f'Failed to set {tol_param}') + # 4. Set tolerance to 0.0 - should cause diagnostics to show error + success = set_parameter(self.test_node, tol_param, 0.0) + self.assertTrue(success, f'Failed to set {tol_param} to 0.0') - time.sleep(0.5) + success, actual_tol = get_parameter(self.test_node, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, 0.0, places=1, + msg=f'Tolerance mismatch: expected 0.0, got {actual_tol}' + ) - received_diagnostics = collect_diagnostics_for_topic( + time.sleep(2.0) + diagnostics_with_zero_tol = collect_diagnostics_for_topic( self.test_node, TEST_TOPIC, expected_count=2, timeout_sec=5.0 ) - self.assertGreaterEqual( - len(received_diagnostics), 2, - 'Topic should still be monitored after tolerance change' + len(diagnostics_with_zero_tol), 2, + 'Topic should still be monitored with zero tolerance' ) - # Verify parameter value - success, actual_tol = get_parameter(self.test_node, tol_param) - self.assertTrue(success, f'Failed to get {tol_param}') - self.assertAlmostEqual( - actual_tol, TEST_TOLERANCE, places=1, - msg=f'Tolerance mismatch: expected {TEST_TOLERANCE}, got {actual_tol}' + # Check that at least one diagnostic has ERROR level (frequency outside 0% tolerance) + has_error = any( + d.level != 0 for d in diagnostics_with_zero_tol + ) + self.assertTrue( + has_error, + 'Expected ERROR diagnostics with 0% tolerance' ) def test_set_frequency_for_nonexistent_topic(self): diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index c770b92..beb22cc 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -26,7 +26,7 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - has_valid_frame_rate, + find_best_diagnostic, MONITOR_NODE_NAMESPACE, ) import launch @@ -94,8 +94,11 @@ def test_frequency_only_uses_default_tolerance(self): len(received_diagnostics), 3, f'Expected at least 3 diagnostics, got {len(received_diagnostics)}' ) - self.assertTrue( - has_valid_frame_rate(received_diagnostics), + best_status, _ = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, 'Should have valid frame rate with default tolerance' ) diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index b295d45..4c3d47d 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -27,7 +27,7 @@ from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, - has_valid_frame_rate, + find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, ) @@ -123,8 +123,11 @@ def test_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from YAML-configured topic' ) - self.assertTrue( - has_valid_frame_rate(received_diagnostics), + best_status, _ = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, 'Should have valid frame rate from YAML config' ) @@ -140,8 +143,11 @@ def test_nested_dict_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from nested YAML-configured topic' ) - self.assertTrue( - has_valid_frame_rate(received_diagnostics), + best_status, _ = find_best_diagnostic( + received_diagnostics, NESTED_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, 'Should have valid frame rate from nested YAML config' ) diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index ce2fece..5b1e4a6 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -26,6 +26,7 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, + find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, ) @@ -120,34 +121,14 @@ def test_topic_configured_via_parameters(self): len(received_diagnostics), 3, f'Expected at least 3 diagnostics for {TEST_TOPIC}, got {len(received_diagnostics)}' ) - - # Verify valid frame rate exists - best_status = None - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - frame_rate = float(kv.value) - if frame_rate > 0: - best_status = status - break - except ValueError: - continue - if best_status: - break - + best_status, best_values = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) self.assertIsNotNone( best_status, 'Should have received diagnostics with valid frame_rate_node' ) - - frame_rate_node = None - for kv in best_status.values: - if kv.key == 'frame_rate_node': - frame_rate_node = float(kv.value) - break - - self.assertIsNotNone(frame_rate_node) + frame_rate_node = best_values[0] tolerance = TEST_FREQUENCY * 0.5 self.assertAlmostEqual( frame_rate_node, TEST_FREQUENCY, delta=tolerance, From 4fdc70cc90168a91a6a71baef16ef1e143571c10 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 15:16:02 -0800 Subject: [PATCH 07/11] Don't maintain pending_topic_configs_ Signed-off-by: Blake McHale --- .../include/greenwave_monitor.hpp | 6 +- greenwave_monitor/src/greenwave_monitor.cpp | 87 ++++++++----------- 2 files changed, 38 insertions(+), 55 deletions(-) diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 496fe9e..e28d3f6 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -76,7 +75,7 @@ class GreenwaveMonitor : public rclcpp::Node rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & parameters); - void apply_topic_config_if_complete(const std::string & topic_name); + void apply_topic_config(const std::string & topic_name, const TopicConfig & incoming); void load_topic_parameters_from_overrides(); @@ -108,11 +107,8 @@ class GreenwaveMonitor : public rclcpp::Node rclcpp::Service::SharedPtr set_expected_frequency_service_; - std::map pending_topic_configs_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - // Mutex protecting message_diagnostics_, subscriptions_, and pending_topic_configs_ - mutable std::mutex state_mutex_; // Flag to skip parameter callback when updating params internally (avoids redundant work) bool updating_params_internally_ = false; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 0e4a005..7716826 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -192,13 +192,11 @@ void GreenwaveMonitor::topic_callback( const std::string & topic, const std::string & type) { auto msg_timestamp = GetTimestampFromSerializedMessage(msg, type); - std::lock_guard lock(state_mutex_); message_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); } void GreenwaveMonitor::timer_callback() { - std::lock_guard lock(state_mutex_); RCLCPP_INFO(this->get_logger(), "===================================================="); if (message_diagnostics_.empty()) { RCLCPP_INFO(this->get_logger(), "No topics to monitor"); @@ -219,7 +217,6 @@ void GreenwaveMonitor::handle_manage_topic( const std::shared_ptr request, std::shared_ptr response) { - std::lock_guard lock(state_mutex_); if (request->add_topic) { response->success = add_topic(request->topic_name, response->message); } else { @@ -231,7 +228,6 @@ void GreenwaveMonitor::handle_set_expected_frequency( const std::shared_ptr request, std::shared_ptr response) { - std::lock_guard lock(state_mutex_); auto it = message_diagnostics_.find(request->topic_name); if (it == message_diagnostics_.end()) { @@ -327,7 +323,8 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( return result; } - std::lock_guard lock(state_mutex_); + // Build a local map of incoming topic configs from this callback + std::map incoming_configs; for (const auto & param : parameters) { auto info = parse_topic_param_name(param.get_name()); @@ -345,7 +342,7 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( } double value = value_opt.value(); - TopicConfig & config = pending_topic_configs_[info.topic_name]; + TopicConfig & config = incoming_configs[info.topic_name]; if (info.field == TopicParamField::kFrequency) { config.expected_frequency = value; @@ -357,38 +354,35 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( this->get_logger(), "Parameter set: %s for topic '%s' = %.2f %s", get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); + } - apply_topic_config_if_complete(info.topic_name); + // Apply configs for each topic affected by this parameter change + for (const auto & [topic_name, incoming] : incoming_configs) { + apply_topic_config(topic_name, incoming); } return result; } -void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_name) +void GreenwaveMonitor::apply_topic_config( + const std::string & topic_name, const TopicConfig & incoming) { - auto it = pending_topic_configs_.find(topic_name); - if (it == pending_topic_configs_.end()) { - return; - } - - const TopicConfig & config = it->second; - - // Get expected frequency from pending config or existing parameter + // Get expected frequency: prefer incoming, fall back to existing parameter double expected_freq = 0.0; - if (config.expected_frequency.has_value()) { - expected_freq = config.expected_frequency.value(); + if (incoming.expected_frequency.has_value()) { + expected_freq = incoming.expected_frequency.value(); } else { auto freq_opt = get_numeric_parameter(make_freq_param_name(topic_name)); if (freq_opt.has_value()) { expected_freq = freq_opt.value(); } else { - // No frequency available, nothing to do + // No frequency available yet, nothing to do return; } } - // Get tolerance from pending config, existing parameter, or default - double tolerance = config.tolerance.value_or( + // Get tolerance: prefer incoming, then existing parameter, then default + double tolerance = incoming.tolerance.value_or( get_numeric_parameter(make_tol_param_name(topic_name)).value_or(kDefaultTolerancePercent) ); @@ -410,20 +404,19 @@ void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_ "Use manage_topic service to add the topic first.", topic_name.c_str(), message.c_str()); } - - pending_topic_configs_.erase(it); } void GreenwaveMonitor::load_topic_parameters_from_overrides() { - std::lock_guard lock(state_mutex_); - // Parameters are automatically declared from overrides due to NodeOptions setting. // List all parameters and filter by prefix manually (list_parameters prefix matching // can be unreliable with deeply nested parameter names). auto all_params = this->list_parameters( {}, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); + // Build a local map of topic configs from startup parameters + std::map startup_configs; + for (const auto & name : all_params.names) { auto info = parse_topic_param_name(name); if (info.field == TopicParamField::kNone || info.topic_name.empty()) { @@ -436,7 +429,7 @@ void GreenwaveMonitor::load_topic_parameters_from_overrides() } double value = value_opt.value(); - TopicConfig & config = pending_topic_configs_[info.topic_name]; + TopicConfig & config = startup_configs[info.topic_name]; if (info.field == TopicParamField::kFrequency) { config.expected_frequency = value; @@ -450,32 +443,26 @@ void GreenwaveMonitor::load_topic_parameters_from_overrides() get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); } - // Apply all complete configs - at startup we can add topics if needed - std::vector topics_to_apply; - for (const auto & [topic, config] : pending_topic_configs_) { + // Apply all configs that have frequency set + for (const auto & [topic, config] : startup_configs) { if (config.expected_frequency.has_value()) { - topics_to_apply.push_back(topic); - } - } - for (const auto & topic : topics_to_apply) { - const TopicConfig & config = pending_topic_configs_[topic]; - double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); - - std::string message; - bool success = set_topic_expected_frequency( - topic, - config.expected_frequency.value(), - tolerance, - true, // add topic if missing - safe at startup - message, - false); // don't update parameters - - if (success) { - RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); - } else { - RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); + double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); + + std::string message; + bool success = set_topic_expected_frequency( + topic, + config.expected_frequency.value(), + tolerance, + true, // add topic if missing - safe at startup + message, + false); // don't update parameters + + if (success) { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } else { + RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); + } } - pending_topic_configs_.erase(topic); } } From f715325f0cbfc09a010823030d8b70fb6d162be2 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 15:50:06 -0800 Subject: [PATCH 08/11] Use same base class for RosNodeTestCase Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 36 +++++++++++++++++++ .../test/parameters/test_param_dynamic.py | 19 ++-------- .../test/parameters/test_param_freq_only.py | 19 ++-------- .../test/parameters/test_param_new_topic.py | 19 ++-------- .../test/parameters/test_param_tol_only.py | 18 ++-------- .../test/parameters/test_param_yaml.py | 18 ++-------- .../test/parameters/test_topic_parameters.py | 31 ++++------------ 7 files changed, 57 insertions(+), 103 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index e9c4324..20ec372 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -17,9 +17,11 @@ # # SPDX-License-Identifier: Apache-2.0 +from abc import ABC import math import time from typing import List, Optional, Tuple +import unittest from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from greenwave_monitor.ui_adaptor import ( @@ -367,3 +369,37 @@ def create_service_clients(node: Node, namespace: str = MONITOR_NODE_NAMESPACE, ) return manage_topic_client, set_frequency_client + + +class RosNodeTestCase(unittest.TestCase, ABC): + """ + Abstract base class for ROS 2 launch tests that need a test node. + + Subclasses must define the TEST_NODE_NAME class attribute to specify + the unique name for the test node. + + Example: + class TestMyFeature(RosNodeTestCase): + TEST_NODE_NAME = 'my_feature_test_node' + + def test_something(self): + # self.test_node is available + ... + """ + + TEST_NODE_NAME: str = None + + @classmethod + def setUpClass(cls): + """Initialize ROS 2 and create test node.""" + if cls.TEST_NODE_NAME is None: + raise ValueError( + f'{cls.__name__} must define TEST_NODE_NAME class attribute') + rclpy.init() + cls.test_node = Node(cls.TEST_NODE_NAME, namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS 2.""" + cls.test_node.destroy_node() + rclpy.shutdown() diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 7b7f2a0..891fa17 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -20,7 +20,6 @@ """Test: dynamic parameter changes via ros2 param set.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -29,14 +28,12 @@ get_parameter, make_freq_param, make_tol_param, - MONITOR_NODE_NAMESPACE, + RosNodeTestCase, set_parameter, ) import launch import launch_testing import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/dynamic_param_topic' @@ -63,20 +60,10 @@ def generate_test_description(): ) -class TestDynamicParameterChanges(unittest.TestCase): +class TestDynamicParameterChanges(RosNodeTestCase): """Test changing parameters dynamically via ros2 param set.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('dynamic_param_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'dynamic_param_test_node' def test_set_parameters(self): """Test setting frequency and tolerance parameters in sequence.""" diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index beb22cc..0744c1b 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -20,20 +20,17 @@ """Test: only expected_frequency specified, tolerance defaults to 5%.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, find_best_diagnostic, - MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_testing import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/freq_only_topic' @@ -67,20 +64,10 @@ def generate_test_description(): ) -class TestFrequencyOnlyParameter(unittest.TestCase): +class TestFrequencyOnlyParameter(RosNodeTestCase): """Test that only specifying frequency works (tolerance defaults).""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('freq_only_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'freq_only_test_node' def test_frequency_only_uses_default_tolerance(self): """Test that specifying only frequency uses default tolerance.""" diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 297a448..b5a8233 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -20,21 +20,18 @@ """Test: add new topic to monitoring via ros2 param set.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, make_freq_param, - MONITOR_NODE_NAMESPACE, + RosNodeTestCase, set_parameter, ) import launch import launch_testing import pytest -import rclpy -from rclpy.node import Node NEW_TOPIC = '/new_dynamic_topic' @@ -59,20 +56,10 @@ def generate_test_description(): ) -class TestAddNewTopicViaParam(unittest.TestCase): +class TestAddNewTopicViaParam(RosNodeTestCase): """Test adding a new topic to monitoring via ros2 param set.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('new_topic_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'new_topic_test_node' def test_add_new_topic_via_frequency_param(self): """Test that setting frequency param for new topic starts monitoring.""" diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index 4e87d9d..8060890 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -20,7 +20,6 @@ """Test: only tolerance specified - should NOT start monitoring.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -28,13 +27,12 @@ make_tol_param, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_ros.actions import launch_testing import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/tol_only_topic' @@ -70,20 +68,10 @@ def generate_test_description(): ) -class TestToleranceOnlyParameter(unittest.TestCase): +class TestToleranceOnlyParameter(RosNodeTestCase): """Test that only specifying tolerance does NOT start monitoring.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('tol_only_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'tol_only_test_node' def test_tolerance_only_does_not_monitor(self): """Test that specifying only tolerance does not start monitoring.""" diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 4c3d47d..630895e 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -22,7 +22,6 @@ import os import tempfile import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -30,13 +29,12 @@ find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_ros.actions import launch_testing import pytest -import rclpy -from rclpy.node import Node YAML_TOPIC = '/yaml_config_topic' @@ -96,20 +94,10 @@ def generate_test_description(): ) -class TestYamlParameterFile(unittest.TestCase): +class TestYamlParameterFile(RosNodeTestCase): """Test loading topic configuration from YAML parameter file.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('yaml_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'yaml_test_node' def test_topic_configured_via_yaml(self): """Test that topic is monitored when configured via YAML file.""" diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 5b1e4a6..0995bec 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -29,6 +29,7 @@ find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_testing @@ -72,20 +73,10 @@ def generate_test_description(): @post_shutdown_test() -class TestTopicParametersPostShutdown(unittest.TestCase): +class TestTopicParametersPostShutdown(RosNodeTestCase): """Post-shutdown tests.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('shutdown_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'shutdown_test_node' def test_node_shutdown(self, proc_info): """Test that the node shuts down correctly.""" @@ -94,20 +85,10 @@ def test_node_shutdown(self, proc_info): assertExitCodes(proc_info, allowable_exit_codes=[0]) -class TestTopicParameters(unittest.TestCase): +class TestTopicParameters(RosNodeTestCase): """Tests for parameter-based topic configuration.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('topic_params_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'topic_params_test_node' def test_topic_configured_via_parameters(self): """Test that topic is monitored when configured via parameters.""" @@ -129,7 +110,7 @@ def test_topic_configured_via_parameters(self): 'Should have received diagnostics with valid frame_rate_node' ) frame_rate_node = best_values[0] - tolerance = TEST_FREQUENCY * 0.5 + tolerance = TEST_FREQUENCY * TEST_TOLERANCE / 100.0 self.assertAlmostEqual( frame_rate_node, TEST_FREQUENCY, delta=tolerance, msg=f'Frame rate {frame_rate_node} not within {tolerance} of {TEST_FREQUENCY}' From 31e91480d1a3eab14fb9811d018c1d0342cfa5ef Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 15:59:00 -0800 Subject: [PATCH 09/11] Update copyrights Signed-off-by: Blake McHale --- greenwave_monitor/examples/example.launch.py | 2 +- greenwave_monitor/greenwave_monitor/test_utils.py | 2 +- greenwave_monitor/greenwave_monitor/ui_adaptor.py | 2 +- greenwave_monitor/include/greenwave_monitor.hpp | 2 +- greenwave_monitor/src/greenwave_monitor.cpp | 2 +- greenwave_monitor/test/parameters/test_param_dynamic.py | 2 +- greenwave_monitor/test/parameters/test_param_freq_only.py | 2 +- greenwave_monitor/test/parameters/test_param_new_topic.py | 2 +- greenwave_monitor/test/parameters/test_param_tol_only.py | 2 +- greenwave_monitor/test/parameters/test_param_yaml.py | 2 +- greenwave_monitor/test/parameters/test_topic_parameters.py | 2 +- 11 files changed, 11 insertions(+), 11 deletions(-) diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index c84b13a..92bb6d2 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. diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 20ec372..e351908 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2024-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. diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 2e7ba8d..09f923c 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# 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. diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index e28d3f6..ce854fb 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// 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. diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 7716826..5c202fc 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// 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. diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 891fa17..74565ff 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# 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. diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index 0744c1b..7e6751e 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# 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. diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index b5a8233..2a8c0fe 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# 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. diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index 8060890..e05ebb8 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# 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. diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 630895e..3cf9c51 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# 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. diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 0995bec..d3f210b 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# 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. From 88ddcd24ddf95341d7a2e1bf40307a255ccea3c2 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 16:31:13 -0800 Subject: [PATCH 10/11] More tests and linting Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 8 -- .../test/parameters/test_param_dynamic.py | 56 +++++++- .../test/parameters/test_param_freq_only.py | 1 + .../parameters/test_param_multiple_topics.py | 124 ++++++++++++++++++ .../test/parameters/test_param_new_topic.py | 1 + .../test/parameters/test_param_tol_only.py | 1 + .../test/parameters/test_param_yaml.py | 1 + .../test/parameters/test_topic_parameters.py | 3 - 8 files changed, 183 insertions(+), 12 deletions(-) create mode 100644 greenwave_monitor/test/parameters/test_param_multiple_topics.py diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index e351908..dc3c274 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -377,14 +377,6 @@ class RosNodeTestCase(unittest.TestCase, ABC): Subclasses must define the TEST_NODE_NAME class attribute to specify the unique name for the test node. - - Example: - class TestMyFeature(RosNodeTestCase): - TEST_NODE_NAME = 'my_feature_test_node' - - def test_something(self): - # self.test_node is available - ... """ TEST_NODE_NAME: str = None diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 74565ff..6e157c5 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -20,6 +20,7 @@ """Test: dynamic parameter changes via ros2 param set.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -143,13 +144,66 @@ def test_set_parameters(self): # Check that at least one diagnostic has ERROR level (frequency outside 0% tolerance) has_error = any( - d.level != 0 for d in diagnostics_with_zero_tol + ord(d.level) != 0 for d in diagnostics_with_zero_tol ) self.assertTrue( has_error, 'Expected ERROR diagnostics with 0% tolerance' ) + # Reset tolerance to 10% - should no longer error + success = set_parameter(self.test_node, tol_param, 10.0) + self.assertTrue(success, f'Failed to reset {tol_param}') + + # Wait for diagnostics to stabilize after tolerance change + time.sleep(3.0) + diagnostics_after_reset = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics_after_reset), 3, + 'Expected diagnostics after resetting tolerance' + ) + + # Verify most recent diagnostic is OK after resetting tolerance + last_diagnostic = diagnostics_after_reset[-1] + self.assertEqual( + ord(last_diagnostic.level), 0, + 'Expected OK diagnostic after resetting tolerance to 10%' + ) + + # 5. Update expected frequency to mismatched value - should cause error + # Publisher is still at 30 Hz, tolerance is 10%, but we set expected to 1 Hz + mismatched_frequency = 1.0 + success = set_parameter(self.test_node, freq_param, mismatched_frequency) + self.assertTrue(success, f'Failed to update {freq_param}') + + success, actual_freq = get_parameter(self.test_node, freq_param) + self.assertTrue(success, f'Failed to get updated {freq_param}') + self.assertAlmostEqual( + actual_freq, mismatched_frequency, places=1, + msg=f'Frequency mismatch: expected {mismatched_frequency}, got {actual_freq}' + ) + + time.sleep(2.0) + diagnostics_mismatched = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics_mismatched), 3, + 'Should still receive diagnostics after frequency update' + ) + + # Verify diagnostics show error due to frequency mismatch + has_error = any( + ord(d.level) != 0 for d in diagnostics_mismatched + ) + self.assertTrue( + has_error, + 'Expected ERROR diagnostics when actual frequency (30 Hz) ' + 'does not match expected (1 Hz)' + ) + def test_set_frequency_for_nonexistent_topic(self): """Test setting expected frequency for a topic that does not exist.""" time.sleep(1.0) diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index 7e6751e..578f427 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -20,6 +20,7 @@ """Test: only expected_frequency specified, tolerance defaults to 5%.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_param_multiple_topics.py b/greenwave_monitor/test/parameters/test_param_multiple_topics.py new file mode 100644 index 0000000..d9a93a0 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_multiple_topics.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 + +# 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 + +"""Test: multiple topics configured via parameters at startup.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + find_best_diagnostic, + RosNodeTestCase, +) +import launch +import launch_testing +import pytest + + +TOPIC_1 = '/multi_topic_1' +TOPIC_2 = '/multi_topic_2' +TOPIC_3 = '/multi_topic_3' +FREQUENCY_1 = 10.0 +FREQUENCY_2 = 25.0 +FREQUENCY_3 = 50.0 +TOLERANCE = 20.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test multiple topics configured via parameters.""" + topic_configs = { + TOPIC_1: { + 'expected_frequency': FREQUENCY_1, + 'tolerance': TOLERANCE + }, + TOPIC_2: { + 'expected_frequency': FREQUENCY_2, + 'tolerance': TOLERANCE + }, + TOPIC_3: { + 'expected_frequency': FREQUENCY_3, + 'tolerance': TOLERANCE + } + } + + ros2_monitor_node = create_monitor_node(topic_configs=topic_configs) + + publisher_1 = create_minimal_publisher(TOPIC_1, FREQUENCY_1, 'imu', '_multi_1') + publisher_2 = create_minimal_publisher(TOPIC_2, FREQUENCY_2, 'imu', '_multi_2') + publisher_3 = create_minimal_publisher(TOPIC_3, FREQUENCY_3, 'imu', '_multi_3') + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher_1, + publisher_2, + publisher_3, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestMultipleTopicsViaParameters(RosNodeTestCase): + """Test that multiple topics can be configured via parameters.""" + + TEST_NODE_NAME = 'multiple_topics_test_node' + + def test_all_topics_monitored(self): + """Test that all configured topics are monitored.""" + time.sleep(2.0) + + topics_to_check = [ + (TOPIC_1, FREQUENCY_1), + (TOPIC_2, FREQUENCY_2), + (TOPIC_3, FREQUENCY_3), + ] + + for topic, expected_freq in topics_to_check: + with self.subTest(topic=topic): + diagnostics = collect_diagnostics_for_topic( + self.test_node, topic, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics), 3, + f'Expected at least 3 diagnostics for {topic}' + ) + + best_status, best_values = find_best_diagnostic( + diagnostics, expected_freq, 'imu' + ) + self.assertIsNotNone( + best_status, + f'Should have valid diagnostics for {topic}' + ) + + frame_rate = best_values[0] + tolerance_hz = expected_freq * TOLERANCE / 100.0 + self.assertAlmostEqual( + frame_rate, expected_freq, delta=tolerance_hz, + msg=f'{topic}: frame rate {frame_rate} not within ' + f'{tolerance_hz} of expected {expected_freq}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 2a8c0fe..7a0048b 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -20,6 +20,7 @@ """Test: add new topic to monitoring via ros2 param set.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index e05ebb8..a90a634 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -20,6 +20,7 @@ """Test: only tolerance specified - should NOT start monitoring.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 3cf9c51..3a7c049 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -22,6 +22,7 @@ import os import tempfile import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index d3f210b..4901635 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -28,7 +28,6 @@ create_monitor_node, find_best_diagnostic, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE, RosNodeTestCase, ) import launch @@ -36,8 +35,6 @@ from launch_testing import post_shutdown_test from launch_testing.asserts import assertExitCodes import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/param_test_topic' From ad2aacb6fd01860db384111f756da42503ed87d3 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 16:49:31 -0800 Subject: [PATCH 11/11] Cover when topics is specified alongside its subfields Signed-off-by: Blake McHale --- .../parameters/test_param_multiple_topics.py | 63 ++++++++++++++++++- 1 file changed, 62 insertions(+), 1 deletion(-) diff --git a/greenwave_monitor/test/parameters/test_param_multiple_topics.py b/greenwave_monitor/test/parameters/test_param_multiple_topics.py index d9a93a0..b351b33 100644 --- a/greenwave_monitor/test/parameters/test_param_multiple_topics.py +++ b/greenwave_monitor/test/parameters/test_param_multiple_topics.py @@ -34,6 +34,7 @@ import pytest +# Topics with expected frequencies configured TOPIC_1 = '/multi_topic_1' TOPIC_2 = '/multi_topic_2' TOPIC_3 = '/multi_topic_3' @@ -42,10 +43,16 @@ FREQUENCY_3 = 50.0 TOLERANCE = 20.0 +# Topics specified as list only (no expected frequency) +TOPIC_LIST_1 = '/multi_topic_list_1' +TOPIC_LIST_2 = '/multi_topic_list_2' +LIST_PUBLISHER_FREQ = 30.0 + @pytest.mark.launch_test def generate_test_description(): """Test multiple topics configured via parameters.""" + # Topics with frequency/tolerance configs topic_configs = { TOPIC_1: { 'expected_frequency': FREQUENCY_1, @@ -61,11 +68,22 @@ def generate_test_description(): } } - ros2_monitor_node = create_monitor_node(topic_configs=topic_configs) + # Also include topics specified as simple list (no frequencies) + topics_list = [TOPIC_LIST_1, TOPIC_LIST_2] + + ros2_monitor_node = create_monitor_node( + topics=topics_list, + topic_configs=topic_configs + ) publisher_1 = create_minimal_publisher(TOPIC_1, FREQUENCY_1, 'imu', '_multi_1') publisher_2 = create_minimal_publisher(TOPIC_2, FREQUENCY_2, 'imu', '_multi_2') publisher_3 = create_minimal_publisher(TOPIC_3, FREQUENCY_3, 'imu', '_multi_3') + # Publishers for topics without expected frequencies + publisher_list_1 = create_minimal_publisher( + TOPIC_LIST_1, LIST_PUBLISHER_FREQ, 'imu', '_list_1') + publisher_list_2 = create_minimal_publisher( + TOPIC_LIST_2, LIST_PUBLISHER_FREQ, 'imu', '_list_2') return ( launch.LaunchDescription([ @@ -73,6 +91,8 @@ def generate_test_description(): publisher_1, publisher_2, publisher_3, + publisher_list_1, + publisher_list_2, launch_testing.actions.ReadyToTest() ]), {} ) @@ -119,6 +139,47 @@ def test_all_topics_monitored(self): f'{tolerance_hz} of expected {expected_freq}' ) + def test_topics_list_monitored_without_expected_frequency(self): + """Test topics in list are monitored but show no expected frequency.""" + time.sleep(2.0) + + for topic in [TOPIC_LIST_1, TOPIC_LIST_2]: + with self.subTest(topic=topic): + diagnostics = collect_diagnostics_for_topic( + self.test_node, topic, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics), 3, + f'Expected at least 3 diagnostics for {topic}' + ) + + # Verify expected_frequency is 0.0 or not present (not configured) + last_diag = diagnostics[-1] + expected_freq_value = None + frame_rate_value = None + for kv in last_diag.values: + if kv.key == 'expected_frequency': + expected_freq_value = float(kv.value) + elif kv.key == 'frame_rate_node': + frame_rate_value = float(kv.value) + + # When not configured, expected_frequency is either not present or 0.0 + self.assertTrue( + expected_freq_value is None or expected_freq_value == 0.0, + f'{topic}: expected_frequency should be None or 0.0, ' + f'got {expected_freq_value}' + ) + + # Verify frame rate is being reported (topic is monitored) + self.assertIsNotNone( + frame_rate_value, + f'{topic}: should have frame_rate_node in diagnostics' + ) + self.assertGreater( + frame_rate_value, 0.0, + f'{topic}: frame_rate_node should be > 0' + ) + if __name__ == '__main__': unittest.main()