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..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. @@ -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, '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 7434b1d..dc3c274 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. @@ -17,13 +17,22 @@ # # 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 ( + FREQ_SUFFIX, + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) 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 @@ -47,6 +56,75 @@ 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, + 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' + + 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=timeout_sec) + + 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 create_minimal_publisher( topic: str, frequency_hz: float, message_type: str, id_suffix: str = ''): """Create a minimal publisher node with the given parameters.""" @@ -65,19 +143,33 @@ 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'] + 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(): + 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' ) @@ -277,3 +369,29 @@ 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. + """ + + 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/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 09ed0f8..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. @@ -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..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. @@ -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(const std::string & topic_name, const TopicConfig & incoming); + + 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,9 @@ class GreenwaveMonitor : public rclcpp::Node manage_topic_service_; rclcpp::Service::SharedPtr set_expected_frequency_service_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // 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 e73fc65..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. @@ -22,17 +22,115 @@ #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); + const size_t len = topic_and_field.length(); + + 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 (is_tol) { + info.topic_name = topic_and_field.substr(0, len - 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 +145,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 +248,258 @@ 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 (request->tolerance_percent < 0.0) { - response->success = false; - response->message = - "Invalid tolerance, must be a non-negative percentage"; - return; + + 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; } - msg_diagnostics_obj.setExpectedDt(request->expected_hz, request->tolerance_percent); + 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) { + 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; + } - 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) + "%"; + 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; + + // Skip if updating from within the node (avoids redundant work and deadlock) + if (updating_params_internally_) { + return result; + } + + // 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()); + 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 = incoming_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 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( + const std::string & topic_name, const TopicConfig & incoming) +{ + // Get expected frequency: prefer incoming, fall back to existing parameter + double expected_freq = 0.0; + 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 yet, nothing to do + return; + } + } + + // 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) + ); + + 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()); + } +} + +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); + + // 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()) { + continue; + } + + auto value_opt = get_numeric_parameter(name); + if (!value_opt.has_value()) { + continue; + } + + double value = value_opt.value(); + TopicConfig & config = startup_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 configs that have frequency set + for (const auto & [topic, config] : startup_configs) { + if (config.expected_frequency.has_value()) { + 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()); + } + } + } +} + +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)); + } +} + +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 +648,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..6e157c5 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# 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. +# 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 time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + get_parameter, + make_freq_param, + make_tol_param, + RosNodeTestCase, + set_parameter, +) +import launch +import launch_testing +import pytest + + +TEST_TOPIC = '/dynamic_param_topic' +TEST_FREQUENCY = 30.0 +TEST_TOLERANCE = 20.0 +NONEXISTENT_TOPIC = '/topic_that_does_not_exist' + + +@pytest.mark.launch_test +def generate_test_description(): + """Test dynamic parameter changes via ros2 param set.""" + ros2_monitor_node = create_monitor_node() + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestDynamicParameterChanges(RosNodeTestCase): + """Test changing parameters dynamically via ros2 param set.""" + + TEST_NODE_NAME = 'dynamic_param_test_node' + + def test_set_parameters(self): + """Test setting frequency and tolerance parameters in sequence.""" + time.sleep(2.0) + + 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 initially' + ) + + # 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}') + + 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}' + ) + + 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' + ) + + # 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( + actual_freq, TEST_FREQUENCY, places=1, + msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' + ) + + 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' + ) + + # 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') + + 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}' + ) + + 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(diagnostics_with_zero_tol), 2, + 'Topic should still be monitored with zero tolerance' + ) + + # Check that at least one diagnostic has ERROR level (frequency outside 0% tolerance) + has_error = any( + 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) + + 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}') + + # 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, TEST_FREQUENCY, places=1, + msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' + ) + + # 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' + ) + + +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..578f427 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# 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. +# 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, + find_best_diagnostic, + RosNodeTestCase, +) +import launch +import launch_testing +import pytest + + +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( + 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(RosNodeTestCase): + """Test that only specifying frequency works (tolerance defaults).""" + + TEST_NODE_NAME = 'freq_only_test_node' + + 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)}' + ) + best_status, _ = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, + 'Should have valid frame rate with default tolerance' + ) + + +if __name__ == '__main__': + unittest.main() 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..b351b33 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_multiple_topics.py @@ -0,0 +1,185 @@ +#!/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 + + +# Topics with expected frequencies configured +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 + +# 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, + 'tolerance': TOLERANCE + }, + TOPIC_2: { + 'expected_frequency': FREQUENCY_2, + 'tolerance': TOLERANCE + }, + TOPIC_3: { + 'expected_frequency': FREQUENCY_3, + 'tolerance': TOLERANCE + } + } + + # 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([ + ros2_monitor_node, + publisher_1, + publisher_2, + publisher_3, + publisher_list_1, + publisher_list_2, + 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}' + ) + + 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() 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..7a0048b --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# 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. +# 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 time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + make_freq_param, + RosNodeTestCase, + set_parameter, +) +import launch +import launch_testing +import pytest + + +NEW_TOPIC = '/new_dynamic_topic' +TEST_FREQUENCY = 50.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test adding a new topic via ros2 param set.""" + ros2_monitor_node = create_monitor_node() + + publisher = create_minimal_publisher( + NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestAddNewTopicViaParam(RosNodeTestCase): + """Test adding a new topic to monitoring via ros2 param set.""" + + 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.""" + 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 = set_parameter(self.test_node, 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..a90a634 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# 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. +# 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, + make_tol_param, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE, + RosNodeTestCase, +) +import launch +import launch_ros.actions +import launch_testing +import pytest + + +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 = { + make_tol_param(TEST_TOPIC): 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(RosNodeTestCase): + """Test that only specifying tolerance does NOT start monitoring.""" + + TEST_NODE_NAME = 'tol_only_test_node' + + 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..3a7c049 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# 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. +# 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, + find_best_diagnostic, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE, + RosNodeTestCase, +) +import launch +import launch_ros.actions +import launch_testing +import pytest + + +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 - 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' # 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() + 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' + ) + + 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() + ]), {} + ) + + +class TestYamlParameterFile(RosNodeTestCase): + """Test loading topic configuration from YAML parameter file.""" + + TEST_NODE_NAME = 'yaml_test_node' + + 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' + ) + best_status, _ = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, + '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' + ) + best_status, _ = find_best_diagnostic( + received_diagnostics, NESTED_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, + '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 new file mode 100644 index 0000000..4901635 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# 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. +# 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, + find_best_diagnostic, + MONITOR_NODE_NAME, + RosNodeTestCase, +) +import launch +import launch_testing +from launch_testing import post_shutdown_test +from launch_testing.asserts import assertExitCodes +import pytest + + +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( + 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(RosNodeTestCase): + """Post-shutdown tests.""" + + TEST_NODE_NAME = 'shutdown_test_node' + + 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(RosNodeTestCase): + """Tests for parameter-based topic configuration.""" + + TEST_NODE_NAME = 'topic_params_test_node' + + 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)}' + ) + 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 = best_values[0] + 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}' + ) + + +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