diff --git a/.github/workflows/debian-packages.yml b/.github/workflows/debian-packages.yml index 77e948c..d5c07e2 100644 --- a/.github/workflows/debian-packages.yml +++ b/.github/workflows/debian-packages.yml @@ -151,10 +151,10 @@ jobs: ls -la debian_packages/${{ matrix.ros_distro }}/ # Install the debian packages on top of ros-core - apt-get install -y ./debian_packages/${{ matrix.ros_distro }}/ros-${{ matrix.ros_distro }}-greenwave-monitor-interfaces_*.deb ./debian_packages/${{ matrix.ros_distro }}/ros-${{ matrix.ros_distro }}-greenwave-monitor_*.deb + apt-get install -y ./debian_packages/${{ matrix.ros_distro }}/ros-${{ matrix.ros_distro }}-greenwave-monitor_*.deb # Verify packages are installed - dpkg -s ros-${{ matrix.ros_distro }}-greenwave-monitor ros-${{ matrix.ros_distro }}-greenwave-monitor-interfaces + dpkg -s ros-${{ matrix.ros_distro }}-greenwave-monitor shell: bash env: DEBIAN_FRONTEND: noninteractive diff --git a/docs/images/SERVICES.md b/docs/images/SERVICES.md index 2f6de1e..a439a17 100644 --- a/docs/images/SERVICES.md +++ b/docs/images/SERVICES.md @@ -1,37 +1,51 @@ -# Services +# Parameters -The Greenwave Monitor provides two services. The `ManageTopic` service dynamically adds or removes topics from monitoring. The `SetExpectedFrequency` service dynamically sets or clears expected frequencies for a specified topic, which enables additional diagnostic values and statuses. +The Greenwave Monitor uses ROS parameters for all topic configuration, including enabling/disabling monitoring and setting expected frequencies. -## Manage Topic +## Topic Monitoring Control -The monitor node exposes a `/greenwave_monitor/manage_topic` service that follows the `greenwave_monitor_interfaces/srv/ManageTopic` service definition. +Topics can be enabled or disabled via the `greenwave_diagnostics..enabled` parameter. **Usage Examples** -To add a topic to the monitoring list: +To enable monitoring for a topic: ```bash -ros2 service call /greenwave_monitor/manage_topic greenwave_monitor_interfaces/srv/ManageTopic "{topic_name: '/topic2', add_topic: true}" +ros2 param set /greenwave_monitor greenwave_diagnostics./topic2.enabled true ``` -To remove a topic from the monitoring list: +To disable monitoring for a topic: ```bash -ros2 service call /greenwave_monitor/manage_topic greenwave_monitor_interfaces/srv/ManageTopic "{topic_name: '/topic2', add_topic: false}" +ros2 param set /greenwave_monitor greenwave_diagnostics./topic2.enabled false ``` -## Set Expected Frequency +## Expected Frequency Parameters -The monitor node exposes a `/greenwave_monitor/set_expected_frequency` service that follows the `greenwave_monitor_interfaces/srv/SetExpectedFrequency` service definition. +Expected frequencies are configured via ROS parameters with the following naming convention: +- `greenwave_diagnostics..expected_frequency` - Expected publish rate in Hz +- `greenwave_diagnostics..tolerance` - Tolerance percentage (default: 5.0%) **Usage Examples** To set the expected frequency for a topic: ```bash -ros2 service call /greenwave_monitor/set_expected_frequency greenwave_monitor_interfaces/srv/SetExpectedFrequency "{topic_name: '/topic2', expected_hz: , tolerance_percent: , add_topic_if_missing: true}" +ros2 param set /greenwave_monitor greenwave_diagnostics./topic2.expected_frequency 30.0 +ros2 param set /greenwave_monitor greenwave_diagnostics./topic2.tolerance 10.0 ``` -To clear the expected frequency for a topic: +Parameters can also be set at launch time via YAML: +```yaml +greenwave_monitor: + ros__parameters: + greenwave_diagnostics./topic2.expected_frequency: 30.0 + greenwave_diagnostics./topic2.tolerance: 10.0 + greenwave_diagnostics./topic2.enabled: true +``` + +Or via command line: ```bash -ros2 service call /greenwave_monitor/set_expected_frequency greenwave_monitor_interfaces/srv/SetExpectedFrequency "{topic_name: '/topic2', clear_expected: true}" +ros2 run greenwave_monitor greenwave_monitor --ros-args \ + -p greenwave_diagnostics./topic2.expected_frequency:=30.0 \ + -p greenwave_diagnostics./topic2.tolerance:=10.0 ``` Note: The topic name must include the leading slash (e.g., '/topic2' not 'topic2'). diff --git a/greenwave_monitor/CMakeLists.txt b/greenwave_monitor/CMakeLists.txt index 7c31f6d..7c87791 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -23,9 +23,9 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# Add message_diagnostics.hpp as a header-only library -add_library(message_diagnostics INTERFACE) -target_include_directories(message_diagnostics INTERFACE +# Add greenwave_diagnostics.hpp as a header-only library +add_library(greenwave_diagnostics INTERFACE) +target_include_directories(greenwave_diagnostics INTERFACE $ $) @@ -34,9 +34,8 @@ ament_target_dependencies(greenwave_monitor rclcpp std_msgs diagnostic_msgs - greenwave_monitor_interfaces ) -target_link_libraries(greenwave_monitor message_diagnostics) +target_link_libraries(greenwave_monitor greenwave_diagnostics) target_include_directories(greenwave_monitor PUBLIC $ @@ -51,7 +50,7 @@ add_executable(minimal_publisher_node src/minimal_publisher_node.cpp src/minimal_publisher_main.cpp) ament_target_dependencies(minimal_publisher_node rclcpp std_msgs sensor_msgs diagnostic_msgs) -target_link_libraries(minimal_publisher_node message_diagnostics) +target_link_libraries(minimal_publisher_node greenwave_diagnostics) target_include_directories(minimal_publisher_node PUBLIC $ $) @@ -60,7 +59,7 @@ install(TARGETS minimal_publisher_node DESTINATION lib/${PROJECT_NAME}) install( - DIRECTORY launch examples + DIRECTORY launch examples config DESTINATION share/${PROJECT_NAME} ) @@ -116,22 +115,28 @@ if(BUILD_TESTING) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) + # Add parameter-based topic configuration tests + ament_add_pytest_test(test_parameters test/test_parameters.py + TIMEOUT 120 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + # Add gtests - ament_add_gtest(test_message_diagnostics test/test_message_diagnostics.cpp + ament_add_gtest(test_greenwave_diagnostics test/test_greenwave_diagnostics.cpp TIMEOUT 60 WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) - ament_target_dependencies(test_message_diagnostics + ament_target_dependencies(test_greenwave_diagnostics rclcpp std_msgs diagnostic_msgs ) - target_link_libraries(test_message_diagnostics message_diagnostics) - target_include_directories(test_message_diagnostics PUBLIC + target_link_libraries(test_greenwave_diagnostics greenwave_diagnostics) + target_include_directories(test_greenwave_diagnostics PUBLIC $ $ ) - target_compile_features(test_message_diagnostics PUBLIC c_std_99 cxx_std_17) + target_compile_features(test_greenwave_diagnostics PUBLIC c_std_99 cxx_std_17) ament_add_gtest(test_minimal_publisher test/test_minimal_publisher.cpp @@ -144,7 +149,7 @@ if(BUILD_TESTING) sensor_msgs diagnostic_msgs ) - target_link_libraries(test_minimal_publisher message_diagnostics) + target_link_libraries(test_minimal_publisher greenwave_diagnostics) target_include_directories(test_minimal_publisher PUBLIC $ $ diff --git a/greenwave_monitor/config/greenwave_monitor.yaml b/greenwave_monitor/config/greenwave_monitor.yaml new file mode 100644 index 0000000..7237445 --- /dev/null +++ b/greenwave_monitor/config/greenwave_monitor.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + # All topics declared in greenwave_diagnostics will be automatically monitored + # if detected at startup. + greenwave_diagnostics: + /params_from_yaml_topic: + enabled: true + expected_frequency: 10.0 + tolerance: 5.0 diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index b7266c1..987c007 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2025, NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,51 +12,97 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import LogInfo from launch_ros.actions import Node def generate_launch_description(): + config_file = os.path.join( + get_package_share_directory('greenwave_monitor'), + 'config', + 'greenwave_monitor.yaml' + ) return LaunchDescription([ Node( package='greenwave_monitor', executable='minimal_publisher_node', - name='minimal_publisher1', + name='minimal_publisher_imu', output='log', parameters=[ - {'topic': 'imu_topic', 'frequency_hz': 100.0} + { + 'topic': '/imu_topic', 'frequency_hz': 100.0, + 'greenwave_diagnostics': { + '/imu_topic': {'expected_frequency': 100.0, 'tolerance': 5.0} + } + } ], ), Node( package='greenwave_monitor', executable='minimal_publisher_node', - name='minimal_publisher2', + name='minimal_publisher_image', output='log', parameters=[ - {'topic': 'image_topic', 'message_type': 'image', 'frequency_hz': 30.0} + { + 'topic': '/image_topic', 'message_type': 'image', 'frequency_hz': 30.0, + 'greenwave_diagnostics': { + '/image_topic': {'expected_frequency': 30.0, 'tolerance': 5.0} + } + } ], ), Node( package='greenwave_monitor', executable='minimal_publisher_node', - name='minimal_publisher3', + name='minimal_publisher_string', output='log', parameters=[ - {'topic': 'string_topic', 'message_type': 'string', 'frequency_hz': 1000.0} + { + 'topic': '/string_topic', 'message_type': 'string', 'frequency_hz': 1000.0, + 'greenwave_diagnostics': { + '/string_topic': {'expected_frequency': 1000.0, 'tolerance': 10.0} + } + } ], ), Node( package='greenwave_monitor', - executable='greenwave_monitor', - name='greenwave_monitor', + executable='minimal_publisher_node', + name='minimal_publisher_params_from_yaml', + output='log', + parameters=[ + { + 'topic': '/params_from_yaml_topic', 'message_type': 'imu', + 'frequency_hz': 10.0, 'enable_greenwave_diagnostics': False + } + ], + ), + Node( + package='greenwave_monitor', + executable='minimal_publisher_node', + name='minimal_publisher_no_startup_monitor', output='log', parameters=[ - {'topics': ['/imu_topic', '/image_topic', '/string_topic']} + { + 'topic': '/no_startup_monitor_topic', 'message_type': 'imu', + 'frequency_hz': 1.0, 'enable_greenwave_diagnostics': False + } ], ), + Node( + package='greenwave_monitor', + executable='greenwave_monitor', + name='greenwave_monitor', + output='log', + parameters=[config_file] + ), LogInfo( - msg='Run `ros2 run r2s_gw r2s_gw` in another terminal to see the demo output ' + msg='Follow the instructions to setup r2s_gw in the README.md, then run ' + '`ros2 run r2s_gw r2s_gw` in another terminal to see the demo output ' 'with the r2s dashboard.' - ), + ) ]) diff --git a/greenwave_monitor/greenwave_monitor/ncurses_frontend.py b/greenwave_monitor/greenwave_monitor/ncurses_frontend.py index 77b76c1..9db473b 100644 --- a/greenwave_monitor/greenwave_monitor/ncurses_frontend.py +++ b/greenwave_monitor/greenwave_monitor/ncurses_frontend.py @@ -335,10 +335,10 @@ def curses_main(stdscr, node): diag.latency.ljust(REALTIME_DELAY_WIDTH) if diag.latency != '-' else 'N/A'.ljust(REALTIME_DELAY_WIDTH)) - # Get expected frequency - expected_hz, tolerance = node.ui_adaptor.get_expected_frequency(topic_name) - if expected_hz > 0: - expected_freq_display = f'{expected_hz:.1f}Hz'.ljust(12) + # Get expected frequency with tolerance + expected_freq_str = node.ui_adaptor.get_expected_frequency_str(topic_name) + if expected_freq_str != '-': + expected_freq_display = expected_freq_str.ljust(14) # Color coding based on status if is_monitored: diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 7434b1d..0f2246a 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,24 @@ # # SPDX-License-Identifier: Apache-2.0 +from abc import ABC import math import time -from typing import List, Optional, Tuple +from typing import Any, List, Optional, Tuple +import unittest from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency +from greenwave_monitor.ui_adaptor import ( + build_full_node_name, + ENABLED_SUFFIX, + FREQ_SUFFIX, + get_ros_parameters, + set_ros_parameters, + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) import launch_ros +from rcl_interfaces.msg import ParameterType, ParameterValue import rclpy from rclpy.node import Node @@ -42,13 +53,50 @@ ] # Standard test constants -MANAGE_TOPIC_TEST_CONFIG = TEST_CONFIGURATIONS[2] +MANAGE_TOPIC_TEST_CONFIG = TEST_CONFIGURATIONS[1] # 100Hz imu MONITOR_NODE_NAME = 'test_greenwave_monitor' MONITOR_NODE_NAMESPACE = 'test_namespace' +def make_enabled_param(topic: str) -> str: + """Build enabled parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{ENABLED_SUFFIX}' + + +def set_parameter(test_node: Node, param_name: str, value, + node_name: str = MONITOR_NODE_NAME, + node_namespace: str = MONITOR_NODE_NAMESPACE, + timeout_sec: float = 10.0) -> bool: + """Set a parameter on a node using rclpy service client.""" + full_node_name = build_full_node_name(node_name, node_namespace) + success, _ = set_ros_parameters(test_node, full_node_name, {param_name: value}, timeout_sec) + return success + + +def get_parameter(test_node: Node, param_name: str, + node_name: str = MONITOR_NODE_NAME, + node_namespace: str = MONITOR_NODE_NAMESPACE) -> Tuple[bool, Any]: + """Get a parameter from a node using rclpy service client.""" + full_node_name = build_full_node_name(node_name, node_namespace) + result = get_ros_parameters(test_node, full_node_name, [param_name]) + value = result.get(param_name) + return (value is not None, value) + + +def delete_parameter(test_node: Node, param_name: str, + node_name: str = MONITOR_NODE_NAME, + node_namespace: str = MONITOR_NODE_NAMESPACE, + timeout_sec: float = 10.0) -> bool: + """Delete a parameter from a node using rclpy service client.""" + full_node_name = build_full_node_name(node_name, node_namespace) + not_set = ParameterValue(type=ParameterType.PARAMETER_NOT_SET) + success, _ = set_ros_parameters(test_node, full_node_name, {param_name: not_set}, timeout_sec) + return success + + def create_minimal_publisher( - topic: str, frequency_hz: float, message_type: str, id_suffix: str = ''): + topic: str, frequency_hz: float, message_type: str, id_suffix: str = '', + enable_diagnostics: bool = True): """Create a minimal publisher node with the given parameters.""" return launch_ros.actions.Node( package='greenwave_monitor', @@ -57,7 +105,8 @@ def create_minimal_publisher( parameters=[{ 'topic': topic, 'frequency_hz': frequency_hz, - 'message_type': message_type + 'message_type': message_type, + 'enable_greenwave_diagnostics': enable_diagnostics }], output='screen' ) @@ -65,19 +114,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' ) @@ -94,54 +157,6 @@ def wait_for_service_connection(node: Node, return service_available -def call_manage_topic_service(node: Node, - service_client, - add: bool, - topic: str, - timeout_sec: float = 8.0 - ) -> Optional[ManageTopic.Response]: - """Call the manage_topic service with given parameters.""" - request = ManageTopic.Request() - request.add_topic = add - request.topic_name = topic - future = service_client.call_async(request) - - rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec) - - if future.result() is None: - node.get_logger().error('Service call failed or timed out') - return None - - return future.result() - - -def call_set_frequency_service(node: Node, - service_client, - topic_name: str, - expected_hz: float = 0.0, - tolerance_percent: float = 0.0, - clear: bool = False, - add_if_missing: bool = True, - timeout_sec: float = 8.0 - ) -> Optional[SetExpectedFrequency.Response]: - """Call the set_expected_frequency service with given parameters.""" - request = SetExpectedFrequency.Request() - request.topic_name = topic_name - request.expected_hz = expected_hz - request.tolerance_percent = tolerance_percent - request.clear_expected = clear - request.add_topic_if_missing = add_if_missing - - future = service_client.call_async(request) - rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec) - - if future.result() is None: - node.get_logger().error('Service call failed or timed out') - return None - - return future.result() - - def collect_diagnostics_for_topic(node: Node, topic_name: str, expected_count: int = 5, @@ -265,15 +280,27 @@ def verify_diagnostic_values(status: DiagnosticStatus, return errors -def create_service_clients(node: Node, namespace: str = MONITOR_NODE_NAMESPACE, - node_name: str = MONITOR_NODE_NAME): - """Create service clients for the monitor node.""" - manage_topic_client = node.create_client( - ManageTopic, f'/{namespace}/{node_name}/manage_topic' - ) +class RosNodeTestCase(unittest.TestCase, ABC): + """ + Abstract base class for ROS 2 launch tests that need a test node. - set_frequency_client = node.create_client( - SetExpectedFrequency, f'/{namespace}/{node_name}/set_expected_frequency' - ) + 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) - return manage_topic_client, set_frequency_client + @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..370ba03 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. @@ -26,25 +26,192 @@ thread-safe, easy-to-consume view (`UiDiagnosticData`) per monitored topic, including the timestamp of the last update for each topic. -In addition to passively subscribing, `GreenwaveUiAdaptor` exposes clients for two -services on the monitor node: -- ManageTopic: start/stop monitoring a topic (`toggle_topic_monitoring`). -- SetExpectedFrequency: set/clear the expected publish rate and tolerance for a topic - (`set_expected_frequency`). Expected rates are also cached locally in - `expected_frequencies` as `(expected_hz, tolerance_percent)` so UIs can display the - configured values alongside live diagnostics. +In addition to passively subscribing, `GreenwaveUiAdaptor` exposes: +- Parameter-based topic monitoring: start/stop monitoring a topic via the + `greenwave_diagnostics..enabled` parameter (`toggle_topic_monitoring`). +- Parameter-based frequency configuration: set/clear the expected publish rate and tolerance + for a topic (`set_expected_frequency`) via ROS parameters. Expected rates are also cached + locally in `expected_frequencies` as `(expected_hz, tolerance_percent)` so UIs can display + the configured values alongside live diagnostics. """ from dataclasses import dataclass +import math 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 Parameter, ParameterEvent, ParameterType, ParameterValue +from rcl_interfaces.srv import GetParameters, ListParameters, SetParameters import rclpy from rclpy.node import Node +# Parameter name constants +TOPIC_PARAM_PREFIX = 'greenwave_diagnostics.' +FREQ_SUFFIX = '.expected_frequency' +TOL_SUFFIX = '.tolerance' +ENABLED_SUFFIX = '.enabled' +DEFAULT_TOLERANCE_PERCENT = 5.0 + + +def build_full_node_name(node_name: str, node_namespace: str, is_client: bool = False) -> str: + """Build full ROS node name from name and namespace.""" + join_list = [] + # Strip leading '/' from namespace to avoid double slashes when joining + if node_namespace and node_namespace != '/': + join_list.append(node_namespace.lstrip('/')) + if node_name: + join_list.append(node_name) + joined = '/'.join(join_list) + if not is_client: + return f'/{joined}' + return joined + + +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 _make_param(name: str, value) -> Parameter: + """Create a Parameter message from a name and Python value (or ParameterValue).""" + param = Parameter() + param.name = name + if isinstance(value, ParameterValue): + param.value = value + else: + param.value = ParameterValue() + if isinstance(value, str): + param.value.type = ParameterType.PARAMETER_STRING + param.value.string_value = value + elif isinstance(value, bool): + param.value.type = ParameterType.PARAMETER_BOOL + param.value.bool_value = value + elif isinstance(value, int): + param.value.type = ParameterType.PARAMETER_INTEGER + param.value.integer_value = value + else: + param.value.type = ParameterType.PARAMETER_DOUBLE + param.value.double_value = float(value) + return param + + +def set_ros_parameters(node: Node, target_node: str, params: dict, + timeout_sec: float = 3.0) -> tuple[bool, list[str]]: + """ + Set one or more parameters on a target node. + + :param node: The ROS node to use for service calls. + :param target_node: Full name of target node (e.g., '/my_node' or '/ns/my_node'). + :param params: Dict mapping parameter names to values. + :param timeout_sec: Service call timeout. + :returns: Tuple of (all_successful, list of failure reasons). + """ + if '/' not in target_node: + target_node = f'/{target_node}' + client = node.create_client(SetParameters, f'{target_node}/set_parameters') + if not client.wait_for_service(timeout_sec=min(timeout_sec, 5.0)): + node.destroy_client(client) + return False, ['Service not available'] + + request = SetParameters.Request() + request.parameters = [_make_param(name, value) for name, value in params.items()] + + try: + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec) + node.destroy_client(client) + + if future.result() is None: + return False, ['Service call timed out'] + + results = future.result().results + failures = [r.reason for r in results if not r.successful and r.reason] + return all(r.successful for r in results), failures + except Exception as e: + node.destroy_client(client) + return False, [str(e)] + + +def get_ros_parameters(node: Node, target_node: str, param_names: list, + timeout_sec: float = 5.0) -> dict: + """ + Get parameters from a target node. + + :param node: The ROS node to use for service calls. + :param target_node: Full name of target node (e.g., '/my_node' or '/ns/my_node'). + :param param_names: List of parameter names to retrieve. + :param timeout_sec: Service call timeout. + :returns: Dict mapping parameter names to their values (float or None if not found/invalid). + """ + client = node.create_client(GetParameters, f'{target_node}/get_parameters') + if not client.wait_for_service(timeout_sec=min(timeout_sec, 5.0)): + node.destroy_client(client) + return {name: None for name in param_names} + + request = GetParameters.Request() + request.names = param_names + + try: + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec) + node.destroy_client(client) + + if future.result() is None or not future.result().values: + return {name: None for name in param_names} + + result = {} + for name, value in zip(param_names, future.result().values): + result[name] = param_value_to_python(value) + return result + except Exception: + node.destroy_client(client) + return {name: None for name in param_names} + + +def parse_topic_param_name(param_name: str) -> tuple[str, str]: + """Parse parameter name to extract topic name and field type.""" + if not param_name.startswith(TOPIC_PARAM_PREFIX): + return ('', '') + + topic_and_field = param_name[len(TOPIC_PARAM_PREFIX):] + + if topic_and_field.endswith(FREQ_SUFFIX): + return (topic_and_field[:-len(FREQ_SUFFIX)], 'freq') + if topic_and_field.endswith(TOL_SUFFIX): + return (topic_and_field[:-len(TOL_SUFFIX)], 'tol') + if topic_and_field.endswith(ENABLED_SUFFIX): + return (topic_and_field[:-len(ENABLED_SUFFIX)], 'enabled') + + return ('', '') + + +def param_value_to_python(value: ParameterValue): + """Convert a ParameterValue to its Python equivalent.""" + if value.type == ParameterType.PARAMETER_BOOL: + return value.bool_value + elif value.type == ParameterType.PARAMETER_DOUBLE: + return value.double_value + elif value.type == ParameterType.PARAMETER_INTEGER: + return value.integer_value + elif value.type == ParameterType.PARAMETER_STRING: + return value.string_value + return None + + +_STATUS_LEVEL_MAP = { + DiagnosticStatus.OK: 'OK', + DiagnosticStatus.WARN: 'WARN', + DiagnosticStatus.ERROR: 'ERROR', + DiagnosticStatus.STALE: 'STALE', +} + @dataclass class UiDiagnosticData: @@ -54,7 +221,6 @@ class UiDiagnosticData: Fields are stored as strings for straightforward rendering in UI components. `status` is one of: 'OK' | 'WARN' | 'ERROR' | 'STALE' | 'UNKNOWN' (or '-' if unset). `last_update` stores the epoch timestamp when diagnostics were last refreshed. - """ pub_rate: str = '-' @@ -67,16 +233,7 @@ class UiDiagnosticData: def from_status(cls, status: DiagnosticStatus) -> 'UiDiagnosticData': """Create UiDiagnosticData from DiagnosticStatus.""" data = cls() - if status.level == DiagnosticStatus.OK: - data.status = 'OK' - elif status.level == DiagnosticStatus.WARN: - data.status = 'WARN' - elif status.level == DiagnosticStatus.ERROR: - data.status = 'ERROR' - elif status.level == DiagnosticStatus.STALE: - data.status = 'STALE' - else: - data.status = 'UNKNOWN' + data.status = _STATUS_LEVEL_MAP.get(status.level, 'UNKNOWN') for kv in status.values: if kv.key == 'frame_rate_node': @@ -93,20 +250,22 @@ class GreenwaveUiAdaptor: Subscribe to `/diagnostics` and manage topic monitoring for UI consumption. Designed for UI frontends, this class keeps per-topic `UiDiagnosticData` up to date, - provides a toggle for monitoring via `ManageTopic`, and exposes helpers to set/clear - expected frequencies via `SetExpectedFrequency`. Service names may be discovered - dynamically or constructed from an optional namespace and node name. + provides a toggle for monitoring via the `greenwave_diagnostics..enabled` parameter, + and exposes helpers to set/clear expected frequencies via ROS parameters. """ def __init__(self, node: Node, monitor_node_name: str = 'greenwave_monitor'): """Initialize the UI adaptor for subscribing to diagnostics and managing topics.""" self.node = node + # Just use the bare node name - ROS will expand with the caller's namespace self.monitor_node_name = monitor_node_name self.data_lock = threading.Lock() self.ui_diagnostics: Dict[str, UiDiagnosticData] = {} # { topic_name : (expected_hz, tolerance) } self.expected_frequencies: Dict[str, tuple[float, float]] = {} + # { topic_name : node_name } - maps topics to the node that has their params + self.topic_to_node: Dict[str, str] = {} self._setup_ros_components() @@ -119,20 +278,156 @@ def _setup_ros_components(self): 100 ) - manage_service_name = f'{self.monitor_node_name}/manage_topic' - set_freq_service_name = f'{self.monitor_node_name}/set_expected_frequency' + self.param_events_subscription = self.node.create_subscription( + ParameterEvent, + '/parameter_events', + self._on_parameter_event, + 10 + ) - self.node.get_logger().info(f'Connecting to monitor service: {manage_service_name}') + # Track pending node queries to prevent garbage collection + self._pending_node_queries: Dict[str, dict] = {} - self.manage_topic_client = self.node.create_client( - ManageTopic, - manage_service_name - ) + # Query initial parameters after a short delay to let nodes come up + self._initial_params_timer = self.node.create_timer( + 2.0, self._fetch_initial_parameters_callback) - self.set_expected_frequency_client = self.node.create_client( - SetExpectedFrequency, - set_freq_service_name - ) + def _fetch_initial_parameters_callback(self): + """Timer callback to fetch initial parameters from all nodes.""" + if self._initial_params_timer is not None: + self._initial_params_timer.cancel() + self._initial_params_timer = None + + for node_name, node_namespace in self.node.get_node_names_and_namespaces(): + if node_name == self.node.get_name(): + continue + + full_node_name = build_full_node_name(node_name, node_namespace, is_client=True) + self._query_node_parameters_async(full_node_name) + + def _query_node_parameters_async(self, full_node_name: str): + """Start async query for topic parameters on a specific node.""" + list_client = self.node.create_client( + ListParameters, f'{full_node_name}/list_parameters') + + if not list_client.service_is_ready(): + self.node.destroy_client(list_client) + return + + query_id = full_node_name + self._pending_node_queries[query_id] = { + 'node_name': full_node_name, + 'list_client': list_client, + 'get_client': None, + 'param_names': [] + } + + list_request = ListParameters.Request() + list_request.prefixes = ['greenwave_diagnostics'] + list_request.depth = 10 + + list_future = list_client.call_async(list_request) + list_future.add_done_callback( + lambda f, qid=query_id: self._on_list_params_response(f, qid)) + + def _on_list_params_response(self, future, query_id: str): + """Handle list_parameters response from a node.""" + try: + query = self._pending_node_queries.get(query_id) + if not query or future.result() is None: + self._cleanup_node_query(query_id) + return + + param_names = [n for n in future.result().result.names + if n.startswith(TOPIC_PARAM_PREFIX)] + if not param_names: + self._cleanup_node_query(query_id) + return + + full_node_name = query['node_name'] + get_client = self.node.create_client( + GetParameters, f'{full_node_name}/get_parameters') + + if not get_client.service_is_ready(): + self.node.destroy_client(get_client) + self._cleanup_node_query(query_id) + return + + query['get_client'] = get_client + query['param_names'] = param_names + + get_request = GetParameters.Request() + get_request.names = param_names + + get_future = get_client.call_async(get_request) + get_future.add_done_callback( + lambda f, qid=query_id: self._on_get_params_response(f, qid)) + + except Exception as e: + self.node.get_logger().debug(f'Error listing parameters: {e}') + self._cleanup_node_query(query_id) + + def _on_get_params_response(self, future, query_id: str): + """Handle get_parameters response from a node.""" + try: + query = self._pending_node_queries.get(query_id) + if not query or future.result() is None: + self._cleanup_node_query(query_id) + return + + full_node_name = query['node_name'] + + # Extract all topic names from param names to build topic -> node mapping + topics_on_this_node = set() + for name in query['param_names']: + topic_name, field = parse_topic_param_name(name) + if topic_name: + topics_on_this_node.add(topic_name) + + topic_configs: Dict[str, Dict[str, float]] = {} + for name, value in zip(query['param_names'], future.result().values): + py_value = param_value_to_python(value) + if not isinstance(py_value, (int, float)): + continue + topic_name, field = parse_topic_param_name(name) + if not topic_name or not field: + continue + if topic_name not in topic_configs: + topic_configs[topic_name] = {} + topic_configs[topic_name][field] = float(py_value) + + with self.data_lock: + for topic_name in topics_on_this_node: + self.topic_to_node[topic_name] = full_node_name + + for topic_name, config in topic_configs.items(): + freq = config.get('freq', 0.0) + tol = config.get('tol', DEFAULT_TOLERANCE_PERCENT) + if freq > 0 and not math.isnan(freq): + self.expected_frequencies[topic_name] = (freq, tol) + + except Exception as e: + self.node.get_logger().debug(f'Error fetching parameters: {e}') + finally: + self._cleanup_node_query(query_id) + + def _cleanup_node_query(self, query_id: str): + """Clean up clients for a completed node query.""" + query = self._pending_node_queries.pop(query_id, None) + if query: + if query.get('list_client'): + self.node.destroy_client(query['list_client']) + if query.get('get_client'): + self.node.destroy_client(query['get_client']) + + def _call_service(self, client, request, timeout_sec: float = 3.0): + """Call a service and return the result, or None on timeout/error.""" + try: + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=timeout_sec) + return future.result() + except Exception: + return None def _extract_topic_name(self, diagnostic_name: str) -> str: """ @@ -168,45 +463,80 @@ def _on_diagnostics(self, msg: DiagnosticArray): topic_name = self._extract_topic_name(status.name) self.ui_diagnostics[topic_name] = ui_data - 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): - return + def _on_parameter_event(self, msg: ParameterEvent): + """Process parameter change events from any node.""" + for param in msg.changed_parameters + msg.new_parameters: + topic_name, field = parse_topic_param_name(param.name) + if not topic_name or field == '': + continue - request = ManageTopic.Request() - request.topic_name = topic_name - - with self.data_lock: - request.add_topic = topic_name not in self.ui_diagnostics - - try: - # Use asynchronous service call to prevent deadlock - future = self.manage_topic_client.call_async(request) - rclpy.spin_until_future_complete(self.node, future, timeout_sec=3.0) - - if future.result() is None: - action = 'start' if request.add_topic else 'stop' - self.node.get_logger().error( - f'Failed to {action} monitoring: Service call timed out') - return - - response = future.result() + with self.data_lock: + if field == 'enabled' and param in msg.new_parameters: + if (param.value.type == ParameterType.PARAMETER_BOOL and + param.value.bool_value): + # Keep the topic to node map up to date when new enabled parameters appear + # This makes it easy to set the right parameter in toggle topic monitoring + self.topic_to_node[topic_name] = msg.node + continue + elif field == 'enabled' and param in msg.changed_parameters: + if (param.value.type == ParameterType.PARAMETER_BOOL and + not param.value.bool_value): + self.ui_diagnostics.pop(topic_name, None) + continue + + value = param_value_to_python(param.value) + if not isinstance(value, (int, float)): + continue + value = float(value) + + current = self.expected_frequencies.get(topic_name, (0.0, 0.0)) + + if field == 'freq': + # Treat NaN or non-positive as "cleared" + if value > 0 or math.isnan(value): + self.expected_frequencies[topic_name] = (value, current[1]) + elif field == 'tol': + if current[0] > 0: # Only update if frequency is set + self.expected_frequencies[topic_name] = (current[0], value) + + for param in msg.deleted_parameters: + topic_name, field = parse_topic_param_name(param.name) + if not topic_name or field == '': + continue with self.data_lock: - if not response.success: - action = 'start' if request.add_topic else 'stop' - self.node.get_logger().error( - f'Failed to {action} monitoring: {response.message}') - return + if field == 'enabled': + # Remove the topic in the map when there is no longer a parameter for it + self.topic_to_node.pop(topic_name, None) + self.ui_diagnostics.pop(topic_name, None) + elif field == 'freq': + self.expected_frequencies.pop(topic_name, None) + elif field == 'tol': + current = self.expected_frequencies.get(topic_name) + if current and current[0] > 0: + self.expected_frequencies[topic_name] = ( + current[0], DEFAULT_TOLERANCE_PERCENT) - if not request.add_topic and topic_name in self.ui_diagnostics: - del self.ui_diagnostics[topic_name] - if topic_name in self.expected_frequencies: - del self.expected_frequencies[topic_name] + def toggle_topic_monitoring(self, topic_name: str): + """Toggle monitoring for a topic via enabled parameter.""" + with self.data_lock: + is_monitoring = topic_name in self.ui_diagnostics + target_node = self.topic_to_node.get(topic_name, self.monitor_node_name) + new_enabled = not is_monitoring + action = 'start' if new_enabled else 'stop' + + enabled_param = f'{TOPIC_PARAM_PREFIX}{topic_name}{ENABLED_SUFFIX}' + success, failures = set_ros_parameters( + self.node, target_node, {enabled_param: new_enabled}) + + if not success: + self.node.get_logger().error( + f'Failed to {action} monitoring: {"; ".join(failures)}') + return - except Exception as e: - action = 'start' if request.add_topic else 'stop' - self.node.get_logger().error(f'Failed to {action} monitoring: {e}') + with self.data_lock: + if not new_enabled and topic_name in self.ui_diagnostics: + self.ui_diagnostics.pop(topic_name, None) def set_expected_frequency(self, topic_name: str, @@ -214,47 +544,28 @@ def set_expected_frequency(self, tolerance_percent: float = 0.0, clear: bool = False ) -> tuple[bool, str]: - """Set or clear the expected frequency for a topic.""" - if not self.set_expected_frequency_client.wait_for_service(timeout_sec=1.0): - return False, 'Could not connect to set_expected_frequency service.' - - request = SetExpectedFrequency.Request() - request.topic_name = topic_name - request.expected_hz = expected_hz - request.tolerance_percent = tolerance_percent - request.clear_expected = clear - request.add_topic_if_missing = True - - # Use asynchronous service call to prevent deadlock - try: - future = self.set_expected_frequency_client.call_async(request) - rclpy.spin_until_future_complete(self.node, future, timeout_sec=3.0) - - if future.result() is None: - action = 'clear' if clear else 'set' - error_msg = f'Failed to {action} expected frequency: Service call timed out' - self.node.get_logger().error(error_msg) - return False, error_msg - - response = future.result() - - if not response.success: - action = 'clear' if clear else 'set' - self.node.get_logger().error( - f'Failed to {action} expected frequency: {response.message}') - return False, response.message + """Set or clear the expected frequency for a topic via parameters.""" + with self.data_lock: + target_node = self.topic_to_node.get(topic_name, self.monitor_node_name) + action = 'clear' if clear else 'set' + + params = { + make_freq_param(topic_name): float('nan') if clear else expected_hz, + make_tol_param(topic_name): DEFAULT_TOLERANCE_PERCENT if clear else tolerance_percent, + } + + success, failures = set_ros_parameters(self.node, target_node, params) + + if not success: + return False, f'Failed to {action} expected frequency: {"; ".join(failures)}' + + with self.data_lock: + if clear: + self.expected_frequencies.pop(topic_name, None) else: - with self.data_lock: - if clear: - self.expected_frequencies.pop(topic_name, None) - else: - self.expected_frequencies[topic_name] = (expected_hz, tolerance_percent) - return True, response.message - except Exception as e: - action = 'clear' if clear else 'set' - error_msg = f'Failed to {action} expected frequency: {e}' - self.node.get_logger().error(error_msg) - return False, error_msg + self.expected_frequencies[topic_name] = (expected_hz, tolerance_percent) + + return True, f'{"Cleared" if clear else "Set"} expected frequency for {topic_name}' def get_topic_diagnostics(self, topic_name: str) -> UiDiagnosticData: """Get diagnostic data for a topic. Returns default values if topic not found.""" @@ -265,3 +576,12 @@ def get_expected_frequency(self, topic_name: str) -> tuple[float, float]: """Get monitoring settings for a topic. Returns (0.0, 0.0) if not set.""" with self.data_lock: return self.expected_frequencies.get(topic_name, (0.0, 0.0)) + + def get_expected_frequency_str(self, topic_name: str) -> str: + """Get expected frequency as formatted string with tolerance (e.g., '30.0Hz ±5%').""" + freq, tol = self.get_expected_frequency(topic_name) + if freq <= 0.0 or math.isnan(freq): + return '-' + if tol > 0.0: + return f'{freq:.1f}Hz±{tol:.0f}%' + return f'{freq:.1f}Hz' diff --git a/greenwave_monitor/include/message_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp similarity index 58% rename from greenwave_monitor/include/message_diagnostics.hpp rename to greenwave_monitor/include/greenwave_diagnostics.hpp index 324d37d..10eb7b3 100644 --- a/greenwave_monitor/include/message_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -18,22 +18,28 @@ #pragma once #include -#include #include #include +#include +#include #include +#include #include #include #include #include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "rcpputils/join.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" #include "std_msgs/msg/header.hpp" #include "builtin_interfaces/msg/time.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/rclcpp.hpp" -namespace message_diagnostics +namespace greenwave_diagnostics { namespace constants { @@ -46,10 +52,18 @@ inline constexpr uint64_t kMillisecondsToSeconds = 1000ULL; inline constexpr int64_t kDropWarnTimeoutSeconds = 5LL; // Cutoff where we consider latency to be nonsense inline constexpr int64_t kNonsenseLatencyMs = 365LL * 24LL * 60LL * 60LL * 1000LL; +// Parameter constants +inline constexpr const char * kTopicParamPrefix = "greenwave_diagnostics."; +inline constexpr const char * kFreqSuffix = ".expected_frequency"; +inline constexpr const char * kTolSuffix = ".tolerance"; +inline constexpr const char * kEnabledSuffix = ".enabled"; +inline constexpr double kDefaultTolerancePercent = 5.0; +inline constexpr double kDefaultFrequencyHz = std::numeric_limits::quiet_NaN(); +inline constexpr bool kDefaultEnabled = true; } // namespace constants // Configurations for a message diagnostics -struct MessageDiagnosticsConfig +struct GreenwaveDiagnosticsConfig { // diagnostics toggle bool enable_diagnostics{false}; @@ -73,13 +87,13 @@ struct MessageDiagnosticsConfig int64_t jitter_tolerance_us{0LL}; }; -class MessageDiagnostics +class GreenwaveDiagnostics { public: - MessageDiagnostics( + GreenwaveDiagnostics( rclcpp::Node & node, const std::string & topic_name, - const MessageDiagnosticsConfig & diagnostics_config) + const GreenwaveDiagnosticsConfig & diagnostics_config) : node_(node), topic_name_(topic_name), diagnostics_config_(diagnostics_config) { clock_ = node_.get_clock(); @@ -106,14 +120,73 @@ class MessageDiagnostics diagnostic_publisher_ = node_.create_publisher( "/diagnostics", 10); + + // Build parameter names for this topic + freq_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + + constants::kFreqSuffix; + tol_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + + constants::kTolSuffix; + enabled_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + + constants::kEnabledSuffix; + + // Register parameter callback for this topic's parameters + param_callback_handle_ = node_.add_on_set_parameters_callback( + std::bind(&GreenwaveDiagnostics::onParameterChange, this, std::placeholders::_1)); + + // Subscribe to parameter events + param_event_subscription_ = node_.create_subscription( + "/parameter_events", 10, + std::bind(&GreenwaveDiagnostics::onParameterEvent, this, std::placeholders::_1)); + + // Convert config's expected_dt_us to frequency if set + double default_freq = constants::kDefaultFrequencyHz; + if (diagnostics_config_.expected_dt_us > 0) { + default_freq = static_cast(constants::kSecondsToMicroseconds) / + static_cast(diagnostics_config_.expected_dt_us); + } + + // Declare frequency, tolerance, and enabled parameters if they won't automatically be declared + // from overrides. Declared parameters cannot be deleted. Use passed DiagnosticsConfig values. + bool auto_declare = node_.get_node_options().automatically_declare_parameters_from_overrides(); + if (!auto_declare) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node_.declare_parameter(enabled_param_name_, constants::kDefaultEnabled); + node_.declare_parameter(tol_param_name_, constants::kDefaultTolerancePercent, descriptor); + node_.declare_parameter(freq_param_name_, default_freq, descriptor); + } else { + // Parameters declared via launch/YAML/constructor are ignored by onParameterChange() + // Re-set parameters to their current value to trigger callbacks. If + // the parameter fails to set, use defaults. + setParameterOrDefault(enabled_param_name_, constants::kDefaultEnabled); + setParameterOrDefault(freq_param_name_, default_freq); + setParameterOrDefault(tol_param_name_, constants::kDefaultTolerancePercent); + } + } + + ~GreenwaveDiagnostics() + { + if (param_callback_handle_) { + node_.remove_on_set_parameters_callback(param_callback_handle_.get()); + param_callback_handle_.reset(); + } + param_event_subscription_.reset(); + diagnostic_publisher_.reset(); } // Update diagnostics numbers. To be called in Subscriber and Publisher void updateDiagnostics(uint64_t msg_timestamp_ns) { + // If the topic is not enabled, skip updating diagnostics + if (!enabled_) { + RCLCPP_DEBUG_THROTTLE( + node_.get_logger(), *clock_, 1000, + "Topic %s is not enabled, skipping update diagnostics", topic_name_.c_str()); + return; + } // Mutex lock to prevent simultaneous access of common parameters // used by updateDiagnostics() and publishDiagnostics() - const std::lock_guard lock(message_diagnostics_mutex_); + const std::lock_guard lock(greenwave_diagnostics_mutex_); // Message diagnostics checks message intervals both using the node clock // and the message timestamp. // All variables name _node refers to the node timestamp checks. @@ -123,9 +196,9 @@ class MessageDiagnostics // Get the current timestamps in microseconds uint64_t current_timestamp_msg_us = - msg_timestamp_ns / message_diagnostics::constants::kMicrosecondsToNanoseconds; + msg_timestamp_ns / greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; uint64_t current_timestamp_node_us = static_cast(clock_->now().nanoseconds() / - message_diagnostics::constants::kMicrosecondsToNanoseconds); + greenwave_diagnostics::constants::kMicrosecondsToNanoseconds); // we can only calculate frame rate after 2 messages have been received if (prev_timestamp_node_us_ != std::numeric_limits::min()) { @@ -138,11 +211,11 @@ class MessageDiagnostics const rclcpp::Time time_from_node = node_.get_clock()->now(); uint64_t ros_node_system_time_us = time_from_node.nanoseconds() / - message_diagnostics::constants::kMicrosecondsToNanoseconds; + greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; const double latency_wrt_current_timestamp_node_ms = static_cast(ros_node_system_time_us - current_timestamp_msg_us) / - message_diagnostics::constants::kMillisecondsToMicroseconds; + greenwave_diagnostics::constants::kMillisecondsToMicroseconds; if (prev_timestamp_msg_us_ != std::numeric_limits::min()) { const int64_t timestamp_diff_msg_us = current_timestamp_msg_us - prev_timestamp_msg_us_; @@ -161,7 +234,7 @@ class MessageDiagnostics // calculate key values for diagnostics status (computed on publish/getters) message_latency_msg_ms_ = latency_wrt_current_timestamp_node_ms; - if (message_latency_msg_ms_ > message_diagnostics::constants::kNonsenseLatencyMs) { + if (message_latency_msg_ms_ > greenwave_diagnostics::constants::kNonsenseLatencyMs) { message_latency_msg_ms_ = std::numeric_limits::quiet_NaN(); } @@ -176,9 +249,16 @@ class MessageDiagnostics // Function to publish diagnostics to a ROS topic void publishDiagnostics() { + // If the topic is not enabled, skip publishing diagnostics + if (!enabled_) { + RCLCPP_DEBUG_THROTTLE( + node_.get_logger(), *clock_, 1000, + "Topic %s is not enabled, skipping publish diagnostics", topic_name_.c_str()); + return; + } // Mutex lock to prevent simultaneous access of common parameters // used by updateDiagnostics() and publishDiagnostics() - const std::lock_guard lock(message_diagnostics_mutex_); + const std::lock_guard lock(greenwave_diagnostics_mutex_); std::vector values; // publish diagnostics stale if message has not been updated since the last call @@ -238,9 +318,9 @@ class MessageDiagnostics // timestamp from std::chrono (steady clock); split into sec/nanosec correctly const uint64_t elapsed_ns = static_cast((clock_->now() - t_start_).nanoseconds()); const uint32_t time_seconds = static_cast( - elapsed_ns / static_cast(message_diagnostics::constants::kSecondsToNanoseconds)); + elapsed_ns / static_cast(greenwave_diagnostics::constants::kSecondsToNanoseconds)); const uint32_t time_ns = static_cast( - elapsed_ns % static_cast(message_diagnostics::constants::kSecondsToNanoseconds)); + elapsed_ns % static_cast(greenwave_diagnostics::constants::kSecondsToNanoseconds)); diagnostic_msgs::msg::DiagnosticArray diagnostic_msg; std_msgs::msg::Header header; @@ -272,9 +352,7 @@ class MessageDiagnostics void setExpectedDt(double expected_hz, double tolerance_percent) { - const std::lock_guard lock(message_diagnostics_mutex_); - diagnostics_config_.enable_node_time_diagnostics = true; - diagnostics_config_.enable_msg_time_diagnostics = true; + const std::lock_guard lock(greenwave_diagnostics_mutex_); // This prevents accidental 0 division in the calculations in case of // a direct function call (not from service in greenwave_monitor.cpp) @@ -290,23 +368,28 @@ class MessageDiagnostics } const int64_t expected_dt_us = - static_cast(message_diagnostics::constants::kSecondsToMicroseconds / expected_hz); + static_cast(greenwave_diagnostics::constants::kSecondsToMicroseconds / expected_hz); diagnostics_config_.expected_dt_us = expected_dt_us; const int tolerance_us = - static_cast((message_diagnostics::constants::kSecondsToMicroseconds / expected_hz) * + static_cast((greenwave_diagnostics::constants::kSecondsToMicroseconds / expected_hz) * (tolerance_percent / 100.0)); diagnostics_config_.jitter_tolerance_us = tolerance_us; + + // Enable node and msg time diagnostics when expected frequency is set + diagnostics_config_.enable_node_time_diagnostics = true; + diagnostics_config_.enable_msg_time_diagnostics = true; } void clearExpectedDt() { - const std::lock_guard lock(message_diagnostics_mutex_); - diagnostics_config_.enable_node_time_diagnostics = false; - diagnostics_config_.enable_msg_time_diagnostics = false; - + const std::lock_guard lock(greenwave_diagnostics_mutex_); diagnostics_config_.expected_dt_us = 0; diagnostics_config_.jitter_tolerance_us = 0; + + // Disable node and msg time diagnostics when expected frequency is cleared + diagnostics_config_.enable_node_time_diagnostics = false; + diagnostics_config_.enable_msg_time_diagnostics = false; } private: @@ -352,7 +435,7 @@ class MessageDiagnostics if (sum_interarrival_us == 0 || interarrival_us.empty()) { return 0.0; } - return static_cast(message_diagnostics::constants::kSecondsToMicroseconds) / + return static_cast(greenwave_diagnostics::constants::kSecondsToMicroseconds) / (static_cast(sum_interarrival_us) / static_cast(interarrival_us.size())); } @@ -364,14 +447,24 @@ class MessageDiagnostics } return sum_jitter_abs_us / static_cast(jitter_abs_us.size()); } + + void clear() + { + interarrival_us = std::queue(); + sum_interarrival_us = 0; + jitter_abs_us = std::queue(); + sum_jitter_abs_us = 0; + max_abs_jitter_us = 0; + outlier_count = 0; + } }; // Mutex lock to prevent simultaneous access of common parameters // used by updateDiagnostics() and publishDiagnostics() - std::mutex message_diagnostics_mutex_; + std::mutex greenwave_diagnostics_mutex_; rclcpp::Node & node_; std::string topic_name_; - MessageDiagnosticsConfig diagnostics_config_; + GreenwaveDiagnosticsConfig diagnostics_config_; std::vector status_vec_; rclcpp::Clock::SharedPtr clock_; rclcpp::Time t_start_; @@ -390,7 +483,8 @@ class MessageDiagnostics { if (status.message.empty()) { status.message = update; - } else { + } else if (status.message.find(update) == std::string::npos) { + // Only append if not already present status.message.append(", ").append(update); } } @@ -412,7 +506,7 @@ class MessageDiagnostics if (missed_deadline_node) { RCLCPP_DEBUG( node_.get_logger(), - "[MessageDiagnostics Node Time]" + "[GreenwaveDiagnostics Node Time]" " Difference of time between messages(%" PRId64 ") and expected time between" " messages(%" PRId64 ") is out of tolerance(%" PRId64 ") by %" PRId64 " for topic %s." " Units are microseconds.", @@ -442,7 +536,7 @@ class MessageDiagnostics prev_drop_ts_ = clock_->now(); RCLCPP_DEBUG( node_.get_logger(), - "[MessageDiagnostics Message Timestamp]" + "[GreenwaveDiagnostics Message Timestamp]" " Difference of time between messages(%" PRId64 ") and expected " "time between" " messages(%" PRId64 ") is out of tolerance(%" PRId64 ") by %" PRId64 " for topic %s. " @@ -455,7 +549,7 @@ class MessageDiagnostics if (prev_drop_ts_.nanoseconds() != 0) { auto time_since_drop = (clock_->now() - prev_drop_ts_).seconds(); - if (time_since_drop < message_diagnostics::constants::kDropWarnTimeoutSeconds) { + if (time_since_drop < greenwave_diagnostics::constants::kDropWarnTimeoutSeconds) { error_found = true; status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; deadlines_missed_since_last_pub_ = 0; @@ -475,14 +569,14 @@ class MessageDiagnostics prev_noninc_msg_ts_ = clock_->now(); RCLCPP_WARN( node_.get_logger(), - "[MessageDiagnostics Message Timestamp Non Increasing]" + "[GreenwaveDiagnostics Message Timestamp Non Increasing]" " Message timestamp is not increasing. Current timestamp: %" PRIu64 ", " " Previous timestamp: %" PRIu64 " for topic %s. Units are microseconds.", current_timestamp_msg_us, prev_timestamp_msg_us_, topic_name_.c_str()); } if (prev_noninc_msg_ts_.nanoseconds() != 0) { auto time_since_noninc = (clock_->now() - prev_noninc_msg_ts_).seconds(); - if (time_since_noninc < message_diagnostics::constants::kDropWarnTimeoutSeconds) { + if (time_since_noninc < greenwave_diagnostics::constants::kDropWarnTimeoutSeconds) { error_found = true; status_vec_[0].level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; deadlines_missed_since_last_pub_ = 0; @@ -491,6 +585,191 @@ class MessageDiagnostics } return error_found; } + + rcl_interfaces::msg::SetParametersResult onParameterChange( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + std::optional new_freq; + std::optional new_tol; + std::optional new_enabled; + std::vector error_reasons; + + ////////////////////////////////////////////////////////////////////////////// + // Validate parameters + ////////////////////////////////////////////////////////////////////////////// + for (const auto & param : parameters) { + // Only handle parameters for this topic + if (param.get_name() != freq_param_name_ && param.get_name() != tol_param_name_ && + param.get_name() != enabled_param_name_) + { + continue; + } + + // Reject parameter deletion attempts + if (param.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + result.successful = false; + error_reasons.push_back(param.get_name() + ": parameter deletion not supported"); + continue; + } + + // Handle numeric types together + if (param.get_name() == freq_param_name_ || param.get_name() == tol_param_name_) { + auto value_opt = paramToDouble(param); + if (!value_opt.has_value()) { + result.successful = false; + error_reasons.push_back( + param.get_name() + + ": must be a numeric type (int or double or NaN)"); + continue; + } + + double value = value_opt.value(); + if (param.get_name() == freq_param_name_) { + if (value <= 0.0 && !std::isnan(value)) { + result.successful = false; + error_reasons.push_back(param.get_name() + ": must be positive or NaN"); + } + new_freq = value; + } else if (param.get_name() == tol_param_name_) { + if (value < 0.0) { + result.successful = false; + error_reasons.push_back(param.get_name() + ": must be non-negative"); + } + new_tol = value; + } + } else if (param.get_name() == enabled_param_name_) { + new_enabled = param.as_bool(); + } + } + + // Exit early if any parameters are invalid. No half changes. + if (!error_reasons.empty()) { + result.successful = false; + result.reason = rcpputils::join(error_reasons, "; "); + } + + // Execution of changes happens in onParameterEvent after parameters are committed + return result; + } + + void onParameterEvent(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) + { + // Only process events from this node + if (msg->node != node_.get_fully_qualified_name()) { + return; + } + + // Process changed and new parameters + bool freq_changed = false; + bool tol_changed = false; + + auto process_params = [&](const auto & params) { + for (const auto & param : params) { + applyParameterChange(param); + if (param.name == freq_param_name_) { + freq_changed = true; + } else if (param.name == tol_param_name_) { + tol_changed = true; + } + } + }; + process_params(msg->changed_parameters); + process_params(msg->new_parameters); + + // Update expected dt only if frequency or tolerance was explicitly changed + if (freq_changed || tol_changed) { + auto freq_opt = getNumericParameter(freq_param_name_); + auto tol_opt = getNumericParameter(tol_param_name_); + double freq = freq_opt.value_or(std::numeric_limits::quiet_NaN()); + double tol = tol_opt.value_or(constants::kDefaultTolerancePercent); + + if (std::isnan(freq) || freq <= 0.0) { + clearExpectedDt(); + } else { + setExpectedDt(freq, tol); + } + } + } + + void applyParameterChange(const rcl_interfaces::msg::Parameter & param) + { + if (param.name == enabled_param_name_) { + bool new_enabled = param.value.bool_value; + if (!new_enabled) { + // Clear windows when disabling diagnostics + const std::lock_guard lock(greenwave_diagnostics_mutex_); + node_window_.clear(); + msg_window_.clear(); + prev_timestamp_node_us_ = std::numeric_limits::min(); + prev_timestamp_msg_us_ = std::numeric_limits::min(); + } + enabled_ = new_enabled; + } + } + + std::optional getNumericParameter(const std::string & param_name) + { + if (!node_.has_parameter(param_name)) { + return std::nullopt; + } + return paramToDouble(node_.get_parameter(param_name)); + } + + static std::optional paramToDouble(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()); + } else if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + std::string str = param.as_string(); + if (str == "nan" || str == "NaN" || str == "NAN") { + return std::numeric_limits::quiet_NaN(); + } + } + return std::nullopt; + } + + template + void setParameterOrDefault(const std::string & param_name, const T & default_value) + { + if (node_.has_parameter(param_name)) { + auto current_param = node_.get_parameter(param_name); + if (current_param.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + auto result = node_.set_parameter(current_param); + if (result.successful) { + return; + } else { + RCLCPP_WARN( + node_.get_logger(), + "Initial parameter %s failed to set for topic %s: %s", + param_name.c_str(), topic_name_.c_str(), result.reason.c_str()); + } + } + } + auto result = node_.set_parameter(rclcpp::Parameter(param_name, default_value)); + if (!result.successful) { + RCLCPP_ERROR( + node_.get_logger(), + "Failed to set default value for parameter %s for topic %s", + param_name.c_str(), topic_name_.c_str()); + } + } + + // Parameter names for this topic + std::string freq_param_name_; + std::string tol_param_name_; + std::string enabled_param_name_; + + // Parameter callback handle + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + rclcpp::Subscription::SharedPtr param_event_subscription_; + + // Flag for indicating if message diagnostics are enabled for this topic + bool enabled_{true}; }; -} // namespace message_diagnostics +} // namespace greenwave_diagnostics diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index f9a39bb..4b24f12 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. @@ -20,9 +20,9 @@ #include #include #include -#include +#include #include -#include +#include #include #include "rclcpp/rclcpp.hpp" @@ -30,50 +30,82 @@ #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" -#include "message_diagnostics.hpp" -#include "greenwave_monitor_interfaces/srv/manage_topic.hpp" -#include "greenwave_monitor_interfaces/srv/set_expected_frequency.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "greenwave_diagnostics.hpp" + +struct TopicValidationResult +{ + bool valid = false; + std::string topic; + std::string message_type; + std::string error_message; +}; class GreenwaveMonitor : public rclcpp::Node { public: explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); + ~GreenwaveMonitor() + { + // Cancel timers first to stop callbacks from firing + if (timer_) { + timer_->cancel(); + } + if (init_timer_) { + init_timer_->cancel(); + } + // Clear diagnostics before base Node destructor runs to avoid accessing invalid node state + greenwave_diagnostics_.clear(); + subscriptions_.clear(); + } private: - std::optional find_topic_type_with_retry( - const std::string & topic, const int max_retries, const int retry_period_s); - void topic_callback( const std::shared_ptr msg, const std::string & topic, const std::string & type); void timer_callback(); - void handle_manage_topic( - const std::shared_ptr request, - std::shared_ptr response); + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters); + + void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg); + + void internal_on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg); - void handle_set_expected_frequency( - const std::shared_ptr request, - std::shared_ptr response); + void external_on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg); - bool add_topic(const std::string & topic, std::string & message); + TopicValidationResult validate_add_topic( + const std::string & topic, int max_retries = 5, double retry_period_s = 1.0); + + bool execute_add_topic(const TopicValidationResult & validated, std::string & message); + + void deferred_init(); + + void fetch_external_topic_map(); + + bool add_topic( + const std::string & topic, std::string & message, + int max_retries = 5, double retry_period_s = 1.0); bool remove_topic(const std::string & topic, std::string & message); bool has_header_from_type(const std::string & type_name); + std::set get_topics_from_parameters(); + std::chrono::time_point GetTimestampFromSerializedMessage( std::shared_ptr serialized_message_ptr, const std::string & type); std::map> message_diagnostics_; + std::unique_ptr> greenwave_diagnostics_; std::vector> subscriptions_; rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Service::SharedPtr - manage_topic_service_; - rclcpp::Service::SharedPtr - set_expected_frequency_service_; + rclcpp::TimerBase::SharedPtr init_timer_; + rclcpp::Subscription::SharedPtr param_event_sub_; + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + std::unordered_map pending_validations_; + std::unordered_map external_topic_to_node_; }; diff --git a/greenwave_monitor/include/minimal_publisher_node.hpp b/greenwave_monitor/include/minimal_publisher_node.hpp index fb3a38c..f1fcbb3 100644 --- a/greenwave_monitor/include/minimal_publisher_node.hpp +++ b/greenwave_monitor/include/minimal_publisher_node.hpp @@ -25,7 +25,7 @@ #include "sensor_msgs/msg/image.hpp" #include "sensor_msgs/msg/imu.hpp" #include "std_msgs/msg/string.hpp" -#include "message_diagnostics.hpp" +#include "greenwave_diagnostics.hpp" #include "rclcpp/subscription_options.hpp" using std::chrono_literals::operator""ms; @@ -35,13 +35,21 @@ class MinimalPublisher : public rclcpp::Node public: explicit MinimalPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~MinimalPublisher() + { + if (timer_) { + timer_->cancel(); + } + greenwave_diagnostics_.reset(); + } + private: void timer_callback(); rclcpp::TimerBase::SharedPtr timer_; rclcpp::PublisherBase::SharedPtr publisher_; rclcpp::SubscriptionBase::SharedPtr subscription_; - std::unique_ptr message_diagnostics_; + std::unique_ptr greenwave_diagnostics_; size_t count_; std::string message_type_; diff --git a/greenwave_monitor/package.xml b/greenwave_monitor/package.xml index 84b0407..87767b1 100644 --- a/greenwave_monitor/package.xml +++ b/greenwave_monitor/package.xml @@ -35,7 +35,6 @@ std_msgs diagnostic_msgs sensor_msgs - greenwave_monitor_interfaces rclpy launch diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e73fc65..f561939 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. @@ -17,69 +17,65 @@ #include "greenwave_monitor.hpp" -#include -#include -#include -#include +#include +#include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" using namespace std::chrono_literals; GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) -: Node("greenwave_monitor", options) +: Node("greenwave_monitor", + rclcpp::NodeOptions(options) + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { RCLCPP_INFO(this->get_logger(), "Starting GreenwaveMonitorNode"); - // Declare and get the topics parameter - this->declare_parameter>("topics", {""}); - auto topics = this->get_parameter("topics").as_string_array(); - - message_diagnostics::MessageDiagnosticsConfig diagnostics_config; - diagnostics_config.enable_all_topic_diagnostics = true; - - auto topic_names_and_types = this->get_topic_names_and_types(); - - for (const auto & topic : topics) { - if (!topic.empty()) { - std::string message; - add_topic(topic, message); - } + // Get the topics parameter (declare only if not already declared from overrides) + if (!this->has_parameter("topics")) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + this->declare_parameter>("topics", {""}, descriptor); } + // Timer callback to publish diagnostics and print feedback timer_ = this->create_wall_timer( 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); - // Add service server - manage_topic_service_ = - this->create_service( - "~/manage_topic", - std::bind( - &GreenwaveMonitor::handle_manage_topic, this, - std::placeholders::_1, std::placeholders::_2)); - - set_expected_frequency_service_ = - this->create_service( - "~/set_expected_frequency", - std::bind( - &GreenwaveMonitor::handle_set_expected_frequency, this, - std::placeholders::_1, std::placeholders::_2)); + // Defer topic discovery to allow the ROS graph to settle before querying other nodes + init_timer_ = this->create_wall_timer( + 0ms, [this]() { + init_timer_->cancel(); + deferred_init(); + }); } -std::optional GreenwaveMonitor::find_topic_type_with_retry( - const std::string & topic, const int max_retries, const int retry_period_s) +void GreenwaveMonitor::deferred_init() { - for (int i = 0; i < max_retries; ++i) { - auto topic_names_and_types = this->get_topic_names_and_types(); - auto it = topic_names_and_types.find(topic); - if (it == topic_names_and_types.end() || it->second.empty()) { - std::this_thread::sleep_for(std::chrono::seconds(retry_period_s)); - continue; - } else { - return it->second[0]; + fetch_external_topic_map(); + + std::set all_topics = get_topics_from_parameters(); + auto topics_param = this->get_parameter("topics").as_string_array(); + for (const auto & topic : topics_param) { + if (!topic.empty()) { + all_topics.insert(topic); } } - return std::nullopt; + + for (const auto & topic : all_topics) { + std::string message; + add_topic(topic, message); + } + + // Register parameter callbacks after initialization is complete + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&GreenwaveMonitor::on_parameter_change, this, std::placeholders::_1)); + + // Subscribe to parameter events to execute pending topic additions and track external monitoring + param_event_sub_ = this->create_subscription( + "/parameter_events", 10, + std::bind(&GreenwaveMonitor::on_parameter_event, this, std::placeholders::_1)); } void GreenwaveMonitor::topic_callback( @@ -87,16 +83,16 @@ void GreenwaveMonitor::topic_callback( const std::string & topic, const std::string & type) { auto msg_timestamp = GetTimestampFromSerializedMessage(msg, type); - message_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); + greenwave_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); } void GreenwaveMonitor::timer_callback() { RCLCPP_INFO(this->get_logger(), "===================================================="); - if (message_diagnostics_.empty()) { + if (greenwave_diagnostics_.empty()) { RCLCPP_INFO(this->get_logger(), "No topics to monitor"); } - for (auto & [topic, diagnostics] : message_diagnostics_) { + for (auto & [topic, diagnostics] : greenwave_diagnostics_) { diagnostics->publishDiagnostics(); RCLCPP_INFO( this->get_logger(), "Frame rate for topic %s: %.2f hz", @@ -108,65 +104,147 @@ void GreenwaveMonitor::timer_callback() RCLCPP_INFO(this->get_logger(), "===================================================="); } -void GreenwaveMonitor::handle_manage_topic( - const std::shared_ptr request, - std::shared_ptr response) +std::optional parse_enabled_topic_param(const std::string & name) { - if (request->add_topic) { - response->success = add_topic(request->topic_name, response->message); - } else { - response->success = remove_topic(request->topic_name, response->message); + const std::string prefix = greenwave_diagnostics::constants::kTopicParamPrefix; + const std::string suffix = greenwave_diagnostics::constants::kEnabledSuffix; + + if (name.rfind(prefix, 0) != 0) { + return std::nullopt; + } + if (name.size() <= prefix.size() + suffix.size()) { + return std::nullopt; + } + if (name.substr(name.size() - suffix.size()) != suffix) { + return std::nullopt; } + + std::string topic = name.substr( + prefix.size(), name.size() - prefix.size() - suffix.size()); + if (topic.empty() || topic[0] != '/') { + return std::nullopt; + } + + return topic; } -void GreenwaveMonitor::handle_set_expected_frequency( - const std::shared_ptr request, - std::shared_ptr response) +rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( + const std::vector & parameters) { - auto it = message_diagnostics_.find(request->topic_name); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + auto topic_opt = parse_enabled_topic_param(param.get_name()); + if (!topic_opt.has_value()) { + continue; + } - if (it == message_diagnostics_.end()) { - if (!request->add_topic_if_missing) { - response->success = false; - response->message = "Failed to find topic"; - return; + if (param.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + continue; } - if (!add_topic(request->topic_name, response->message)) { - response->success = false; - return; + const std::string & topic = topic_opt.value(); + bool enabled = param.as_bool(); + + if (enabled) { + // Skip if already monitored or already pending (prevents recursive additions) + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end() || + pending_validations_.find(topic) != pending_validations_.end()) + { + continue; + } + // Allow 1 retry with 0.5s wait for publisher discovery + auto validation = validate_add_topic(topic, 1, 0.5); + if (!validation.valid) { + result.successful = false; + result.reason = validation.error_message; + return result; + } + pending_validations_[topic] = validation; + } else { + if (external_topic_to_node_.find(topic) != external_topic_to_node_.end()) { + result.successful = false; + result.reason = "Topic being monitored by external node: " + external_topic_to_node_[topic]; + return result; + } + if (greenwave_diagnostics_.find(topic) == greenwave_diagnostics_.end()) { + result.successful = false; + result.reason = "Topic not being monitored: " + topic; + return result; + } } - it = message_diagnostics_.find(request->topic_name); } - message_diagnostics::MessageDiagnostics & msg_diagnostics_obj = *(it->second); + return result; +} - if (request->clear_expected) { - msg_diagnostics_obj.clearExpectedDt(); - response->success = true; - response->message = "Successfully cleared expected frequency for topic '" + - request->topic_name + "'"; - return; +void GreenwaveMonitor::on_parameter_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) +{ + if (msg->node == this->get_fully_qualified_name()) { + internal_on_parameter_event(msg); + } else { + external_on_parameter_event(msg); } +} - if (request->expected_hz <= 0.0) { - response->success = false; - response->message = "Invalid expected frequency, must be set to a positive value"; - return; - } - if (request->tolerance_percent < 0.0) { - response->success = false; - response->message = - "Invalid tolerance, must be a non-negative percentage"; - return; - } +void GreenwaveMonitor::internal_on_parameter_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) +{ + // Process new and changed parameters - execute pending topic additions and removals + auto process_params = [this](const auto & params) { + for (const auto & param : params) { + auto topic_opt = parse_enabled_topic_param(param.name); + if (!topic_opt.has_value()) { + continue; + } + + const std::string & topic = topic_opt.value(); + bool enabled = param.value.bool_value; + std::string message; + + if (enabled) { + auto it = pending_validations_.find(topic); + if (it != pending_validations_.end()) { + auto validation = std::move(it->second); + pending_validations_.erase(it); + execute_add_topic(validation, message); + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } + } else { + remove_topic(topic, message); + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } + } + }; + + process_params(msg->new_parameters); + process_params(msg->changed_parameters); +} - msg_diagnostics_obj.setExpectedDt(request->expected_hz, request->tolerance_percent); +void GreenwaveMonitor::external_on_parameter_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) +{ + // Process new parameters - track external nodes with enabled parameters + for (const auto & param : msg->new_parameters) { + auto topic_opt = parse_enabled_topic_param(param.name); + if (!topic_opt.has_value()) { + continue; + } + if (param.value.bool_value) { + external_topic_to_node_[topic_opt.value()] = msg->node; + } + } - 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) + "%"; + // Process deleted parameters - remove from external map + for (const auto & param : msg->deleted_parameters) { + auto topic_opt = parse_enabled_topic_param(param.name); + if (!topic_opt.has_value()) { + continue; + } + external_topic_to_node_.erase(topic_opt.value()); + } } bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) @@ -254,26 +332,74 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) return has_header; } -bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & message) +TopicValidationResult GreenwaveMonitor::validate_add_topic( + const std::string & topic, int max_retries, double retry_period_s) { - // Check if topic already exists - if (message_diagnostics_.find(topic) != message_diagnostics_.end()) { - message = "Topic already being monitored"; - return false; + TopicValidationResult result; + result.topic = topic; + + auto it = external_topic_to_node_.find(topic); + if (it != external_topic_to_node_.end()) { + result.error_message = "Topic already monitored by external node: " + it->second; + return result; } - RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { + result.error_message = "Topic already monitored: " + topic; + return result; + } - const int max_retries = 5; - const int retry_period_s = 1; - auto maybe_type = find_topic_type_with_retry(topic, max_retries, retry_period_s); - if (!maybe_type.has_value()) { - RCLCPP_ERROR(this->get_logger(), "Failed to find type for topic '%s'", topic.c_str()); - message = "Failed to find type for topic"; + std::vector publishers; + for (int attempt = 0; attempt <= max_retries; ++attempt) { + try { + publishers = this->get_publishers_info_by_topic(topic); + } catch (const rclcpp::exceptions::RCLError & e) { + // Context may be invalid during shutdown + result.error_message = "Node context invalid (shutting down)"; + return result; + } + if (!publishers.empty()) { + break; + } + if (attempt < max_retries) { + RCLCPP_INFO( + this->get_logger(), + "No publishers found for topic '%s', retrying in %.1fs (%d/%d)", + topic.c_str(), retry_period_s, attempt + 1, max_retries); + std::this_thread::sleep_for( + std::chrono::milliseconds(static_cast(retry_period_s * 1000))); + } + } + + if (publishers.empty()) { + result.error_message = "No publishers found for topic: " + topic; + return result; + } + + result.valid = true; + result.message_type = publishers[0].topic_type(); + return result; +} + +bool GreenwaveMonitor::execute_add_topic( + const TopicValidationResult & validated, std::string & message) +{ + if (!validated.valid) { + message = validated.error_message; return false; } - const std::string type = maybe_type.value(); + const std::string & topic = validated.topic; + const std::string & type = validated.message_type; + + // Guard against duplicate subscriptions from parameter re-set in GreenwaveDiagnostics + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { + message = "Topic already monitored: " + topic; + return true; + } + + RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); + auto sub = this->create_generic_subscription( topic, type, @@ -283,25 +409,39 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag this->topic_callback(msg, topic, type); }); - message_diagnostics::MessageDiagnosticsConfig diagnostics_config; + greenwave_diagnostics::GreenwaveDiagnosticsConfig diagnostics_config; diagnostics_config.enable_all_topic_diagnostics = true; subscriptions_.push_back(sub); - message_diagnostics_.emplace( + greenwave_diagnostics_.emplace( topic, - std::make_unique(*this, topic, diagnostics_config)); + std::make_unique( + *this, topic, diagnostics_config)); - message = "Successfully added topic"; + message = "Successfully added topic: " + topic; return true; } -bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & message) +bool GreenwaveMonitor::add_topic( + const std::string & topic, std::string & message, + int max_retries, double retry_period_s) { - auto diag_it = message_diagnostics_.find(topic); - if (diag_it == message_diagnostics_.end()) { - message = "Topic not found"; + auto validation = validate_add_topic(topic, max_retries, retry_period_s); + if (!validation.valid) { + RCLCPP_ERROR(this->get_logger(), "%s", validation.error_message.c_str()); + message = validation.error_message; return false; } + return execute_add_topic(validation, message); +} + +bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & message) +{ + auto diag_it = greenwave_diagnostics_.find(topic); + if (diag_it == greenwave_diagnostics_.end()) { + message = "Nothing to remove, topic not being monitored"; + return true; + } // Find and remove the subscription auto sub_it = std::find_if( @@ -314,11 +454,140 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes subscriptions_.erase(sub_it); } - message_diagnostics_.erase(diag_it); + // NOTE: the parameters are not removed when the diagnostics object is destroyed. This allows + // for settings to persist even when a topic is not available. + greenwave_diagnostics_.erase(diag_it); + message = "Successfully removed topic"; return true; } +void GreenwaveMonitor::fetch_external_topic_map() +{ + const std::string our_node = this->get_fully_qualified_name(); + const std::string prefix = greenwave_diagnostics::constants::kTopicParamPrefix; + const std::string suffix = greenwave_diagnostics::constants::kEnabledSuffix; + + static uint64_t temp_node_counter = 0; + rclcpp::NodeOptions temp_options; + temp_options.start_parameter_services(false); + temp_options.start_parameter_event_publisher(false); + auto temp_node = std::make_shared( + "_gw_init_" + std::to_string(temp_node_counter++), + "/_greenwave_internal", + temp_options); + + auto node_names = this->get_node_names(); + for (const auto & full_name : node_names) { + if (full_name == our_node) { + continue; + } + + auto param_client = std::make_shared( + temp_node, full_name); + if (!param_client->wait_for_service(std::chrono::milliseconds(100))) { + continue; + } + + std::vector param_names; + std::vector params; + try { + param_names = param_client->list_parameters({"greenwave_diagnostics"}, 10).names; + if (param_names.empty()) { + continue; + } + params = param_client->get_parameters(param_names); + } catch (const std::exception & e) { + RCLCPP_DEBUG( + this->get_logger(), + "Failed to query parameters from node '%s': %s", + full_name.c_str(), e.what()); + continue; + } + + for (size_t i = 0; i < param_names.size(); ++i) { + const auto & name = param_names[i]; + const auto & param = params[i]; + + if (name.rfind(prefix, 0) != 0) { + continue; + } + if (name.size() <= prefix.size() + suffix.size()) { + continue; + } + if (name.substr(name.size() - suffix.size()) != suffix) { + continue; + } + + std::string topic = name.substr( + prefix.size(), name.size() - prefix.size() - suffix.size()); + if (topic.empty() || topic[0] != '/') { + continue; + } + + if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL && param.as_bool()) { + external_topic_to_node_[topic] = full_name; + RCLCPP_DEBUG( + this->get_logger(), + "Found external monitoring for topic '%s' on node '%s'", + topic.c_str(), full_name.c_str()); + } + } + } +} + +std::set GreenwaveMonitor::get_topics_from_parameters() +{ + std::set topics; + + // List all parameters with "greenwave_diagnostics." prefix + auto list_result = this->list_parameters({"greenwave_diagnostics"}, 10); + + for (const auto & param_name : list_result.names) { + // Parameter names are like "greenwave_diagnostics./my_topic.enabled" + // We need to extract the topic name (e.g., "/my_topic") + if (param_name.find(greenwave_diagnostics::constants::kTopicParamPrefix) != 0) { + continue; + } + + // Remove the "greenwave_diagnostics." prefix + std::string remainder = param_name.substr( + std::strlen(greenwave_diagnostics::constants::kTopicParamPrefix)); + + // Find the last '.' to separate topic name from parameter suffix + // Topic names can contain '/' but parameter suffixes are like ".enabled", ".tolerance", etc. + size_t last_dot = remainder.rfind('.'); + if (last_dot == std::string::npos || last_dot == 0) { + continue; + } + + std::string topic_name = remainder.substr(0, last_dot); + if (!topic_name.empty() && topic_name[0] == '/') { + topics.insert(topic_name); + } + } + + // Filter out topics with enabled=false + std::set filtered_topics; + for (const auto & topic : topics) { + std::string enabled_param = + std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + + topic + greenwave_diagnostics::constants::kEnabledSuffix; + + if (this->has_parameter(enabled_param)) { + auto param = this->get_parameter(enabled_param); + if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL && + !param.as_bool()) + { + continue; + } + } + filtered_topics.insert(topic); + } + + return filtered_topics; +} + // From ros2_benchmark monitor_node.cpp // This assumes the message has a std_msgs header as the first std::chrono::time_point diff --git a/greenwave_monitor/src/greenwave_monitor_main.cpp b/greenwave_monitor/src/greenwave_monitor_main.cpp index 5be5a58..537bd1b 100644 --- a/greenwave_monitor/src/greenwave_monitor_main.cpp +++ b/greenwave_monitor/src/greenwave_monitor_main.cpp @@ -22,7 +22,15 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); rclcpp::NodeOptions options; auto node = std::make_shared(options); - rclcpp::spin(node); + if (rclcpp::ok()) { + try { + rclcpp::spin(node); + } catch (const rclcpp::exceptions::RCLError & e) { + RCLCPP_DEBUG( + node->get_logger(), + "Context became invalid during spin (expected during shutdown): %s", e.what()); + } + } rclcpp::shutdown(); return 0; } diff --git a/greenwave_monitor/src/minimal_publisher_node.cpp b/greenwave_monitor/src/minimal_publisher_node.cpp index e7de807..bb3db0b 100644 --- a/greenwave_monitor/src/minimal_publisher_node.cpp +++ b/greenwave_monitor/src/minimal_publisher_node.cpp @@ -25,12 +25,15 @@ MinimalPublisher::MinimalPublisher(const rclcpp::NodeOptions & options) this->declare_parameter("frequency_hz", 1.0); this->declare_parameter("message_type", "imu"); this->declare_parameter("create_subscriber", false); + this->declare_parameter("enable_greenwave_diagnostics", true); const auto topic = this->get_parameter("topic").as_string(); const auto frequency_hz = this->get_parameter("frequency_hz").as_double(); const auto period_ns = static_cast( - ::message_diagnostics::constants::kSecondsToNanoseconds / frequency_hz); + ::greenwave_diagnostics::constants::kSecondsToNanoseconds / frequency_hz); const auto create_subscriber = this->get_parameter("create_subscriber").as_bool(); + const auto enable_greenwave_diagnostics = + this->get_parameter("enable_greenwave_diagnostics").as_bool(); message_type_ = this->get_parameter("message_type").as_string(); // Validate message type @@ -82,10 +85,12 @@ MinimalPublisher::MinimalPublisher(const rclcpp::NodeOptions & options) timer_ = this->create_wall_timer( std::chrono::nanoseconds(period_ns), std::bind(&MinimalPublisher::timer_callback, this)); - message_diagnostics::MessageDiagnosticsConfig diagnostics_config; - diagnostics_config.enable_all_topic_diagnostics = true; - message_diagnostics_ = std::make_unique( - *this, topic, diagnostics_config); + if (enable_greenwave_diagnostics) { + greenwave_diagnostics::GreenwaveDiagnosticsConfig diagnostics_config; + diagnostics_config.enable_all_topic_diagnostics = true; + greenwave_diagnostics_ = std::make_unique( + *this, topic, diagnostics_config); + } } void MinimalPublisher::timer_callback() @@ -137,7 +142,13 @@ void MinimalPublisher::timer_callback() message); } - const auto msg_timestamp = this->now(); - message_diagnostics_->updateDiagnostics(msg_timestamp.nanoseconds()); - // message_diagnostics_->publishDiagnostics(); + if (greenwave_diagnostics_) { + // Use actual message timestamp for types with headers, 0 for headerless types + uint64_t msg_timestamp_ns = 0; + if (message_type_ != "string") { + msg_timestamp_ns = this->now().nanoseconds(); + } + greenwave_diagnostics_->updateDiagnostics(msg_timestamp_ns); + greenwave_diagnostics_->publishDiagnostics(); + } } diff --git a/greenwave_monitor/test/test_greenwave_diagnostics.cpp b/greenwave_monitor/test/test_greenwave_diagnostics.cpp new file mode 100644 index 0000000..9951aad --- /dev/null +++ b/greenwave_monitor/test/test_greenwave_diagnostics.cpp @@ -0,0 +1,916 @@ +// 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 + +/** +Unit tests for functionality in greenwave_diagnostics.hpp, +such as frame rate and latency calculation accuracy. +**/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "greenwave_diagnostics.hpp" + +namespace test_constants +{ +inline constexpr uint64_t kMillisecondsToSeconds = 1000ULL; +inline constexpr uint64_t kStartTimestampNs = 10000000ULL; + +// Short aliases for parameter suffixes +constexpr auto kFreq = greenwave_diagnostics::constants::kFreqSuffix; +constexpr auto kTol = greenwave_diagnostics::constants::kTolSuffix; +constexpr auto kEnabled = greenwave_diagnostics::constants::kEnabledSuffix; + +inline std::string makeParam(const std::string & topic, const char * suffix) +{ + return std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + topic + suffix; +} +} // namespace test_constants + +class GreenwaveDiagnosticsTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() + { + rclcpp::shutdown(); + } + + void SetUp() override + { + node_ = std::make_shared("test_node"); + } + + void TearDown() override + { + received_diagnostics_.clear(); + diagnostic_subscription_.reset(); + node_.reset(); + } + + void recreateNode() + { + received_diagnostics_.clear(); + diagnostic_subscription_.reset(); + node_.reset(); + node_ = std::make_shared("test_node"); + } + + void setupDiagnosticSubscription() + { + received_diagnostics_.clear(); + diagnostic_subscription_ = + node_->create_subscription( + "/diagnostics", 10, + [this](const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) { + received_diagnostics_.push_back(msg); + }); + } + + void spinAndWait(int iterations = 5, int sleep_ms = 10) + { + for (int i = 0; i < iterations; i++) { + rclcpp::spin_some(node_); + std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms)); + } + } + + double getDiagnosticValue(const std::string & key) const + { + if (received_diagnostics_.empty() || + received_diagnostics_.back()->status.empty()) + { + return 0.0; + } + for (const auto & val : received_diagnostics_.back()->status[0].values) { + if (val.key == key) { + return std::stod(val.value); + } + } + return 0.0; + } + + bool hasDiagnosticMessage(const std::string & message_substring) const + { + for (const auto & diag : received_diagnostics_) { + if (!diag->status.empty() && + diag->status[0].message.find(message_substring) != std::string::npos) + { + return true; + } + } + return false; + } + + bool hasErrorStatusForTopic(const std::string & topic_name) const + { + for (const auto & diag : received_diagnostics_) { + for (const auto & status : diag->status) { + if (status.name.find(topic_name) != std::string::npos && + status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + return true; + } + } + } + return false; + } + + template + void setParam(const std::string & param_name, const T & value) + { + auto result = node_->set_parameter(rclcpp::Parameter(param_name, value)); + EXPECT_TRUE(result.successful) << "Failed to set " << param_name << ": " << result.reason; + } + + template + void setParamFail( + const std::string & param_name, + const T & value, + const std::string & expected_error) + { + auto result = node_->set_parameter(rclcpp::Parameter(param_name, value)); + EXPECT_FALSE(result.successful) << "Expected " << param_name << " to fail"; + EXPECT_TRUE(result.reason.find(expected_error) != std::string::npos) + << "Expected error containing '" << expected_error << "', got: " << result.reason; + } + + void testBooleanParameterAcceptance(const std::string & param_name) + { + setParam(param_name, true); + setParam(param_name, false); + } + + // Send messages, optionally with publishing, jitter, sleep, and interval override callback + // Callback signature: int64_t(int iteration, int64_t default_interval) -> interval to use + void sendMessages( + greenwave_diagnostics::GreenwaveDiagnostics & diag, + uint64_t & timestamp, + int64_t interval_ns, + int count, + bool publish = false, + int64_t jitter_ns = 0, + int sleep_ms = 0, + const std::function & interval_override = nullptr) + { + for (int i = 0; i < count; i++) { + diag.updateDiagnostics(timestamp); + if (publish) { + diag.publishDiagnostics(); + rclcpp::spin_some(node_); + } + int64_t jitter = (jitter_ns != 0) ? ((i % 2 == 0) ? jitter_ns : -jitter_ns) : 0; + int64_t interval = interval_override ? interval_override(i, interval_ns) : interval_ns; + timestamp += interval + jitter; + if (sleep_ms > 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms)); + } + } + } + + std::shared_ptr node_; + std::vector received_diagnostics_; + rclcpp::Subscription::SharedPtr diagnostic_subscription_; +}; + +TEST_F(GreenwaveDiagnosticsTest, FrameRateMsgTest) +{ + greenwave_diagnostics::GreenwaveDiagnostics diag( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); + + uint64_t timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, timestamp, 10000000, 1000); // 10 ms = 100 Hz + EXPECT_EQ(diag.getFrameRateMsg(), 100); // 100 Hz +} + +TEST_F(GreenwaveDiagnosticsTest, FrameRateNodeTest) +{ + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); + + // dummy timestamp, not used for node time calculation + constexpr auto timestamp = test_constants::kStartTimestampNs; + const auto start_time = std::chrono::high_resolution_clock::now(); + + constexpr int num_messages = 1000; + constexpr int interarrival_time_ms = 10; // 100 hz + + for (int i = 0; i < num_messages; i++) { + greenwave_diagnostics.updateDiagnostics(timestamp); + std::this_thread::sleep_for(std::chrono::milliseconds(interarrival_time_ms)); + } + + const auto end_time = std::chrono::high_resolution_clock::now(); + const std::chrono::duration total_duration = end_time - start_time; + + const double expected_frame_rate = static_cast(num_messages) / total_duration.count(); + + // allow 2.0 Hz error + EXPECT_NEAR(greenwave_diagnostics.getFrameRateNode(), expected_frame_rate, 2.0); +} + +TEST_F(GreenwaveDiagnosticsTest, MessageLatencyTest) +{ + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); + + const rclcpp::Time current_time = node_->get_clock()->now(); + // Make message timestamp a certain amount of time earlier than current time + constexpr double expected_latency_ms = 10.0; + const rclcpp::Time msg_timestamp = + current_time - rclcpp::Duration::from_seconds( + expected_latency_ms / static_cast(test_constants::kMillisecondsToSeconds)); + + greenwave_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); + + // allow 1 ms tolerance + EXPECT_NEAR(greenwave_diagnostics.getLatency(), expected_latency_ms, 1.0); +} + +TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) +{ + constexpr int input_frequency = 50; // 50 Hz + // 20 ms in nanoseconds + const int64_t interarrival_time_ns = static_cast( + ::greenwave_diagnostics::constants::kSecondsToNanoseconds / input_frequency); + + // Initialize GreenwaveDiagnostics with diagnostics enabled + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + config.enable_node_time_diagnostics = true; + config.enable_increasing_msg_time_diagnostics = true; + // in us + config.expected_dt_us = interarrival_time_ns / + ::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; + + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics(*node_, "test_topic", config); + + // Create a subscriber to receive diagnostic messages + std::vector received_diagnostics; + const auto diagnostic_subscription = + node_->create_subscription( + "/diagnostics", 10, + [&received_diagnostics](const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) { + received_diagnostics.push_back(msg); + }); + + // 50 ms delay + constexpr int64_t delay_time_ns = 50 * + static_cast(::greenwave_diagnostics::constants::kMillisecondsToMicroseconds) * + static_cast(::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds); + // Starting message timestamp in nanoseconds + auto msg_timestamp = test_constants::kStartTimestampNs; + + int sent_count = 0; + const auto start_time = std::chrono::high_resolution_clock::now(); + + // Send 100 messages + constexpr int num_messages = 100; + while (sent_count < num_messages) { + if (sent_count != 0) { + msg_timestamp += interarrival_time_ns; + } + + sent_count++; + + greenwave_diagnostics.updateDiagnostics(msg_timestamp); + greenwave_diagnostics.publishDiagnostics(); + + // Add a non-increasing timestamp at count 5 + if (sent_count == 5) { + msg_timestamp -= interarrival_time_ns; + } + // Add a jitter by delaying at count 10 + if (sent_count == 10) { + std::this_thread::sleep_for(std::chrono::nanoseconds(delay_time_ns)); // 50 ms delay + msg_timestamp += delay_time_ns; + } + + rclcpp::spin_some(node_); + + std::this_thread::sleep_for(std::chrono::nanoseconds(interarrival_time_ns)); + } + + ASSERT_EQ(received_diagnostics.size(), num_messages); + + const int interarrival_time_count = sent_count - 1; + // Calculate expected node and message frame rates + const auto actual_end_time = std::chrono::high_resolution_clock::now(); + const std::chrono::duration total_duration = actual_end_time - start_time; + const double expected_frame_rate_node = static_cast(interarrival_time_count) / + total_duration.count(); + + const auto sum_interarrival_time_msg_sec = static_cast( + msg_timestamp - test_constants::kStartTimestampNs) / + static_cast(::greenwave_diagnostics::constants::kSecondsToNanoseconds); + const double expected_frame_rate_msg = + static_cast(interarrival_time_count) / sum_interarrival_time_msg_sec; + + // Verify that we received diagnostic messages + ASSERT_FALSE(received_diagnostics.empty()); + + // Use the last diagnostic message + const auto & last_diagnostic = received_diagnostics.back(); + ASSERT_FALSE(last_diagnostic->status.empty()); + + // Verify the diagnostic status information + const auto & diagnostic_status = last_diagnostic->status[0]; + EXPECT_TRUE(diagnostic_status.name.find("test_topic") != std::string::npos); + EXPECT_EQ(diagnostic_status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(diagnostic_status.message, "FRAME DROP DETECTED, NONINCREASING TIMESTAMP"); + + // Parse diagnostic values + std::map diagnostics_values = { + {"frame_rate_node", 0.0}, + {"num_non_increasing_msg", 0.0}, + {"num_jitter_outliers_msg", 0.0}, + {"num_jitter_outliers_node", 0.0}, + {"max_abs_jitter_msg", 0.0}, + {"max_abs_jitter_node", 0.0}, + {"mean_abs_jitter_msg", 0.0}, + {"mean_abs_jitter_node", 0.0}, + {"frame_rate_msg", 0.0}, + {"total_dropped_frames", 0.0} + }; + for (const auto & value : diagnostic_status.values) { + if (diagnostics_values.find(value.key) != diagnostics_values.end()) { + diagnostics_values[value.key] = std::stod(value.value); + } + } + + // Sometimes diagnostics may arrive out of order, so we use getter methods instead of values from + // the last diagnostic message to prevent flakiness + EXPECT_NEAR(greenwave_diagnostics.getFrameRateNode(), expected_frame_rate_node, 1.0); + // Allow small floating point differences for frame rate msg + constexpr double frame_rate_msg_tolerance = 0.001; + EXPECT_NEAR( + greenwave_diagnostics.getFrameRateMsg(), expected_frame_rate_msg, frame_rate_msg_tolerance); + + // Sometimes diagnostics may arrive out of order, so we need to check all received diagnostics + // to see if the expected msg frame rate is somewhere in there + double smallest_msg_frame_rate_diff = std::numeric_limits::infinity(); + for (const auto & diag_msg : received_diagnostics) { + if (diag_msg->status.empty()) { + continue; + } + const auto & status = diag_msg->status[0]; + double frame_rate_msg = 0.0; + for (const auto & value : status.values) { + if (value.key == "frame_rate_msg") { + frame_rate_msg = std::stod(value.value); + break; + } + } + if (std::abs(frame_rate_msg - expected_frame_rate_msg) < smallest_msg_frame_rate_diff) { + smallest_msg_frame_rate_diff = std::abs(frame_rate_msg - expected_frame_rate_msg); + } + } + + EXPECT_LT(smallest_msg_frame_rate_diff, frame_rate_msg_tolerance); + + // Diagnostics should have at least one jitter due to the intentional delay + // possibly more if the system was very busy + EXPECT_GE(diagnostics_values["num_jitter_outliers_node"], 1.0); + EXPECT_GE(diagnostics_values["max_abs_jitter_node"], 0.0); + EXPECT_GE(diagnostics_values["mean_abs_jitter_node"], 0.0); + + EXPECT_GE(diagnostics_values["num_jitter_outliers_msg"], 1.0); + EXPECT_GE(diagnostics_values["max_abs_jitter_msg"], 0.0); + EXPECT_GE(diagnostics_values["mean_abs_jitter_msg"], 0.0); + + EXPECT_GE(diagnostics_values["total_dropped_frames"], 1.0); + EXPECT_GE(diagnostics_values["num_non_increasing_msg"], 1.0); +} + +TEST_F(GreenwaveDiagnosticsTest, InvalidFrequencyParameterTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + const std::string freq_param = test_constants::makeParam("test_topic", test_constants::kFreq); + + // Test negative frequency is rejected + setParamFail(freq_param, -10.0, "must be positive"); + + // Test zero frequency is rejected + setParamFail(freq_param, 0.0, "must be positive"); + + // Test NaN is accepted (clears the expected frequency) + setParam(freq_param, std::numeric_limits::quiet_NaN()); + + // Test positive frequency is accepted + setParam(freq_param, 30.0); +} + +TEST_F(GreenwaveDiagnosticsTest, InvalidToleranceParameterTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + const std::string tol_param = test_constants::makeParam("test_topic", test_constants::kTol); + + // Test negative tolerance is rejected + setParamFail(tol_param, -5.0, "must be non-negative"); + + // Test zero tolerance is accepted + setParam(tol_param, 0.0); + + // Test positive tolerance is accepted + setParam(tol_param, 10.0); +} + +TEST_F(GreenwaveDiagnosticsTest, EnabledParameterImpactTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic", config); + + const std::string enabled_param = + test_constants::makeParam("test_topic", test_constants::kEnabled); + constexpr int64_t interval_ns = 10000000; // 10 ms + + // Send messages while enabled (default) + uint64_t timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, timestamp, interval_ns, 100); + EXPECT_GT(diag.getFrameRateMsg(), 0.0); + + // Disable diagnostics + setParam(enabled_param, false); + spinAndWait(10); + + // Frame rate should be cleared after disabling (windows are cleared) + EXPECT_EQ(diag.getFrameRateMsg(), 0.0); + + // Send more messages while disabled (they should be ignored) + sendMessages(diag, timestamp, interval_ns, 100); + EXPECT_EQ(diag.getFrameRateMsg(), 0.0); + + // Re-enable diagnostics + setParam(enabled_param, true); + spinAndWait(10); + + // Send messages while re-enabled + sendMessages(diag, timestamp, interval_ns, 100); + EXPECT_GT(diag.getFrameRateMsg(), 0.0); +} + +TEST_F(GreenwaveDiagnosticsTest, EnableMsgTimeConfigImpactTest) +{ + // Test that enable_msg_time_diagnostics config controls frame drop detection + constexpr int input_frequency = 50; + const int64_t interarrival_time_ns = static_cast( + ::greenwave_diagnostics::constants::kSecondsToNanoseconds / input_frequency); + + // Large delay to trigger frame drop (100ms when expecting ~20ms) + constexpr int64_t large_delay_ns = 100 * + static_cast(::greenwave_diagnostics::constants::kMillisecondsToMicroseconds) * + static_cast(::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds); + + // Test with msg_time diagnostics ENABLED - should detect frame drop + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + config.expected_dt_us = interarrival_time_ns / + ::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; + config.jitter_tolerance_us = 1000; + + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic", config); + + setupDiagnosticSubscription(); + + // Lambda to inject large delay at iteration 10 + auto delay_at_10 = [large_delay_ns](int i, int64_t default_interval) { + return (i == 10) ? large_delay_ns : default_interval; + }; + + auto msg_timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, msg_timestamp, interarrival_time_ns, 20, true, 0, 0, delay_at_10); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_TRUE(hasErrorStatusForTopic("test_topic")) + << "ERROR status should occur with msg_time enabled when frame drop happens"; +} + +TEST_F(GreenwaveDiagnosticsTest, EnableIncreasingMsgTimeConfigImpactTest) +{ + // Test that enable_increasing_msg_time_diagnostics config controls detection + constexpr int64_t normal_interval_ns = 10000000ULL; + constexpr int64_t decrement_ns = -500000000LL; + + // Lambda to decrement timestamp at iteration 10 + auto decrement_at_10 = [decrement_ns](int i, int64_t default_interval) { + return (i == 10) ? decrement_ns : default_interval; + }; + + // Test with increasing msg time diagnostics DISABLED + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_increasing_msg_time_diagnostics = false; + + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_disabled", config); + + setupDiagnosticSubscription(); + + uint64_t msg_timestamp = test_constants::kStartTimestampNs + 1000000000ULL; + sendMessages(diag, msg_timestamp, normal_interval_ns, 20, true, 0, 0, decrement_at_10); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_FALSE(hasDiagnosticMessage("NONINCREASING")) + << "NONINCREASING should NOT be detected with increasing_msg_time disabled"; + } + + recreateNode(); + + // Test with increasing msg time diagnostics ENABLED + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_increasing_msg_time_diagnostics = true; + + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_enabled", config); + + setupDiagnosticSubscription(); + + uint64_t msg_timestamp = test_constants::kStartTimestampNs + 1000000000ULL; + sendMessages(diag, msg_timestamp, normal_interval_ns, 20, true, 0, 0, decrement_at_10); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_TRUE(hasDiagnosticMessage("NONINCREASING")) + << "NONINCREASING should be detected with increasing_msg_time enabled"; + } +} + +TEST_F(GreenwaveDiagnosticsTest, FrequencyParameterAffectsJitterCalculationTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + const std::string freq_param = test_constants::makeParam("test_topic", test_constants::kFreq); + const std::string tol_param = test_constants::makeParam("test_topic", test_constants::kTol); + + setupDiagnosticSubscription(); + + // Set expected frequency to 100 Hz (10ms between messages) with tight tolerance + setParam(freq_param, 100.0); + setParam(tol_param, 1.0); // 1% tolerance + + // Send messages at exactly 100 Hz (should be within tolerance) + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + constexpr int64_t interarrival_100hz_ns = 10000000; // 10ms + + sendMessages(greenwave_diagnostics, msg_timestamp, interarrival_100hz_ns, 50, true, 0, 10); + + // Should not detect frame drops with correct frequency + EXPECT_FALSE(hasDiagnosticMessage("FRAME DROP")); + + received_diagnostics_.clear(); + + // Now change to expect 50 Hz, but keep sending at 100 Hz + // This should trigger jitter detection since messages arrive too fast + setParam(freq_param, 50.0); + + sendMessages(greenwave_diagnostics, msg_timestamp, interarrival_100hz_ns, 50, true, 0, 10); + + // Should detect frame drops now due to frequency mismatch + EXPECT_TRUE(hasDiagnosticMessage("FRAME DROP")); +} + +TEST_F(GreenwaveDiagnosticsTest, ToleranceParameterAffectsJitterDetectionTest) +{ + // Test that tolerance parameter controls how sensitive jitter detection is + const std::string freq_param = test_constants::makeParam("test_topic", test_constants::kFreq); + const std::string tol_param = test_constants::makeParam("test_topic", test_constants::kTol); + + constexpr int64_t base_interval_ns = 10000000; // 10ms = 100 Hz + constexpr int64_t jitter_ns = 500000; // 0.5ms jitter (5% of interval) + + // Test with tight tolerance (1%) - jitter should exceed tolerance + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic", config); + + setParam(freq_param, 100.0); + setParam(tol_param, 1.0); // 1% tolerance = 100us + rclcpp::spin_some(node_); + + setupDiagnosticSubscription(); + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, msg_timestamp, base_interval_ns, 30, true, jitter_ns); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_GT(getDiagnosticValue("num_jitter_outliers_msg"), 0.0) + << "Expected jitter outliers with 1% tolerance and 5% jitter"; + } + + recreateNode(); + + // Test with loose tolerance (50%) - same jitter should not exceed tolerance + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic", config); + + setParam(freq_param, 100.0); + setParam(tol_param, 50.0); // 50% tolerance = 5000us + rclcpp::spin_some(node_); + + setupDiagnosticSubscription(); + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, msg_timestamp, base_interval_ns, 30, true, jitter_ns); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_EQ(getDiagnosticValue("num_jitter_outliers_msg"), 0.0) + << "Expected no jitter outliers with 50% tolerance and 5% jitter"; + } +} + +TEST_F(GreenwaveDiagnosticsTest, BooleanParameterAcceptanceTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + // Test enabled boolean parameter accepts valid values + testBooleanParameterAcceptance( + test_constants::makeParam("test_topic", test_constants::kEnabled)); +} + +TEST_F(GreenwaveDiagnosticsTest, FrequencyNaNClearsExpectedDtTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + const std::string freq_param = test_constants::makeParam("test_topic", test_constants::kFreq); + const std::string tol_param = test_constants::makeParam("test_topic", test_constants::kTol); + + setupDiagnosticSubscription(); + + // Set frequency first + setParam(freq_param, 100.0); + setParam(tol_param, 1.0); + spinAndWait(); + + // Send messages at wrong frequency to trigger frame drop detection + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + constexpr int64_t wrong_interval_ns = 50000000; // 50ms = 20 Hz instead of 100 Hz + + sendMessages(greenwave_diagnostics, msg_timestamp, wrong_interval_ns, 20, true); + + EXPECT_TRUE(hasDiagnosticMessage("FRAME DROP")); + + // Clear diagnostics and set frequency to NaN + received_diagnostics_.clear(); + setParam(freq_param, std::numeric_limits::quiet_NaN()); + spinAndWait(); + + // Send more messages at wrong frequency + sendMessages(greenwave_diagnostics, msg_timestamp, wrong_interval_ns, 20, true); + + // Should NOT detect frame drops after clearing with NaN + EXPECT_FALSE(hasDiagnosticMessage("FRAME DROP")); +} + +TEST_F(GreenwaveDiagnosticsTest, ParameterBoundaryValuesTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + const std::string freq_param = test_constants::makeParam("test_topic", test_constants::kFreq); + const std::string tol_param = test_constants::makeParam("test_topic", test_constants::kTol); + + // Tolerance boundary values + setParam(tol_param, 0.0); + setParam(tol_param, 1000.0); + setParam(tol_param, 0.001); + + // Frequency boundary values + setParam(freq_param, 0.001); + setParam(freq_param, 10000.0); + setParam(freq_param, std::numeric_limits::infinity()); +} + +TEST_F(GreenwaveDiagnosticsTest, FilterWindowSizeImpactTest) +{ + constexpr int64_t interval_100hz_ns = 10000000; // 10 ms = 100 Hz + constexpr int64_t interval_50hz_ns = 20000000; // 20 ms = 50 Hz + + // Test with small window size - frame rate should stabilize quickly + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.filter_window_size = 10; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_small", config); + + uint64_t timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, timestamp, interval_100hz_ns, 15); + + EXPECT_NEAR(diag.getFrameRateMsg(), 100.0, 1.0); + } + + // Test with large window size - frame rate calculation includes more history + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.filter_window_size = 100; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_large", config); + + uint64_t timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, timestamp, interval_100hz_ns, 50); // 100 Hz + sendMessages(diag, timestamp, interval_50hz_ns, 50); // 50 Hz + + // With large window, frame rate should be between 50 and 100 Hz + double frame_rate = diag.getFrameRateMsg(); + EXPECT_GT(frame_rate, 50.0); + EXPECT_LT(frame_rate, 100.0); + } +} + +TEST_F(GreenwaveDiagnosticsTest, FilterWindowSizeSmallVsLargeStabilityTest) +{ + constexpr int64_t interarrival_time_ns = 10000000; // 10ms = 100 Hz + constexpr int64_t jitter_ns = 2000000; // 2ms jitter + + // Small window: more sensitive to recent jitter + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.filter_window_size = 5; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_jitter_small", config); + + uint64_t timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, timestamp, interarrival_time_ns, 20); + double stable_rate = diag.getFrameRateMsg(); + + // Send jittery messages + sendMessages(diag, timestamp, interarrival_time_ns, 10, false, jitter_ns); + double jittery_rate = diag.getFrameRateMsg(); + + // Small window should show more variation + EXPECT_GT(std::abs(stable_rate - jittery_rate), 0.0); + } + + // Large window: more stable despite recent jitter + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.filter_window_size = 100; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_jitter_large", config); + + uint64_t timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, timestamp, interarrival_time_ns, 50); + double stable_rate = diag.getFrameRateMsg(); + + // Send jittery messages + sendMessages(diag, timestamp, interarrival_time_ns, 10, false, jitter_ns); + double jittery_rate = diag.getFrameRateMsg(); + + // Large window should show less variation (more stable) + EXPECT_LT(std::abs(stable_rate - jittery_rate), 5.0); + } +} + +TEST_F(GreenwaveDiagnosticsTest, ToleranceZeroDetectsAllJitterTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic", config); + + const std::string freq_param = test_constants::makeParam("test_topic", test_constants::kFreq); + const std::string tol_param = test_constants::makeParam("test_topic", test_constants::kTol); + + setParam(freq_param, 100.0); + setParam(tol_param, 0.0); + spinAndWait(); + + setupDiagnosticSubscription(); + + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + constexpr int64_t interarrival_100hz_ns = 10000000; // 10ms + constexpr int64_t tiny_jitter_ns = 1000; // 1 microsecond + sendMessages(diag, msg_timestamp, interarrival_100hz_ns, 30, true, tiny_jitter_ns); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_GT(getDiagnosticValue("num_jitter_outliers_msg"), 0.0) + << "Zero tolerance should detect even tiny jitter"; +} + +TEST_F(GreenwaveDiagnosticsTest, FrequencyIntegerParameterTest) +{ + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + const std::string freq_param = test_constants::makeParam("test_topic", test_constants::kFreq); + const std::string tol_param = test_constants::makeParam("test_topic", test_constants::kTol); + + // Test integer frequency is accepted and converted to double + setParam(freq_param, 100); + + // Test integer tolerance is accepted and converted to double + setParam(tol_param, 5); + spinAndWait(); + + setupDiagnosticSubscription(); + + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + constexpr int64_t interarrival_100hz_ns = 10000000; + + sendMessages(greenwave_diagnostics, msg_timestamp, interarrival_100hz_ns, 20, true); + + // Should not have errors with matching frequency + EXPECT_FALSE(hasErrorStatusForTopic("test_topic")); +} + +TEST_F(GreenwaveDiagnosticsTest, ExpectedDtUsConfigImpactTest) +{ + // Test that expected_dt_us in config affects jitter calculations + constexpr int64_t expected_dt_us = 10000; // 10ms = 100 Hz + constexpr int64_t jitter_tolerance_us = 500; // 0.5ms + + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + config.expected_dt_us = expected_dt_us; + config.jitter_tolerance_us = jitter_tolerance_us; + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", config); + + setupDiagnosticSubscription(); + + // Send messages at wrong frequency (50 Hz instead of 100 Hz) + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + constexpr int64_t wrong_interval_ns = 20000000; // 20ms = 50 Hz + + sendMessages(greenwave_diagnostics, msg_timestamp, wrong_interval_ns, 30, true); + + // Should detect frame drops due to config expected_dt_us mismatch + EXPECT_TRUE(hasDiagnosticMessage("FRAME DROP")); +} + +TEST_F(GreenwaveDiagnosticsTest, JitterToleranceUsConfigImpactTest) +{ + // Test that jitter_tolerance_us in config affects outlier detection + constexpr int64_t expected_dt_us = 10000; // 10ms = 100 Hz + constexpr int64_t base_interval_ns = 10000000; + constexpr int64_t jitter_ns = 500000; // 500us + + // Test with tight tolerance - should detect outliers + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + config.expected_dt_us = expected_dt_us; + config.jitter_tolerance_us = 100; // 100us tolerance + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_tight", config); + + setupDiagnosticSubscription(); + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, msg_timestamp, base_interval_ns, 30, true, jitter_ns); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_GT(getDiagnosticValue("num_jitter_outliers_msg"), 0.0) + << "Tight tolerance should detect 500us jitter as outliers"; + } + + recreateNode(); + + // Test with loose tolerance - should not detect outliers + { + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; + config.enable_msg_time_diagnostics = true; + config.expected_dt_us = expected_dt_us; + config.jitter_tolerance_us = 1000000; // 1000ms tolerance (very loose) + greenwave_diagnostics::GreenwaveDiagnostics diag(*node_, "test_topic_loose", config); + + setupDiagnosticSubscription(); + uint64_t msg_timestamp = test_constants::kStartTimestampNs; + sendMessages(diag, msg_timestamp, base_interval_ns, 30, true, jitter_ns); + + ASSERT_FALSE(received_diagnostics_.empty()); + EXPECT_EQ(getDiagnosticValue("num_jitter_outliers_msg"), 0.0) + << "Loose tolerance should not detect 500us jitter as outliers"; + } +} diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 2c49e2d..556fe23 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -22,18 +22,17 @@ import unittest from greenwave_monitor.test_utils import ( - call_manage_topic_service, collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - create_service_clients, find_best_diagnostic, + make_enabled_param, MANAGE_TOPIC_TEST_CONFIG, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, + set_parameter, TEST_CONFIGURATIONS, verify_diagnostic_values, - wait_for_service_connection ) import launch import launch_testing @@ -55,13 +54,16 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): topics=['/test_topic'] ) - # Create publishers + # Create publishers (all with diagnostics disabled so greenwave_monitor can add them) publishers = [ # Main test topic publisher with parametrized frequency - create_minimal_publisher('/test_topic', expected_frequency, message_type), + create_minimal_publisher( + '/test_topic', expected_frequency, message_type, enable_diagnostics=False), # Additional publishers for topic management tests - create_minimal_publisher('/test_topic1', expected_frequency, message_type, '1'), - create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2') + create_minimal_publisher( + '/test_topic1', expected_frequency, message_type, '1', enable_diagnostics=False), + create_minimal_publisher( + '/test_topic2', expected_frequency, message_type, '2', enable_diagnostics=False) ] context = { @@ -117,20 +119,19 @@ def tearDownClass(cls): def check_node_launches_successfully(self): """Test that the node launches without errors.""" - # Create a service client to check if the node is ready - # Service discovery is more reliable than node discovery in launch_testing - manage_client, set_freq_client = create_service_clients( - self.test_node, MONITOR_NODE_NAMESPACE, MONITOR_NODE_NAME - ) - service_available = wait_for_service_connection( - self.test_node, manage_client, timeout_sec=3.0, - service_name=f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}/manage_topic' - ) - self.assertTrue( - service_available, - f'Service "/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}/manage_topic" ' - 'not available within timeout') - return manage_client + # Wait for the node to be discoverable + end_time = time.time() + 5.0 + node_found = False + while time.time() < end_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + node_names = self.test_node.get_node_names_and_namespaces() + for name, namespace in node_names: + if name == MONITOR_NODE_NAME and namespace == f'/{MONITOR_NODE_NAMESPACE}': + node_found = True + break + if node_found: + break + self.assertTrue(node_found, f'Node {MONITOR_NODE_NAME} not found within timeout') def verify_diagnostics(self, topic_name, expected_frequency, message_type, tolerance_hz): """Verify diagnostics for a given topic.""" @@ -166,52 +167,40 @@ def test_frequency_monitoring(self, expected_frequency, message_type, tolerance_ self.check_node_launches_successfully() self.verify_diagnostics('/test_topic', expected_frequency, message_type, tolerance_hz) - def call_manage_topic(self, add, topic, service_client): - """Service call helper.""" - response = call_manage_topic_service( - self.test_node, service_client, add, topic, timeout_sec=8.0 - ) - self.assertIsNotNone(response, 'Service call failed or timed out') - return response + def set_topic_enabled(self, topic: str, enabled: bool) -> bool: + """Set a topic's enabled parameter.""" + param_name = make_enabled_param(topic) + return set_parameter(self.test_node, param_name, enabled) def test_manage_one_topic(self, expected_frequency, message_type, tolerance_hz): - """Test that add_topic() and remove_topic() work correctly for one topic.""" + """Test that add_topic() and remove_topic() work via enabled parameter.""" if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: self.skipTest('Only running manage topic tests once') - service_client = self.check_node_launches_successfully() + self.check_node_launches_successfully() TEST_TOPIC = '/test_topic' - # 1. Remove an existing topic – should succeed on first attempt. - response = self.call_manage_topic( - add=False, topic=TEST_TOPIC, service_client=service_client) - self.assertTrue(response.success) + # 1. Disable monitoring for the topic + success = self.set_topic_enabled(TEST_TOPIC, False) + self.assertTrue(success, 'Failed to disable topic monitoring') - # 2. Removing the same topic again should fail because it no longer exists. - response = self.call_manage_topic( - add=False, topic=TEST_TOPIC, service_client=service_client) - self.assertFalse(response.success) + # Allow time for the parameter event to be processed + time.sleep(0.5) - # 3. Add the topic back – should succeed now. - response = self.call_manage_topic( - add=True, topic=TEST_TOPIC, service_client=service_client) - self.assertTrue(response.success) + # 2. Re-enable monitoring for the topic + success = self.set_topic_enabled(TEST_TOPIC, True) + self.assertTrue(success, 'Failed to enable topic monitoring') - # Verify diagnostics after adding the topic back + # Verify diagnostics after enabling the topic self.verify_diagnostics(TEST_TOPIC, expected_frequency, message_type, tolerance_hz) - # 4. Adding the same topic again should fail because it's already monitored. - response = self.call_manage_topic( - add=True, topic=TEST_TOPIC, service_client=service_client) - self.assertFalse(response.success) - def test_manage_multiple_topics(self, expected_frequency, message_type, tolerance_hz): - """Test that add_topic() and remove_topic() work correctly for multiple topics.""" + """Test add_topic() and remove_topic() work via enabled parameter for multiple topics.""" if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: - self.skipTest('Only running manage topic tests once for 30 hz images') + self.skipTest('Only running manage topic tests once') - service_client = self.check_node_launches_successfully() + self.check_node_launches_successfully() TEST_TOPIC1 = '/test_topic1' TEST_TOPIC2 = '/test_topic2' @@ -221,32 +210,23 @@ def test_manage_multiple_topics(self, expected_frequency, message_type, toleranc while time.time() < end_time: rclpy.spin_once(self.test_node, timeout_sec=0.1) - # Try to add a non-existent topic - should fail - nonexistent_topic = '/test/nonexistent_topic' - response = self.call_manage_topic( - add=True, topic=nonexistent_topic, service_client=service_client) - self.assertFalse(response.success) - - # 1. Add first topic – should succeed. - response = self.call_manage_topic( - add=True, topic=TEST_TOPIC1, service_client=service_client) - self.assertTrue(response.success) + # 1. Enable monitoring for first topic via parameter + success = self.set_topic_enabled(TEST_TOPIC1, True) + self.assertTrue(success, 'Failed to enable first topic') - # Verify diagnostics after adding the first topic + # Verify diagnostics after enabling the first topic self.verify_diagnostics(TEST_TOPIC1, expected_frequency, message_type, tolerance_hz) - # 2. Add second topic – should succeed. - response = self.call_manage_topic( - add=True, topic=TEST_TOPIC2, service_client=service_client) - self.assertTrue(response.success) + # 2. Enable monitoring for second topic via parameter + success = self.set_topic_enabled(TEST_TOPIC2, True) + self.assertTrue(success, 'Failed to enable second topic') - # Verify diagnostics after adding the second topic + # Verify diagnostics after enabling the second topic self.verify_diagnostics(TEST_TOPIC2, expected_frequency, message_type, tolerance_hz) - # 3. Remove first topic – should succeed. - response = self.call_manage_topic( - add=False, topic=TEST_TOPIC1, service_client=service_client) - self.assertTrue(response.success) + # 3. Disable first topic via parameter + success = self.set_topic_enabled(TEST_TOPIC1, False) + self.assertTrue(success, 'Failed to disable first topic') if __name__ == '__main__': diff --git a/greenwave_monitor/test/test_message_diagnostics.cpp b/greenwave_monitor/test/test_message_diagnostics.cpp deleted file mode 100644 index df3e0dc..0000000 --- a/greenwave_monitor/test/test_message_diagnostics.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// 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 - -/** -Unit tests for functionality in message_diagnostics.hpp, -such as frame rate and latency calculation accuracy. -**/ - -#include -#include -#include -#include -#include -#include -#include - -#include "message_diagnostics.hpp" - -namespace test_constants -{ -inline constexpr uint64_t kMillisecondsToSeconds = 1000ULL; -inline constexpr uint64_t kStartTimestampNs = 10000000ULL; -} // namespace test_constants - -class MessageDiagnosticsTest : public ::testing::Test -{ -protected: - static void SetUpTestSuite() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestSuite() - { - rclcpp::shutdown(); - } - - void SetUp() override - { - node_ = std::make_shared("test_node"); - } - - void TearDown() override - { - node_.reset(); - } - - std::shared_ptr node_; -}; - -TEST_F(MessageDiagnosticsTest, FrameRateMsgTest) -{ - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); - - uint64_t timestamp = test_constants::kStartTimestampNs; // in nanoseconds - for (int i = 0; i < 1000; i++) { - message_diagnostics.updateDiagnostics(timestamp); - timestamp += 10000000; // 10 ms in nanoseconds - } - EXPECT_EQ(message_diagnostics.getFrameRateMsg(), 100); // 100 Hz -} - -TEST_F(MessageDiagnosticsTest, FrameRateNodeTest) -{ - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); - - // dummy timestamp, not used for node time calculation - constexpr auto timestamp = test_constants::kStartTimestampNs; - const auto start_time = std::chrono::high_resolution_clock::now(); - - constexpr int num_messages = 1000; - constexpr int interarrival_time_ms = 10; // 100 hz - - for (int i = 0; i < num_messages; i++) { - message_diagnostics.updateDiagnostics(timestamp); - std::this_thread::sleep_for(std::chrono::milliseconds(interarrival_time_ms)); - } - - const auto end_time = std::chrono::high_resolution_clock::now(); - const std::chrono::duration total_duration = end_time - start_time; - - const double expected_frame_rate = static_cast(num_messages) / total_duration.count(); - - // allow 2.0 Hz error - EXPECT_NEAR(message_diagnostics.getFrameRateNode(), expected_frame_rate, 2.0); -} - -TEST_F(MessageDiagnosticsTest, MessageLatencyTest) -{ - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); - - const rclcpp::Time current_time = node_->get_clock()->now(); - // Make message timestamp a certain amount of time earlier than current time - constexpr double expected_latency_ms = 10.0; - const rclcpp::Time msg_timestamp = - current_time - rclcpp::Duration::from_seconds( - expected_latency_ms / static_cast(test_constants::kMillisecondsToSeconds)); - - message_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); - - EXPECT_NEAR(message_diagnostics.getLatency(), expected_latency_ms, 1.0); // allow 1 ms tolerance -} - -TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) -{ - constexpr int input_frequency = 50; // 50 Hz - // 20 ms in nanoseconds - const int64_t interarrival_time_ns = static_cast( - ::message_diagnostics::constants::kSecondsToNanoseconds / input_frequency); - - // Initialize MessageDiagnostics with diagnostics enabled - message_diagnostics::MessageDiagnosticsConfig config; - config.enable_msg_time_diagnostics = true; - config.enable_node_time_diagnostics = true; - config.enable_increasing_msg_time_diagnostics = true; - // in us - config.expected_dt_us = interarrival_time_ns / - ::message_diagnostics::constants::kMicrosecondsToNanoseconds; - - message_diagnostics::MessageDiagnostics message_diagnostics(*node_, "test_topic", config); - - // Create a subscriber to receive diagnostic messages - std::vector received_diagnostics; - const auto diagnostic_subscription = - node_->create_subscription( - "/diagnostics", 10, - [&received_diagnostics](const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) { - received_diagnostics.push_back(msg); - }); - - // 50 ms delay - constexpr int64_t delay_time_ns = 50 * - static_cast(::message_diagnostics::constants::kMillisecondsToMicroseconds) * - static_cast(::message_diagnostics::constants::kMicrosecondsToNanoseconds); - // Starting message timestamp in nanoseconds - auto msg_timestamp = test_constants::kStartTimestampNs; - - int sent_count = 0; - const auto start_time = std::chrono::high_resolution_clock::now(); - - // Send 100 messages - constexpr int num_messages = 100; - while (sent_count < num_messages) { - if (sent_count != 0) { - msg_timestamp += interarrival_time_ns; - } - - sent_count++; - - message_diagnostics.updateDiagnostics(msg_timestamp); - message_diagnostics.publishDiagnostics(); - - // Add a non-increasing timestamp at count 5 - if (sent_count == 5) { - msg_timestamp -= interarrival_time_ns; - } - // Add a jitter by delaying at count 10 - if (sent_count == 10) { - std::this_thread::sleep_for(std::chrono::nanoseconds(delay_time_ns)); // 50 ms delay - msg_timestamp += delay_time_ns; - } - - rclcpp::spin_some(node_); - - std::this_thread::sleep_for(std::chrono::nanoseconds(interarrival_time_ns)); - } - - ASSERT_EQ(received_diagnostics.size(), num_messages); - - const int interarrival_time_count = sent_count - 1; - // Calculate expected node and message frame rates - const auto actual_end_time = std::chrono::high_resolution_clock::now(); - const std::chrono::duration total_duration = actual_end_time - start_time; - const double expected_frame_rate_node = static_cast(interarrival_time_count) / - total_duration.count(); - - const auto sum_interarrival_time_msg_sec = static_cast( - msg_timestamp - test_constants::kStartTimestampNs) / - static_cast(::message_diagnostics::constants::kSecondsToNanoseconds); - const double expected_frame_rate_msg = - static_cast(interarrival_time_count) / sum_interarrival_time_msg_sec; - - // Verify that we received diagnostic messages - ASSERT_FALSE(received_diagnostics.empty()); - - // Use the last diagnostic message - const auto & last_diagnostic = received_diagnostics.back(); - ASSERT_FALSE(last_diagnostic->status.empty()); - - // Verify the diagnostic status information - const auto & diagnostic_status = last_diagnostic->status[0]; - EXPECT_TRUE(diagnostic_status.name.find("test_topic") != std::string::npos); - EXPECT_EQ(diagnostic_status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); - EXPECT_EQ(diagnostic_status.message, "FRAME DROP DETECTED, NONINCREASING TIMESTAMP"); - - // Parse diagnostic values - std::map diagnostics_values = { - {"frame_rate_node", 0.0}, - {"num_non_increasing_msg", 0.0}, - {"num_jitter_outliers_msg", 0.0}, - {"num_jitter_outliers_node", 0.0}, - {"max_abs_jitter_msg", 0.0}, - {"max_abs_jitter_node", 0.0}, - {"mean_abs_jitter_msg", 0.0}, - {"mean_abs_jitter_node", 0.0}, - {"frame_rate_msg", 0.0}, - {"total_dropped_frames", 0.0} - }; - for (const auto & value : diagnostic_status.values) { - if (diagnostics_values.find(value.key) != diagnostics_values.end()) { - diagnostics_values[value.key] = std::stod(value.value); - } - } - - // Sometimes diagnostics may arrive out of order, so we use getter methods instead of values from - // the last diagnostic message to prevent flakiness - EXPECT_NEAR(message_diagnostics.getFrameRateNode(), expected_frame_rate_node, 1.0); - // Allow small floating point differences for frame rate msg - constexpr double frame_rate_msg_tolerance = 0.001; - EXPECT_NEAR( - message_diagnostics.getFrameRateMsg(), expected_frame_rate_msg, frame_rate_msg_tolerance); - - // Sometimes diagnostics may arrive out of order, so we need to check all received diagnostics - // to see if the expected msg frame rate is somewhere in there - double smallest_msg_frame_rate_diff = std::numeric_limits::infinity(); - for (const auto & diag_msg : received_diagnostics) { - if (diag_msg->status.empty()) { - continue; - } - const auto & status = diag_msg->status[0]; - double frame_rate_msg = 0.0; - for (const auto & value : status.values) { - if (value.key == "frame_rate_msg") { - frame_rate_msg = std::stod(value.value); - break; - } - } - if (std::abs(frame_rate_msg - expected_frame_rate_msg) < smallest_msg_frame_rate_diff) { - smallest_msg_frame_rate_diff = std::abs(frame_rate_msg - expected_frame_rate_msg); - } - } - - EXPECT_LT(smallest_msg_frame_rate_diff, frame_rate_msg_tolerance); - - // Diagnostics should have at least one jitter due to the intentional delay - // possibly more if the system was very busy - EXPECT_GE(diagnostics_values["num_jitter_outliers_node"], 1.0); - EXPECT_GE(diagnostics_values["max_abs_jitter_node"], 0.0); - EXPECT_GE(diagnostics_values["mean_abs_jitter_node"], 0.0); - - EXPECT_GE(diagnostics_values["num_jitter_outliers_msg"], 1.0); - EXPECT_GE(diagnostics_values["max_abs_jitter_msg"], 0.0); - EXPECT_GE(diagnostics_values["mean_abs_jitter_msg"], 0.0); - - EXPECT_GE(diagnostics_values["total_dropped_frames"], 1.0); - EXPECT_GE(diagnostics_values["num_non_increasing_msg"], 1.0); -} diff --git a/greenwave_monitor/test/test_parameters.py b/greenwave_monitor/test/test_parameters.py new file mode 100644 index 0000000..ff89c37 --- /dev/null +++ b/greenwave_monitor/test/test_parameters.py @@ -0,0 +1,506 @@ +#!/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 + +"""Consolidated tests for parameter-based topic configuration.""" + +import os +import tempfile +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + delete_parameter, + find_best_diagnostic, + get_parameter, + make_enabled_param, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE, + RosNodeTestCase, + set_parameter, +) +from greenwave_monitor.ui_adaptor import ( + ENABLED_SUFFIX, + make_freq_param, + make_tol_param, + TOPIC_PARAM_PREFIX, +) +import launch +import launch_ros.actions +import launch_testing +from launch_testing import post_shutdown_test +from launch_testing.asserts import assertExitCodes +import pytest +from rcl_interfaces.srv import GetParameters +import rclpy + + +# Topic configurations for all tests +ENABLED_FALSE_TOPIC = '/enabled_false_topic' +TOL_ONLY_TOPIC = '/tol_only_topic' +FREQ_ONLY_TOPIC = '/freq_only_topic' +FULL_CONFIG_TOPIC = '/full_config_topic' +DYNAMIC_TOPIC = '/dynamic_param_topic' +DYNAMIC_SET_PARAMS_TOPIC = '/dynamic_param_topic_set_params' +DYNAMIC_DELETE_TOPIC = '/dynamic_param_topic_delete_param' +NONEXISTENT_TOPIC = '/topic_that_does_not_exist' +MULTI_TOPIC_1 = '/multi_topic_1' +MULTI_TOPIC_2 = '/multi_topic_2' +MULTI_TOPIC_3 = '/multi_topic_3' +MULTI_LIST_TOPIC_1 = '/multi_topic_list_1' +MULTI_LIST_TOPIC_2 = '/multi_topic_list_2' +YAML_TOPIC = '/yaml_config_topic' +NESTED_YAML_TOPIC = '/nested_yaml_topic' +ENABLE_EXISTING_TOPIC = '/enable_existing_test_topic' +NEW_DYNAMIC_TOPIC = '/new_dynamic_topic' + +# Test frequencies and tolerances +STD_FREQUENCY = 50.0 +STD_TOLERANCE = 10.0 +MULTI_FREQ_1 = 10.0 +MULTI_FREQ_2 = 25.0 +MULTI_FREQ_3 = 50.0 +MULTI_TOLERANCE = 20.0 +MULTI_LIST_FREQ = 30.0 +YAML_FREQUENCY = 50.0 +YAML_TOLERANCE = 10.0 +NESTED_FREQUENCY = 25.0 +DYNAMIC_FREQUENCY = 30.0 +DYNAMIC_TOLERANCE = 20.0 + +# Publisher node names +PUBLISHER_DYNAMIC = 'minimal_publisher_node_dynamic' +PUBLISHER_SET_PARAMS = 'minimal_publisher_node_set_params' +PUBLISHER_ENABLE_TEST = 'minimal_publisher_node_enable_test' + + +def _make_yaml_file(): + """Create a temp YAML file for parameter loading test.""" + yaml_content = ( + f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n' + f' ros__parameters:\n' + f' "greenwave_diagnostics.{YAML_TOPIC}.expected_frequency": {YAML_FREQUENCY}\n' + f' "greenwave_diagnostics.{YAML_TOPIC}.tolerance": {YAML_TOLERANCE}\n' + f' greenwave_diagnostics:\n' + f' {NESTED_YAML_TOPIC}:\n' + f' expected_frequency: {NESTED_FREQUENCY}\n' + f' tolerance: {YAML_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) + return yaml_path + + +@pytest.mark.launch_test +def generate_test_description(): + """Generate comprehensive launch for all parameter tests.""" + yaml_path = _make_yaml_file() + + # Build comprehensive parameter set for monitor node + params = { + # enabled=false test + make_freq_param(ENABLED_FALSE_TOPIC): STD_FREQUENCY, + make_enabled_param(ENABLED_FALSE_TOPIC): False, + # tolerance only test + make_tol_param(TOL_ONLY_TOPIC): 15.0, + # frequency only test + make_freq_param(FREQ_ONLY_TOPIC): STD_FREQUENCY, + # full config test + make_freq_param(FULL_CONFIG_TOPIC): STD_FREQUENCY, + make_tol_param(FULL_CONFIG_TOPIC): STD_TOLERANCE, + # multiple topics test + make_freq_param(MULTI_TOPIC_1): MULTI_FREQ_1, + make_tol_param(MULTI_TOPIC_1): MULTI_TOLERANCE, + make_freq_param(MULTI_TOPIC_2): MULTI_FREQ_2, + make_tol_param(MULTI_TOPIC_2): MULTI_TOLERANCE, + make_freq_param(MULTI_TOPIC_3): MULTI_FREQ_3, + make_tol_param(MULTI_TOPIC_3): MULTI_TOLERANCE, + # Topics list (no expected frequency) + 'topics': [MULTI_LIST_TOPIC_1, MULTI_LIST_TOPIC_2, ENABLE_EXISTING_TOPIC], + } + + monitor_node = launch_ros.actions.Node( + package='greenwave_monitor', + executable='greenwave_monitor', + name=MONITOR_NODE_NAME, + namespace=MONITOR_NODE_NAMESPACE, + parameters=[params, yaml_path], + output='screen' + ) + + publishers = [ + create_minimal_publisher( + ENABLED_FALSE_TOPIC, STD_FREQUENCY, 'imu', '_enabled_false', + enable_diagnostics=False), + create_minimal_publisher( + TOL_ONLY_TOPIC, STD_FREQUENCY, 'imu', '_tol_only', + enable_diagnostics=False), + create_minimal_publisher( + FREQ_ONLY_TOPIC, STD_FREQUENCY, 'imu', '_freq_only'), + create_minimal_publisher( + FULL_CONFIG_TOPIC, STD_FREQUENCY, 'imu', '_full_config'), + create_minimal_publisher( + DYNAMIC_TOPIC, DYNAMIC_FREQUENCY, 'imu', '_dynamic'), + create_minimal_publisher( + DYNAMIC_SET_PARAMS_TOPIC, DYNAMIC_FREQUENCY, 'imu', '_set_params', + enable_diagnostics=True), + create_minimal_publisher( + DYNAMIC_DELETE_TOPIC, DYNAMIC_FREQUENCY, 'imu', '_delete_param', + enable_diagnostics=False), + create_minimal_publisher( + MULTI_TOPIC_1, MULTI_FREQ_1, 'imu', '_multi_1'), + create_minimal_publisher( + MULTI_TOPIC_2, MULTI_FREQ_2, 'imu', '_multi_2'), + create_minimal_publisher( + MULTI_TOPIC_3, MULTI_FREQ_3, 'imu', '_multi_3'), + create_minimal_publisher( + MULTI_LIST_TOPIC_1, MULTI_LIST_FREQ, 'imu', '_list_1'), + create_minimal_publisher( + MULTI_LIST_TOPIC_2, MULTI_LIST_FREQ, 'imu', '_list_2'), + create_minimal_publisher( + YAML_TOPIC, YAML_FREQUENCY, 'imu', '_yaml'), + create_minimal_publisher( + NESTED_YAML_TOPIC, NESTED_FREQUENCY, 'imu', '_nested_yaml'), + create_minimal_publisher( + ENABLE_EXISTING_TOPIC, STD_FREQUENCY, 'imu', '_enable_test'), + create_minimal_publisher( + NEW_DYNAMIC_TOPIC, STD_FREQUENCY, 'imu', '_new_dynamic', + enable_diagnostics=False), + ] + + return ( + launch.LaunchDescription([monitor_node] + publishers + [ + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +@post_shutdown_test() +class TestPostShutdown(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 TestStartupBehavior(RosNodeTestCase): + """Tests for parameter behavior at node startup.""" + + TEST_NODE_NAME = 'startup_behavior_test_node' + + def test_enabled_false_does_not_monitor(self): + """Test that enabled=false prevents topic monitoring.""" + time.sleep(2.0) + diagnostics = collect_diagnostics_for_topic( + self.test_node, ENABLED_FALSE_TOPIC, expected_count=1, timeout_sec=3.0) + self.assertEqual( + len(diagnostics), 0, + f'Should not monitor topic with enabled=false, got {len(diagnostics)}') + + def test_tolerance_only_starts_monitoring(self): + """Test that specifying only tolerance starts monitoring.""" + time.sleep(2.0) + diagnostics = collect_diagnostics_for_topic( + self.test_node, TOL_ONLY_TOPIC, expected_count=3, timeout_sec=5.0) + self.assertGreaterEqual( + len(diagnostics), 3, + f'Should monitor when tolerance is set, got {len(diagnostics)}') + + def test_frequency_only_uses_default_tolerance(self): + """Test that specifying only frequency uses default tolerance.""" + time.sleep(2.0) + diagnostics = collect_diagnostics_for_topic( + self.test_node, FREQ_ONLY_TOPIC, expected_count=3, timeout_sec=10.0) + self.assertGreaterEqual(len(diagnostics), 3) + best_status, _ = find_best_diagnostic(diagnostics, STD_FREQUENCY, 'imu') + self.assertIsNotNone(best_status, 'Should have valid frame rate') + + def test_full_config_monitors_correctly(self): + """Test topic with both frequency and tolerance configured.""" + time.sleep(2.0) + diagnostics = collect_diagnostics_for_topic( + self.test_node, FULL_CONFIG_TOPIC, expected_count=3, timeout_sec=10.0) + self.assertGreaterEqual(len(diagnostics), 3) + best_status, best_values = find_best_diagnostic( + diagnostics, STD_FREQUENCY, 'imu') + self.assertIsNotNone(best_status) + frame_rate = best_values[0] + tolerance = STD_FREQUENCY * STD_TOLERANCE / 100.0 + self.assertAlmostEqual(frame_rate, STD_FREQUENCY, delta=tolerance) + + +class TestMultipleTopics(RosNodeTestCase): + """Tests for multiple topic configuration.""" + + TEST_NODE_NAME = 'multiple_topics_test_node' + + def test_all_configured_topics_monitored(self): + """Test that all configured topics are monitored.""" + time.sleep(2.0) + topics_to_check = [ + (MULTI_TOPIC_1, MULTI_FREQ_1), + (MULTI_TOPIC_2, MULTI_FREQ_2), + (MULTI_TOPIC_3, MULTI_FREQ_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) + best_status, best_values = find_best_diagnostic( + diagnostics, expected_freq, 'imu') + self.assertIsNotNone(best_status) + tolerance_hz = expected_freq * MULTI_TOLERANCE / 100.0 + self.assertAlmostEqual( + best_values[0], expected_freq, delta=tolerance_hz) + + 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 [MULTI_LIST_TOPIC_1, MULTI_LIST_TOPIC_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) + 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) + self.assertTrue( + expected_freq_value is None or expected_freq_value == 0.0) + self.assertIsNotNone(frame_rate_value) + self.assertGreater(frame_rate_value, 0.0) + + +class TestYamlConfiguration(RosNodeTestCase): + """Tests for YAML parameter file configuration.""" + + TEST_NODE_NAME = 'yaml_test_node' + + def test_topic_configured_via_yaml(self): + """Test that topic configured via YAML file is monitored.""" + time.sleep(2.0) + diagnostics = collect_diagnostics_for_topic( + self.test_node, YAML_TOPIC, expected_count=3, timeout_sec=10.0) + self.assertGreaterEqual(len(diagnostics), 3) + best_status, _ = find_best_diagnostic( + diagnostics, YAML_FREQUENCY, 'imu') + self.assertIsNotNone(best_status) + + def test_nested_dict_topic_configured_via_yaml(self): + """Test topic configured via nested YAML dict.""" + time.sleep(2.0) + diagnostics = collect_diagnostics_for_topic( + self.test_node, NESTED_YAML_TOPIC, expected_count=3, timeout_sec=10.0) + self.assertGreaterEqual(len(diagnostics), 3) + best_status, _ = find_best_diagnostic( + diagnostics, NESTED_FREQUENCY, 'imu') + self.assertIsNotNone(best_status) + + +class TestDynamicParameters(RosNodeTestCase): + """Tests for dynamic parameter changes.""" + + TEST_NODE_NAME = 'dynamic_param_test_node' + + def test_set_parameters_dynamically(self): + """Test setting frequency and tolerance parameters dynamically.""" + time.sleep(2.0) + freq_param = make_freq_param(DYNAMIC_SET_PARAMS_TOPIC) + tol_param = make_tol_param(DYNAMIC_SET_PARAMS_TOPIC) + + # Verify diagnostics are published + diagnostics = collect_diagnostics_for_topic( + self.test_node, DYNAMIC_SET_PARAMS_TOPIC, expected_count=3, timeout_sec=5.0) + self.assertGreaterEqual(len(diagnostics), 3) + + # Set parameters on publisher node + success = set_parameter( + self.test_node, freq_param, DYNAMIC_FREQUENCY, + node_name=PUBLISHER_SET_PARAMS, node_namespace='') + self.assertTrue(success, f'Failed to set {freq_param}') + + success = set_parameter( + self.test_node, tol_param, DYNAMIC_TOLERANCE, + node_name=PUBLISHER_SET_PARAMS, node_namespace='') + self.assertTrue(success, f'Failed to set {tol_param}') + + # Verify parameters were set + success, actual_freq = get_parameter( + self.test_node, freq_param, + node_name=PUBLISHER_SET_PARAMS, node_namespace='') + self.assertTrue(success) + self.assertAlmostEqual(actual_freq, DYNAMIC_FREQUENCY, places=1) + + # Update to mismatched frequency - should cause error diagnostics + mismatched_frequency = 1.0 + success = set_parameter( + self.test_node, freq_param, mismatched_frequency, + node_name=PUBLISHER_SET_PARAMS, node_namespace='') + self.assertTrue(success) + + time.sleep(2.0) + diagnostics = collect_diagnostics_for_topic( + self.test_node, DYNAMIC_SET_PARAMS_TOPIC, expected_count=3, timeout_sec=10.0) + self.assertGreaterEqual(len(diagnostics), 3) + has_non_ok = any(ord(d.level) != 0 for d in diagnostics) + self.assertTrue(has_non_ok, 'Expected non-OK due to frequency mismatch') + + def test_add_new_topic_via_frequency_param(self): + """Test that frequency param can be set for new topic.""" + time.sleep(2.0) + freq_param = make_freq_param(NEW_DYNAMIC_TOPIC) + success = set_parameter(self.test_node, freq_param, STD_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + success, value = get_parameter(self.test_node, freq_param) + self.assertTrue(success) + self.assertAlmostEqual(value, STD_FREQUENCY, places=1) + + def test_set_frequency_for_nonexistent_topic(self): + """Test setting frequency for a topic that doesn't exist.""" + time.sleep(1.0) + freq_param = make_freq_param(NONEXISTENT_TOPIC) + success = set_parameter(self.test_node, freq_param, STD_FREQUENCY) + self.assertTrue(success) + + success, actual_freq = get_parameter(self.test_node, freq_param) + self.assertTrue(success) + self.assertAlmostEqual(actual_freq, STD_FREQUENCY, places=1) + + diagnostics = collect_diagnostics_for_topic( + self.test_node, NONEXISTENT_TOPIC, expected_count=1, timeout_sec=3.0) + self.assertEqual(len(diagnostics), 0) + + def test_non_numeric_parameter_rejected(self): + """Test that non-numeric parameter values are rejected.""" + time.sleep(1.0) + freq_param = make_freq_param(DYNAMIC_TOPIC) + success = set_parameter( + self.test_node, freq_param, 'not_a_number', + node_name=PUBLISHER_DYNAMIC, node_namespace='') + self.assertFalse(success) + + tol_param = make_tol_param(DYNAMIC_TOPIC) + success = set_parameter( + self.test_node, tol_param, 'invalid', + node_name=PUBLISHER_DYNAMIC, node_namespace='') + self.assertFalse(success) + + def test_non_positive_frequency_rejected(self): + """Test that non-positive frequency values are rejected.""" + time.sleep(1.0) + freq_param = make_freq_param(DYNAMIC_TOPIC) + + success = set_parameter( + self.test_node, freq_param, 0.0, + node_name=PUBLISHER_DYNAMIC, node_namespace='') + self.assertFalse(success, 'Zero frequency should be rejected') + + success = set_parameter( + self.test_node, freq_param, -10.0, + node_name=PUBLISHER_DYNAMIC, node_namespace='') + self.assertFalse(success, 'Negative frequency should be rejected') + + def test_negative_tolerance_rejected(self): + """Test that negative tolerance values are rejected.""" + time.sleep(1.0) + tol_param = make_tol_param(DYNAMIC_TOPIC) + success = set_parameter( + self.test_node, tol_param, -5.0, + node_name=PUBLISHER_DYNAMIC, node_namespace='') + self.assertFalse(success) + + def test_delete_parameter_rejected(self): + """Test that deleting a parameter is rejected.""" + time.sleep(2.0) + freq_param = make_freq_param(DYNAMIC_TOPIC) + success = delete_parameter( + self.test_node, freq_param, + node_name=PUBLISHER_DYNAMIC, node_namespace='') + self.assertFalse(success) + + +class TestEnableExistingNode(RosNodeTestCase): + """Tests for enabling/disabling monitoring via enabled parameter.""" + + TEST_NODE_NAME = 'enable_existing_test_node' + + def test_enabled_parameter_toggles_monitoring(self): + """Test that setting enabled parameter toggles monitoring.""" + time.sleep(3.0) + publisher_full_name = f'/{PUBLISHER_ENABLE_TEST}' + enabled_param_name = f'{TOPIC_PARAM_PREFIX}{ENABLE_EXISTING_TOPIC}{ENABLED_SUFFIX}' + + get_params_client = self.test_node.create_client( + GetParameters, f'{publisher_full_name}/get_parameters') + self.assertTrue( + get_params_client.wait_for_service(timeout_sec=5.0), + 'Get parameters service not available') + + # Verify publisher has enabled parameter + request = GetParameters.Request() + request.names = [enabled_param_name] + future = get_params_client.call_async(request) + rclpy.spin_until_future_complete(self.test_node, future, timeout_sec=5.0) + self.assertIsNotNone(future.result()) + self.assertTrue(len(future.result().values) > 0) + + # Disable monitoring via enabled parameter + success = set_parameter( + self.test_node, enabled_param_name, False) + self.assertTrue(success, 'Failed to disable monitoring') + + time.sleep(0.5) + + # Verify enabled=false on monitor node + success, value = get_parameter(self.test_node, enabled_param_name) + self.assertTrue(success) + self.assertFalse(value) + + # Re-enable monitoring via enabled parameter + success = set_parameter( + self.test_node, enabled_param_name, True) + self.assertTrue(success, 'Failed to enable monitoring') + + time.sleep(0.5) + + # Verify enabled=true + success, value = get_parameter(self.test_node, enabled_param_name) + self.assertTrue(success) + self.assertTrue(value) + + self.test_node.destroy_client(get_params_client) + + +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..95ca012 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -30,8 +30,7 @@ MONITOR_NODE_NAMESPACE, TEST_CONFIGURATIONS ) -from greenwave_monitor.ui_adaptor import GreenwaveUiAdaptor, UiDiagnosticData -from greenwave_monitor_interfaces.srv import ManageTopic +from greenwave_monitor.ui_adaptor import build_full_node_name, GreenwaveUiAdaptor, UiDiagnosticData import launch import launch_testing from launch_testing import post_shutdown_test @@ -51,15 +50,19 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): topics=['/test_topic'] ) - # Create publishers for testing + # Create publishers for testing (diagnostics disabled so monitor can add them) publishers = [ # Main test topic publisher with parametrized frequency - create_minimal_publisher('/test_topic', expected_frequency, message_type), + create_minimal_publisher( + '/test_topic', expected_frequency, message_type, enable_diagnostics=False), # Additional publishers for topic management tests - create_minimal_publisher('/test_topic1', expected_frequency, message_type, '1'), - create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2'), + create_minimal_publisher( + '/test_topic1', expected_frequency, message_type, '1', enable_diagnostics=False), + create_minimal_publisher( + '/test_topic2', expected_frequency, message_type, '2', enable_diagnostics=False), # Publisher for service discovery tests - create_minimal_publisher('/discovery_test_topic', 50.0, 'imu', '_discovery') + create_minimal_publisher( + '/discovery_test_topic', 50.0, 'imu', '_discovery', enable_diagnostics=False) ] context = { @@ -120,7 +123,7 @@ def setUp(self): # Create a fresh GreenwaveUiAdaptor instance for each test with proper namespace self.diagnostics_monitor = GreenwaveUiAdaptor( self.test_node, - monitor_node_name=MONITOR_NODE_NAME + monitor_node_name=build_full_node_name(MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE) ) # Allow time for service discovery in test environment (reduced from 2.0s) @@ -132,10 +135,13 @@ 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_client(self.diagnostics_monitor.manage_topic_client) - self.test_node.destroy_client( - self.diagnostics_monitor.set_expected_frequency_client) + self.test_node.destroy_subscription( + self.diagnostics_monitor.param_events_subscription) except Exception: pass # Ignore cleanup errors @@ -145,19 +151,11 @@ def test_service_discovery_default_namespace( if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: self.skipTest('Only running service discovery tests once') - # The monitor should discover the services automatically - self.assertIsNotNone(self.diagnostics_monitor.manage_topic_client) - self.assertIsNotNone(self.diagnostics_monitor.set_expected_frequency_client) - - # Verify services are available - manage_available = self.diagnostics_monitor.manage_topic_client.wait_for_service( - timeout_sec=5.0) - set_freq_available = ( - self.diagnostics_monitor.set_expected_frequency_client - .wait_for_service(timeout_sec=5.0)) - - self.assertTrue(manage_available, 'ManageTopic service should be available') - self.assertTrue(set_freq_available, 'SetExpectedFrequency service should be available') + # The monitor should be able to set parameters on the discovered node + # Test by setting and then clearing an expected frequency + self.diagnostics_monitor.set_expected_frequency('/test_topic', 50.0, 10.0) + time.sleep(0.5) + self.diagnostics_monitor.set_expected_frequency('/test_topic', float('nan'), 0.0) def test_diagnostic_data_conversion(self, expected_frequency, message_type, tolerance_hz): """Test conversion from DiagnosticStatus to UiDiagnosticData.""" @@ -290,11 +288,24 @@ def test_diagnostic_data_thread_safety(self, expected_frequency, message_type, t update_count = 0 error_occurred = False + # Wait for diagnostic data to be available before starting thread safety test + max_wait_time = 5.0 + start_time = time.time() + while time.time() - start_time < max_wait_time: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + data = self.diagnostics_monitor.get_topic_diagnostics(test_topic) + if data.status != '-': + break + time.sleep(0.1) + + self.assertNotEqual( + self.diagnostics_monitor.get_topic_diagnostics(test_topic).status, '-', + f'Diagnostics not available after {max_wait_time}s') + def update_thread(): nonlocal update_count, error_occurred try: for _ in range(50): - # Simulate concurrent diagnostic updates data = self.diagnostics_monitor.get_topic_diagnostics(test_topic) if data.status != '-': update_count += 1 @@ -311,7 +322,6 @@ def spin_thread(): except Exception: error_occurred = True - # Start concurrent threads threads = [ threading.Thread(target=update_thread), threading.Thread(target=spin_thread) @@ -323,7 +333,6 @@ def spin_thread(): for thread in threads: thread.join() - # Should not have encountered any thread safety issues self.assertFalse(error_occurred, 'Thread safety error occurred') self.assertGreater(update_count, 0, 'Should have received some diagnostic updates') @@ -364,26 +373,17 @@ def test_diagnostics_callback_processing(self, expected_frequency, message_type, # Check that timestamp was updated recently self.assertGreater(topic_data.last_update, time.time() - 10.0) - def test_service_timeout_handling(self, expected_frequency, message_type, tolerance_hz): - """Test service call timeout handling.""" + def test_toggle_topic_monitoring(self, expected_frequency, message_type, tolerance_hz): + """Test toggling topic monitoring via enabled parameter.""" if (message_type, expected_frequency, tolerance_hz) != MANAGE_TOPIC_TEST_CONFIG: - self.skipTest('Only running timeout handling tests once') - - # Create a client to a non-existent service - fake_client = self.test_node.create_client(ManageTopic, '/nonexistent_service') - - # Replace the real client temporarily - original_client = self.diagnostics_monitor.manage_topic_client - self.diagnostics_monitor.manage_topic_client = fake_client - - try: - # This should handle the service not being available gracefully - self.diagnostics_monitor.toggle_topic_monitoring('/some_topic') - # Should not crash or raise exceptions - finally: - # Restore original client - self.diagnostics_monitor.manage_topic_client = original_client - self.test_node.destroy_client(fake_client) + self.skipTest('Only running toggle monitoring tests once') + + # Wait for initial diagnostics to be received + time.sleep(2.0) + + # This should handle toggling the enabled parameter + self.diagnostics_monitor.toggle_topic_monitoring('/test_topic') + # Should not crash or raise exceptions if __name__ == '__main__': diff --git a/greenwave_monitor_interfaces/CMakeLists.txt b/greenwave_monitor_interfaces/CMakeLists.txt deleted file mode 100644 index a3c2b46..0000000 --- a/greenwave_monitor_interfaces/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -# 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 - -cmake_minimum_required(VERSION 3.8) -project(greenwave_monitor_interfaces) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/ManageTopic.srv" - "srv/SetExpectedFrequency.srv" -) - -ament_package() \ No newline at end of file diff --git a/greenwave_monitor_interfaces/package.xml b/greenwave_monitor_interfaces/package.xml deleted file mode 100644 index 8e9e6ee..0000000 --- a/greenwave_monitor_interfaces/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - greenwave_monitor_interfaces - 0.1.0 - Interfaces for the greenwave_monitor package - Sean Gillen - Apache-2.0 - Sean Gillen/sgillen@nvidia.com, Ann Li/liann@nvidia.com> - - ament_cmake - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/greenwave_monitor_interfaces/srv/ManageTopic.srv b/greenwave_monitor_interfaces/srv/ManageTopic.srv deleted file mode 100644 index ae8abb1..0000000 --- a/greenwave_monitor_interfaces/srv/ManageTopic.srv +++ /dev/null @@ -1,24 +0,0 @@ -# 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 - -# Request -string topic_name -bool add_topic # true to add, false to remove ---- -# Response -bool success -string message \ No newline at end of file diff --git a/greenwave_monitor_interfaces/srv/SetExpectedFrequency.srv b/greenwave_monitor_interfaces/srv/SetExpectedFrequency.srv deleted file mode 100644 index cf61ff4..0000000 --- a/greenwave_monitor_interfaces/srv/SetExpectedFrequency.srv +++ /dev/null @@ -1,27 +0,0 @@ -# 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 - -# Request -string topic_name -float64 expected_hz -float64 tolerance_percent # i.e. 5 = 5% of expected_hz -bool clear_expected -bool add_topic_if_missing # add topic to monitoring if not already ---- -# Response -bool success -string message \ No newline at end of file diff --git a/scripts/build_debian_packages.sh b/scripts/build_debian_packages.sh index 8e7f8be..6e5acf0 100755 --- a/scripts/build_debian_packages.sh +++ b/scripts/build_debian_packages.sh @@ -19,7 +19,7 @@ # Build Debian packages for greenwave_monitor # -# This script builds .deb packages for greenwave_monitor_interfaces and greenwave_monitor +# This script builds .deb packages for greenwave_monitor # # Usage: ./scripts/build_debian_packages.sh [ROS_DISTRO] [UBUNTU_DISTRO] # Examples: @@ -146,10 +146,6 @@ source install/setup.bash echo "Setting up rosdep mappings..." mkdir -p ~/.ros/rosdep cat >~/.ros/rosdep/local.yaml <