Add controlling topic expected frequencies and tolerances through ROS 2 parameters

Signed-off-by: Blake McHale <bmchale@nvidia.com>

Author: bmchalenv

greenwave_monitor/CMakeLists.txt

@@ -110,6 +110,17 @@ if(BUILD_TESTING)
     WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
   )
 
+  # Add parameter-based topic configuration tests (in test/parameters/)
+  # Automatically discover and add all test_*.py files in the parameters directory
+  file(GLOB PARAM_TEST_FILES "${CMAKE_SOURCE_DIR}/test/parameters/test_*.py")
+  foreach(TEST_FILE ${PARAM_TEST_FILES})
+    get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE)
+    ament_add_pytest_test(${TEST_NAME} test/parameters/${TEST_NAME}.py
+      TIMEOUT 120
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+  endforeach()
+
   # Add gtests
   ament_add_gtest(test_message_diagnostics test/test_message_diagnostics.cpp
     TIMEOUT 60

greenwave_monitor/examples/example.launch.py

@@ -13,7 +13,6 @@
 # limitations under the License.
 
 from launch import LaunchDescription
-from launch.actions import LogInfo
 from launch_ros.actions import Node
 
 
@@ -52,11 +51,13 @@ def generate_launch_description():
             name='greenwave_monitor',
             output='log',
             parameters=[
-                {'topics': ['/imu_topic', '/image_topic', '/string_topic']}
-            ],
-        ),
-        LogInfo(
-            msg='Run `ros2 run r2s_gw r2s_gw` in another terminal to see the demo output '
-                'with the r2s dashboard.'
-        ),
+                {
+                    'topics': {
+                        '/imu_topic': {'expected_frequency': 100.0},
+                        '/image_topic': {'expected_frequency': 30.0},
+                        '/string_topic': {'expected_frequency': 1000.0}
+                    },
+                }
+            ]
+        )
     ])

greenwave_monitor/greenwave_monitor/test_utils.py

@@ -22,6 +22,11 @@
 from typing import List, Optional, Tuple
 
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
+from greenwave_monitor.ui_adaptor import (
+    FREQ_SUFFIX,
+    TOL_SUFFIX,
+    TOPIC_PARAM_PREFIX,
+)
 from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency
 import launch_ros
 import rclpy
@@ -65,19 +70,32 @@ def create_minimal_publisher(
 
 def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE,
                         node_name: str = MONITOR_NODE_NAME,
-                        topics: List[str] = None):
+                        topics: List[str] = None,
+                        topic_configs: dict = None):
     """Create a greenwave_monitor node for testing."""
     if topics is None:
         topics = ['/test_topic']
 
+    # Ensure topics list has at least one element (even if empty string)
+    if not topics:
+        topics = ['']
+
+    params = {'topics': topics}
+
+    if topic_configs:
+        for topic, config in topic_configs.items():
+            if 'expected_frequency' in config:
+                params[f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}'] = float(
+                    config['expected_frequency'])
+            if 'tolerance' in config:
+                params[f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}'] = float(config['tolerance'])
+
     return launch_ros.actions.Node(
         package='greenwave_monitor',
         executable='greenwave_monitor',
         name=node_name,
         namespace=namespace,
-        parameters=[{
-            'topics': topics
-        }],
+        parameters=[params],
         output='screen'
     )
 

greenwave_monitor/greenwave_monitor/ui_adaptor.py

@@ -36,15 +36,58 @@
 """
 
 from dataclasses import dataclass
+from enum import Enum
 import threading
 import time
 from typing import Dict
 
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
 from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency
+from rcl_interfaces.msg import ParameterEvent, ParameterType, ParameterValue
+from rcl_interfaces.srv import GetParameters, ListParameters
 import rclpy
 from rclpy.node import Node
 
+# Parameter name constants
+TOPIC_PARAM_PREFIX = 'topics.'
+FREQ_SUFFIX = '.expected_frequency'
+TOL_SUFFIX = '.tolerance'
+DEFAULT_TOLERANCE_PERCENT = 5.0
+
+
+class TopicParamField(Enum):
+    """Type of topic parameter field."""
+
+    NONE = 0
+    FREQUENCY = 1
+    TOLERANCE = 2
+
+
+def parse_topic_param_name(param_name: str) -> tuple[str, TopicParamField]:
+    """Parse parameter name to extract topic name and field type."""
+    if not param_name.startswith(TOPIC_PARAM_PREFIX):
+        return ('', TopicParamField.NONE)
+
+    topic_and_field = param_name[len(TOPIC_PARAM_PREFIX):]
+
+    if topic_and_field.endswith(FREQ_SUFFIX):
+        topic_name = topic_and_field[:-len(FREQ_SUFFIX)]
+        return (topic_name, TopicParamField.FREQUENCY)
+    elif topic_and_field.endswith(TOL_SUFFIX):
+        topic_name = topic_and_field[:-len(TOL_SUFFIX)]
+        return (topic_name, TopicParamField.TOLERANCE)
+
+    return ('', TopicParamField.NONE)
+
+
+def param_value_to_float(value: ParameterValue) -> float | None:
+    """Convert a ParameterValue to float if it's a numeric type."""
+    if value.type == ParameterType.PARAMETER_DOUBLE:
+        return value.double_value
+    elif value.type == ParameterType.PARAMETER_INTEGER:
+        return float(value.integer_value)
+    return None
+
 
 @dataclass
 class UiDiagnosticData:
@@ -119,6 +162,13 @@ def _setup_ros_components(self):
             100
         )
 
+        self.param_events_subscription = self.node.create_subscription(
+            ParameterEvent,
+            '/parameter_events',
+            self._on_parameter_event,
+            10
+        )
+
         manage_service_name = f'{self.monitor_node_name}/manage_topic'
         set_freq_service_name = f'{self.monitor_node_name}/set_expected_frequency'
 
@@ -134,6 +184,108 @@ def _setup_ros_components(self):
             set_freq_service_name
         )
 
+        # Parameter service clients for querying initial state
+        self.list_params_client = self.node.create_client(
+            ListParameters,
+            f'{self.monitor_node_name}/list_parameters'
+        )
+        self.get_params_client = self.node.create_client(
+            GetParameters,
+            f'{self.monitor_node_name}/get_parameters'
+        )
+
+        # Query initial parameters after a short delay to let services come up
+        self._initial_params_timer = self.node.create_timer(
+            0.1, self._fetch_initial_parameters_callback)
+
+    def _fetch_initial_parameters_callback(self):
+        """Timer callback to fetch initial parameters (retries until services ready)."""
+        # Check if services are available (non-blocking)
+        if not self.list_params_client.service_is_ready():
+            return  # Timer will retry
+
+        if not self.get_params_client.service_is_ready():
+            return  # Timer will retry
+
+        # Cancel timer now that services are ready
+        if self._initial_params_timer is not None:
+            self._initial_params_timer.cancel()
+            self._initial_params_timer = None
+
+        # List all parameters (prefixes filter is unreliable with nested names)
+        list_request = ListParameters.Request()
+        list_request.prefixes = ['topics']
+        list_request.depth = 10
+
+        list_future = self.list_params_client.call_async(list_request)
+        list_future.add_done_callback(self._on_list_parameters_response)
+
+    def _on_list_parameters_response(self, future):
+        """Handle response from list_parameters service."""
+        try:
+            if future.result() is None:
+                return
+
+            all_param_names = future.result().result.names
+
+            # Filter to only topic parameters (prefixes filter is unreliable)
+            param_names = [n for n in all_param_names if n.startswith(TOPIC_PARAM_PREFIX)]
+
+            if not param_names:
+                return
+
+            # Store for use in get callback
+            self._pending_param_names = param_names
+
+            # Get values for topic parameters only
+            get_request = GetParameters.Request()
+            get_request.names = param_names
+
+            get_future = self.get_params_client.call_async(get_request)
+            get_future.add_done_callback(self._on_get_parameters_response)
+        except Exception as e:
+            self.node.get_logger().debug(f'Error listing parameters: {e}')
+
+    def _on_get_parameters_response(self, future):
+        """Handle response from get_parameters service."""
+        try:
+            if future.result() is None:
+                return
+
+            param_names = getattr(self, '_pending_param_names', [])
+            values = future.result().values
+
+            # Parse parameters into expected_frequencies
+            topic_configs: Dict[str, Dict[str, float]] = {}
+
+            for name, value in zip(param_names, values):
+                numeric_value = param_value_to_float(value)
+                if numeric_value is None:
+                    continue
+
+                topic_name, field = parse_topic_param_name(name)
+                if not topic_name or field == TopicParamField.NONE:
+                    continue
+
+                if topic_name not in topic_configs:
+                    topic_configs[topic_name] = {}
+
+                if field == TopicParamField.FREQUENCY:
+                    topic_configs[topic_name]['freq'] = numeric_value
+                elif field == TopicParamField.TOLERANCE:
+                    topic_configs[topic_name]['tol'] = numeric_value
+
+            # Update expected_frequencies with valid configs
+            with self.data_lock:
+                for topic_name, config in topic_configs.items():
+                    freq = config.get('freq', 0.0)
+                    tol = config.get('tol', DEFAULT_TOLERANCE_PERCENT)
+                    if freq > 0:
+                        self.expected_frequencies[topic_name] = (freq, tol)
+
+        except Exception as e:
+            self.node.get_logger().debug(f'Error fetching parameters: {e}')
+
     def _extract_topic_name(self, diagnostic_name: str) -> str:
         """
         Extract topic name from diagnostic status name.
@@ -168,6 +320,35 @@ def _on_diagnostics(self, msg: DiagnosticArray):
                 topic_name = self._extract_topic_name(status.name)
                 self.ui_diagnostics[topic_name] = ui_data
 
+    def _on_parameter_event(self, msg: ParameterEvent):
+        """Process parameter change events from the monitor node."""
+        # Only process events from the monitor node
+        if self.monitor_node_name not in msg.node:
+            return
+
+        # Process changed and new parameters
+        for param in msg.changed_parameters + msg.new_parameters:
+            value = param_value_to_float(param.value)
+            if value is None:
+                continue
+
+            topic_name, field = parse_topic_param_name(param.name)
+            if not topic_name or field == TopicParamField.NONE:
+                continue
+
+            with self.data_lock:
+                current = self.expected_frequencies.get(topic_name, (0.0, 0.0))
+
+                if field == TopicParamField.FREQUENCY:
+                    if value > 0:
+                        self.expected_frequencies[topic_name] = (value, current[1])
+                    elif topic_name in self.expected_frequencies:
+                        del self.expected_frequencies[topic_name]
+
+                elif field == TopicParamField.TOLERANCE:
+                    if current[0] > 0:  # Only update if frequency is set
+                        self.expected_frequencies[topic_name] = (current[0], value)
+
     def toggle_topic_monitoring(self, topic_name: str):
         """Toggle monitoring for a topic."""
         if not self.manage_topic_client.wait_for_service(timeout_sec=1.0):

greenwave_monitor/include/greenwave_monitor.hpp

@@ -26,6 +26,7 @@
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "diagnostic_msgs/msg/diagnostic_array.hpp"
 #include "diagnostic_msgs/msg/diagnostic_status.hpp"
@@ -40,6 +41,12 @@ class GreenwaveMonitor : public rclcpp::Node
   explicit GreenwaveMonitor(const rclcpp::NodeOptions & options);
 
 private:
+  struct TopicConfig
+  {
+    std::optional<double> expected_frequency;
+    std::optional<double> tolerance;
+  };
+
   std::optional<std::string> find_topic_type_with_retry(
     const std::string & topic, const int max_retries, const int retry_period_s);
 
@@ -57,6 +64,29 @@ class GreenwaveMonitor : public rclcpp::Node
     const std::shared_ptr<greenwave_monitor_interfaces::srv::SetExpectedFrequency::Request> request,
     std::shared_ptr<greenwave_monitor_interfaces::srv::SetExpectedFrequency::Response> response);
 
+  bool set_topic_expected_frequency(
+    const std::string & topic_name,
+    double expected_hz,
+    double tolerance_percent,
+    bool add_topic_if_missing,
+    std::string & message,
+    bool update_parameters = true);
+
+  rcl_interfaces::msg::SetParametersResult on_parameter_change(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  void apply_topic_config_if_complete(const std::string & topic_name);
+
+  void load_topic_parameters_from_overrides();
+
+  std::optional<double> get_numeric_parameter(const std::string & param_name);
+
+  void try_undeclare_parameter(const std::string & param_name);
+
+  void declare_or_set_parameter(const std::string & param_name, double value);
+
+  void undeclare_topic_parameters(const std::string & topic_name);
+
   bool add_topic(const std::string & topic, std::string & message);
 
   bool remove_topic(const std::string & topic, std::string & message);
@@ -76,4 +106,7 @@ class GreenwaveMonitor : public rclcpp::Node
     manage_topic_service_;
   rclcpp::Service<greenwave_monitor_interfaces::srv::SetExpectedFrequency>::SharedPtr
     set_expected_frequency_service_;
+
+  std::map<std::string, TopicConfig> pending_topic_configs_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
 };