From 256a121442a361863b2b8937317786e111a58f7a Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 09:35:40 -0800 Subject: [PATCH 01/33] Add controlling topic expected frequencies and tolerances through ROS 2 parameters Signed-off-by: Blake McHale --- greenwave_monitor/CMakeLists.txt | 11 + greenwave_monitor/examples/example.launch.py | 17 +- .../greenwave_monitor/test_utils.py | 26 +- .../greenwave_monitor/ui_adaptor.py | 181 +++++++++ .../include/greenwave_monitor.hpp | 33 ++ greenwave_monitor/src/greenwave_monitor.cpp | 365 +++++++++++++++++- .../test/parameters/test_param_dynamic.py | 199 ++++++++++ .../test/parameters/test_param_freq_only.py | 118 ++++++ .../test/parameters/test_param_new_topic.py | 131 +++++++ .../test/parameters/test_param_tol_only.py | 107 +++++ .../test/parameters/test_param_yaml.py | 132 +++++++ .../test/parameters/test_topic_parameters.py | 162 ++++++++ .../test/test_topic_monitoring_integration.py | 8 + 13 files changed, 1462 insertions(+), 28 deletions(-) create mode 100644 greenwave_monitor/test/parameters/test_param_dynamic.py create mode 100644 greenwave_monitor/test/parameters/test_param_freq_only.py create mode 100644 greenwave_monitor/test/parameters/test_param_new_topic.py create mode 100644 greenwave_monitor/test/parameters/test_param_tol_only.py create mode 100644 greenwave_monitor/test/parameters/test_param_yaml.py create mode 100644 greenwave_monitor/test/parameters/test_topic_parameters.py diff --git a/greenwave_monitor/CMakeLists.txt b/greenwave_monitor/CMakeLists.txt index 7c31f6d..29c1a56 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -116,6 +116,17 @@ if(BUILD_TESTING) WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) + # Add parameter-based topic configuration tests (in test/parameters/) + # Automatically discover and add all test_*.py files in the parameters directory + file(GLOB PARAM_TEST_FILES "${CMAKE_SOURCE_DIR}/test/parameters/test_*.py") + foreach(TEST_FILE ${PARAM_TEST_FILES}) + get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) + ament_add_pytest_test(${TEST_NAME} test/parameters/${TEST_NAME}.py + TIMEOUT 120 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + # Add gtests ament_add_gtest(test_message_diagnostics test/test_message_diagnostics.cpp TIMEOUT 60 diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index b7266c1..6f05710 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -13,7 +13,6 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import LogInfo from launch_ros.actions import Node @@ -52,11 +51,13 @@ def generate_launch_description(): name='greenwave_monitor', output='log', parameters=[ - {'topics': ['/imu_topic', '/image_topic', '/string_topic']} - ], - ), - LogInfo( - msg='Run `ros2 run r2s_gw r2s_gw` in another terminal to see the demo output ' - 'with the r2s dashboard.' - ), + { + 'topics': { + '/imu_topic': {'expected_frequency': 100.0}, + '/image_topic': {'expected_frequency': 30.0}, + '/string_topic': {'expected_frequency': 1000.0} + }, + } + ] + ) ]) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 7434b1d..67d2d87 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -22,6 +22,11 @@ from typing import List, Optional, Tuple from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from greenwave_monitor.ui_adaptor import ( + FREQ_SUFFIX, + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency import launch_ros import rclpy @@ -65,19 +70,32 @@ def create_minimal_publisher( def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE, node_name: str = MONITOR_NODE_NAME, - topics: List[str] = None): + topics: List[str] = None, + topic_configs: dict = None): """Create a greenwave_monitor node for testing.""" if topics is None: topics = ['/test_topic'] + # Ensure topics list has at least one element (even if empty string) + if not topics: + topics = [''] + + params = {'topics': topics} + + if topic_configs: + for topic, config in topic_configs.items(): + if 'expected_frequency' in config: + params[f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}'] = float( + config['expected_frequency']) + if 'tolerance' in config: + params[f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}'] = float(config['tolerance']) + return launch_ros.actions.Node( package='greenwave_monitor', executable='greenwave_monitor', name=node_name, namespace=namespace, - parameters=[{ - 'topics': topics - }], + parameters=[params], output='screen' ) diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 09ed0f8..2e7ba8d 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -36,15 +36,58 @@ """ from dataclasses import dataclass +from enum import Enum import threading import time from typing import Dict from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency +from rcl_interfaces.msg import ParameterEvent, ParameterType, ParameterValue +from rcl_interfaces.srv import GetParameters, ListParameters import rclpy from rclpy.node import Node +# Parameter name constants +TOPIC_PARAM_PREFIX = 'topics.' +FREQ_SUFFIX = '.expected_frequency' +TOL_SUFFIX = '.tolerance' +DEFAULT_TOLERANCE_PERCENT = 5.0 + + +class TopicParamField(Enum): + """Type of topic parameter field.""" + + NONE = 0 + FREQUENCY = 1 + TOLERANCE = 2 + + +def parse_topic_param_name(param_name: str) -> tuple[str, TopicParamField]: + """Parse parameter name to extract topic name and field type.""" + if not param_name.startswith(TOPIC_PARAM_PREFIX): + return ('', TopicParamField.NONE) + + topic_and_field = param_name[len(TOPIC_PARAM_PREFIX):] + + if topic_and_field.endswith(FREQ_SUFFIX): + topic_name = topic_and_field[:-len(FREQ_SUFFIX)] + return (topic_name, TopicParamField.FREQUENCY) + elif topic_and_field.endswith(TOL_SUFFIX): + topic_name = topic_and_field[:-len(TOL_SUFFIX)] + return (topic_name, TopicParamField.TOLERANCE) + + return ('', TopicParamField.NONE) + + +def param_value_to_float(value: ParameterValue) -> float | None: + """Convert a ParameterValue to float if it's a numeric type.""" + if value.type == ParameterType.PARAMETER_DOUBLE: + return value.double_value + elif value.type == ParameterType.PARAMETER_INTEGER: + return float(value.integer_value) + return None + @dataclass class UiDiagnosticData: @@ -119,6 +162,13 @@ def _setup_ros_components(self): 100 ) + self.param_events_subscription = self.node.create_subscription( + ParameterEvent, + '/parameter_events', + self._on_parameter_event, + 10 + ) + manage_service_name = f'{self.monitor_node_name}/manage_topic' set_freq_service_name = f'{self.monitor_node_name}/set_expected_frequency' @@ -134,6 +184,108 @@ def _setup_ros_components(self): set_freq_service_name ) + # Parameter service clients for querying initial state + self.list_params_client = self.node.create_client( + ListParameters, + f'{self.monitor_node_name}/list_parameters' + ) + self.get_params_client = self.node.create_client( + GetParameters, + f'{self.monitor_node_name}/get_parameters' + ) + + # Query initial parameters after a short delay to let services come up + self._initial_params_timer = self.node.create_timer( + 0.1, self._fetch_initial_parameters_callback) + + def _fetch_initial_parameters_callback(self): + """Timer callback to fetch initial parameters (retries until services ready).""" + # Check if services are available (non-blocking) + if not self.list_params_client.service_is_ready(): + return # Timer will retry + + if not self.get_params_client.service_is_ready(): + return # Timer will retry + + # Cancel timer now that services are ready + if self._initial_params_timer is not None: + self._initial_params_timer.cancel() + self._initial_params_timer = None + + # List all parameters (prefixes filter is unreliable with nested names) + list_request = ListParameters.Request() + list_request.prefixes = ['topics'] + list_request.depth = 10 + + list_future = self.list_params_client.call_async(list_request) + list_future.add_done_callback(self._on_list_parameters_response) + + def _on_list_parameters_response(self, future): + """Handle response from list_parameters service.""" + try: + if future.result() is None: + return + + all_param_names = future.result().result.names + + # Filter to only topic parameters (prefixes filter is unreliable) + param_names = [n for n in all_param_names if n.startswith(TOPIC_PARAM_PREFIX)] + + if not param_names: + return + + # Store for use in get callback + self._pending_param_names = param_names + + # Get values for topic parameters only + get_request = GetParameters.Request() + get_request.names = param_names + + get_future = self.get_params_client.call_async(get_request) + get_future.add_done_callback(self._on_get_parameters_response) + except Exception as e: + self.node.get_logger().debug(f'Error listing parameters: {e}') + + def _on_get_parameters_response(self, future): + """Handle response from get_parameters service.""" + try: + if future.result() is None: + return + + param_names = getattr(self, '_pending_param_names', []) + values = future.result().values + + # Parse parameters into expected_frequencies + topic_configs: Dict[str, Dict[str, float]] = {} + + for name, value in zip(param_names, values): + numeric_value = param_value_to_float(value) + if numeric_value is None: + continue + + topic_name, field = parse_topic_param_name(name) + if not topic_name or field == TopicParamField.NONE: + continue + + if topic_name not in topic_configs: + topic_configs[topic_name] = {} + + if field == TopicParamField.FREQUENCY: + topic_configs[topic_name]['freq'] = numeric_value + elif field == TopicParamField.TOLERANCE: + topic_configs[topic_name]['tol'] = numeric_value + + # Update expected_frequencies with valid configs + with self.data_lock: + for topic_name, config in topic_configs.items(): + freq = config.get('freq', 0.0) + tol = config.get('tol', DEFAULT_TOLERANCE_PERCENT) + if freq > 0: + self.expected_frequencies[topic_name] = (freq, tol) + + except Exception as e: + self.node.get_logger().debug(f'Error fetching parameters: {e}') + def _extract_topic_name(self, diagnostic_name: str) -> str: """ Extract topic name from diagnostic status name. @@ -168,6 +320,35 @@ def _on_diagnostics(self, msg: DiagnosticArray): topic_name = self._extract_topic_name(status.name) self.ui_diagnostics[topic_name] = ui_data + def _on_parameter_event(self, msg: ParameterEvent): + """Process parameter change events from the monitor node.""" + # Only process events from the monitor node + if self.monitor_node_name not in msg.node: + return + + # Process changed and new parameters + for param in msg.changed_parameters + msg.new_parameters: + value = param_value_to_float(param.value) + if value is None: + continue + + topic_name, field = parse_topic_param_name(param.name) + if not topic_name or field == TopicParamField.NONE: + continue + + with self.data_lock: + current = self.expected_frequencies.get(topic_name, (0.0, 0.0)) + + if field == TopicParamField.FREQUENCY: + if value > 0: + self.expected_frequencies[topic_name] = (value, current[1]) + elif topic_name in self.expected_frequencies: + del self.expected_frequencies[topic_name] + + elif field == TopicParamField.TOLERANCE: + if current[0] > 0: # Only update if frequency is set + self.expected_frequencies[topic_name] = (current[0], value) + def toggle_topic_monitoring(self, topic_name: str): """Toggle monitoring for a topic.""" if not self.manage_topic_client.wait_for_service(timeout_sec=1.0): diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index f9a39bb..c736639 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -26,6 +26,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" #include "std_msgs/msg/string.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" @@ -40,6 +41,12 @@ class GreenwaveMonitor : public rclcpp::Node explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); private: + struct TopicConfig + { + std::optional expected_frequency; + std::optional tolerance; + }; + std::optional find_topic_type_with_retry( const std::string & topic, const int max_retries, const int retry_period_s); @@ -57,6 +64,29 @@ class GreenwaveMonitor : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + bool set_topic_expected_frequency( + const std::string & topic_name, + double expected_hz, + double tolerance_percent, + bool add_topic_if_missing, + std::string & message, + bool update_parameters = true); + + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters); + + void apply_topic_config_if_complete(const std::string & topic_name); + + void load_topic_parameters_from_overrides(); + + std::optional get_numeric_parameter(const std::string & param_name); + + void try_undeclare_parameter(const std::string & param_name); + + void declare_or_set_parameter(const std::string & param_name, double value); + + void undeclare_topic_parameters(const std::string & topic_name); + bool add_topic(const std::string & topic, std::string & message); bool remove_topic(const std::string & topic, std::string & message); @@ -76,4 +106,7 @@ class GreenwaveMonitor : public rclcpp::Node manage_topic_service_; rclcpp::Service::SharedPtr set_expected_frequency_service_; + + std::map pending_topic_configs_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e73fc65..86183d9 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -22,17 +22,113 @@ #include #include +#include "rcl_interfaces/srv/list_parameters.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" using namespace std::chrono_literals; +namespace +{ +constexpr const char * kTopicParamPrefix = "topics."; +constexpr const char * kFreqSuffix = ".expected_frequency"; +constexpr const char * kTolSuffix = ".tolerance"; +constexpr double kDefaultTolerancePercent = 5.0; + +std::string make_freq_param_name(const std::string & topic_name) +{ + return std::string(kTopicParamPrefix) + topic_name + kFreqSuffix; +} + +std::string make_tol_param_name(const std::string & topic_name) +{ + return std::string(kTopicParamPrefix) + topic_name + kTolSuffix; +} + +enum class TopicParamField { kNone, kFrequency, kTolerance }; + +struct TopicParamInfo +{ + std::string topic_name; + TopicParamField field = TopicParamField::kNone; +}; + +// Parse a parameter name like "topics./my_topic.expected_frequency" into topic name and field type +TopicParamInfo parse_topic_param_name(const std::string & param_name) +{ + TopicParamInfo info; + + if (param_name.rfind(kTopicParamPrefix, 0) != 0) { + return info; + } + + std::string topic_and_field = param_name.substr(strlen(kTopicParamPrefix)); + + const size_t freq_suffix_len = strlen(kFreqSuffix); + const size_t tol_suffix_len = strlen(kTolSuffix); + + if (topic_and_field.length() > freq_suffix_len && + topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) + { + info.topic_name = topic_and_field.substr(0, topic_and_field.length() - freq_suffix_len); + info.field = TopicParamField::kFrequency; + } else if (topic_and_field.length() > tol_suffix_len && + topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) + { + info.topic_name = topic_and_field.substr(0, topic_and_field.length() - tol_suffix_len); + info.field = TopicParamField::kTolerance; + } + + return info; +} + +// Convert a parameter to double if it's a numeric type +std::optional param_to_double(const rclcpp::Parameter & param) +{ + if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { + return param.as_double(); + } else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { + return static_cast(param.as_int()); + } + return std::nullopt; +} + +const char * get_field_name(TopicParamField field) +{ + if (field == TopicParamField::kNone) { + return "none"; + } else if (field == TopicParamField::kFrequency) { + return "expected_frequency"; + } else if (field == TopicParamField::kTolerance) { + return "tolerance"; + } + return "unknown"; +} + +const char * get_field_unit(TopicParamField field) +{ + if (field == TopicParamField::kNone) { + return ""; + } else if (field == TopicParamField::kFrequency) { + return "Hz"; + } else if (field == TopicParamField::kTolerance) { + return "%"; + } + return "unknown"; +} +} // namespace + GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) -: Node("greenwave_monitor", options) +: Node("greenwave_monitor", + rclcpp::NodeOptions(options) + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true)) { RCLCPP_INFO(this->get_logger(), "Starting GreenwaveMonitorNode"); - // Declare and get the topics parameter - this->declare_parameter>("topics", {""}); + // Get the topics parameter (declare only if not already declared from overrides) + if (!this->has_parameter("topics")) { + this->declare_parameter>("topics", {""}); + } auto topics = this->get_parameter("topics").as_string_array(); message_diagnostics::MessageDiagnosticsConfig diagnostics_config; @@ -47,6 +143,13 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) } } + // Register parameter callback for dynamic topic configuration + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&GreenwaveMonitor::on_parameter_change, this, std::placeholders::_1)); + + // Process any topic parameters passed at startup + load_topic_parameters_from_overrides(); + timer_ = this->create_wall_timer( 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); @@ -143,30 +246,256 @@ void GreenwaveMonitor::handle_set_expected_frequency( if (request->clear_expected) { msg_diagnostics_obj.clearExpectedDt(); + undeclare_topic_parameters(request->topic_name); + response->success = true; response->message = "Successfully cleared expected frequency for topic '" + request->topic_name + "'"; return; } - if (request->expected_hz <= 0.0) { - response->success = false; - response->message = "Invalid expected frequency, must be set to a positive value"; - return; + response->success = set_topic_expected_frequency( + request->topic_name, + request->expected_hz, + request->tolerance_percent, + false, // topic already exists at this point + response->message); +} + +bool GreenwaveMonitor::set_topic_expected_frequency( + const std::string & topic_name, + double expected_hz, + double tolerance_percent, + bool add_topic_if_missing, + std::string & message, + bool update_parameters) +{ + auto it = message_diagnostics_.find(topic_name); + + if (it == message_diagnostics_.end()) { + if (!add_topic_if_missing) { + message = "Failed to find topic '" + topic_name + "'"; + return false; + } + + if (!add_topic(topic_name, message)) { + return false; + } + it = message_diagnostics_.find(topic_name); + } + + if (expected_hz <= 0.0) { + message = "Invalid expected frequency, must be set to a positive value"; + return false; + } + if (tolerance_percent < 0.0) { + message = "Invalid tolerance, must be a non-negative percentage"; + return false; + } + + message_diagnostics::MessageDiagnostics & msg_diagnostics_obj = *(it->second); + msg_diagnostics_obj.setExpectedDt(expected_hz, tolerance_percent); + + // Sync parameters with the new values + if (update_parameters) { + declare_or_set_parameter(make_freq_param_name(topic_name), expected_hz); + declare_or_set_parameter(make_tol_param_name(topic_name), tolerance_percent); } - if (request->tolerance_percent < 0.0) { - response->success = false; - response->message = - "Invalid tolerance, must be a non-negative percentage"; + + message = "Successfully set expected frequency for topic '" + + topic_name + "' to " + std::to_string(expected_hz) + + " hz with tolerance " + std::to_string(tolerance_percent) + "%"; + return true; +} + +rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + auto info = parse_topic_param_name(param.get_name()); + if (info.field == TopicParamField::kNone || info.topic_name.empty()) { + continue; + } + + auto value_opt = param_to_double(param); + if (!value_opt.has_value()) { + RCLCPP_WARN( + this->get_logger(), + "Parameter '%s' is not a numeric type, skipping", + param.get_name().c_str()); + continue; + } + + double value = value_opt.value(); + TopicConfig & config = pending_topic_configs_[info.topic_name]; + + if (info.field == TopicParamField::kFrequency) { + config.expected_frequency = value; + } else { + config.tolerance = value; + } + + RCLCPP_INFO( + this->get_logger(), + "Parameter set: %s for topic '%s' = %.2f %s", + get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); + + apply_topic_config_if_complete(info.topic_name); + } + + return result; +} + +void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_name) +{ + auto it = pending_topic_configs_.find(topic_name); + if (it == pending_topic_configs_.end()) { return; } - msg_diagnostics_obj.setExpectedDt(request->expected_hz, request->tolerance_percent); + const TopicConfig & config = it->second; + + // Get expected frequency from pending config or existing parameter + double expected_freq = 0.0; + if (config.expected_frequency.has_value()) { + expected_freq = config.expected_frequency.value(); + } else { + auto freq_opt = get_numeric_parameter(make_freq_param_name(topic_name)); + if (freq_opt.has_value()) { + expected_freq = freq_opt.value(); + } else { + // No frequency available, nothing to do + return; + } + } + + // Get tolerance from pending config, existing parameter, or default + double tolerance = config.tolerance.value_or( + get_numeric_parameter(make_tol_param_name(topic_name)).value_or(kDefaultTolerancePercent) + ); + + std::string message; + bool success = set_topic_expected_frequency( + topic_name, + expected_freq, + tolerance, + true, + message, + false); // don't update parameters - called from parameter change + + if (success) { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } else { + RCLCPP_WARN( + this->get_logger(), + "Could not apply config for topic '%s': %s. " + "Use manage_topic service to add the topic first.", + topic_name.c_str(), message.c_str()); + } + + pending_topic_configs_.erase(it); +} + +void GreenwaveMonitor::load_topic_parameters_from_overrides() +{ + // Parameters are automatically declared from overrides due to NodeOptions setting. + // List all parameters and filter by prefix manually (list_parameters prefix matching + // can be unreliable with deeply nested parameter names). + auto all_params = this->list_parameters( + {}, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); + + for (const auto & name : all_params.names) { + auto info = parse_topic_param_name(name); + if (info.field == TopicParamField::kNone || info.topic_name.empty()) { + continue; + } + + auto value_opt = get_numeric_parameter(name); + if (!value_opt.has_value()) { + continue; + } + + double value = value_opt.value(); + TopicConfig & config = pending_topic_configs_[info.topic_name]; + + if (info.field == TopicParamField::kFrequency) { + config.expected_frequency = value; + } else { + config.tolerance = value; + } + + RCLCPP_INFO( + this->get_logger(), + "Initial parameter: %s for topic '%s' = %.2f %s", + get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); + } + + // Apply all complete configs - at startup we can add topics if needed + std::vector topics_to_apply; + for (const auto & [topic, config] : pending_topic_configs_) { + if (config.expected_frequency.has_value()) { + topics_to_apply.push_back(topic); + } + } + for (const auto & topic : topics_to_apply) { + const TopicConfig & config = pending_topic_configs_[topic]; + double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); + + std::string message; + bool success = set_topic_expected_frequency( + topic, + config.expected_frequency.value(), + tolerance, + true, // add topic if missing - safe at startup + message, + false); // don't update parameters + + if (success) { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } else { + RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); + } + pending_topic_configs_.erase(topic); + } +} + +std::optional GreenwaveMonitor::get_numeric_parameter(const std::string & param_name) +{ + if (!this->has_parameter(param_name)) { + return std::nullopt; + } + return param_to_double(this->get_parameter(param_name)); +} + +void GreenwaveMonitor::try_undeclare_parameter(const std::string & param_name) +{ + try { + if (this->has_parameter(param_name)) { + this->undeclare_parameter(param_name); + } + } catch (const std::exception & e) { + RCLCPP_WARN( + this->get_logger(), "Could not undeclare %s: %s", + param_name.c_str(), e.what()); + } +} + +void GreenwaveMonitor::declare_or_set_parameter(const std::string & param_name, double value) +{ + if (!this->has_parameter(param_name)) { + this->declare_parameter(param_name, value); + } else { + this->set_parameter(rclcpp::Parameter(param_name, value)); + } +} - response->success = true; - response->message = "Successfully set expected frequency for topic '" + - request->topic_name + "' to " + std::to_string(request->expected_hz) + - " hz with tolerance " + std::to_string(request->tolerance_percent) + "%"; +void GreenwaveMonitor::undeclare_topic_parameters(const std::string & topic_name) +{ + try_undeclare_parameter(make_freq_param_name(topic_name)); + try_undeclare_parameter(make_tol_param_name(topic_name)); } bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) @@ -315,6 +644,10 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes } message_diagnostics_.erase(diag_it); + + // Clear any associated parameters + undeclare_topic_parameters(topic); + message = "Successfully removed topic"; return true; } diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py new file mode 100644 index 0000000..392b950 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -0,0 +1,199 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: dynamic parameter changes via ros2 param set.""" + +import subprocess +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +from greenwave_monitor.ui_adaptor import ( + FREQ_SUFFIX, + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) +import launch +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/dynamic_param_topic' +TEST_FREQUENCY = 30.0 + + +def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: + """Run ros2 param set command and return success status.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] + try: + result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) + return result.returncode == 0 + except subprocess.TimeoutExpired: + return False + + +def run_ros2_param_get(node_name: str, param_name: str) -> tuple[bool, float | None]: + """Run ros2 param get command and return (success, value).""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + cmd = ['ros2', 'param', 'get', full_node_name, param_name] + try: + result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) + if result.returncode != 0: + return False, None + # Parse output like "Double value is: 30.0" or "Integer value is: 30" + output = result.stdout.strip() + if 'value is:' in output: + value_str = output.split('value is:')[1].strip() + return True, float(value_str) + return False, None + except (subprocess.TimeoutExpired, ValueError): + return False, None + + +def make_freq_param(topic: str) -> str: + """Build frequency parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' + + +def make_tol_param(topic: str) -> str: + """Build tolerance parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' + + +@pytest.mark.launch_test +def generate_test_description(): + """Test dynamic parameter changes via ros2 param set.""" + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[TEST_TOPIC], # Topic exists but no expected frequency + topic_configs={} + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestDynamicParameterChanges(unittest.TestCase): + """Test changing parameters dynamically via ros2 param set.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('dynamic_param_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_set_expected_frequency_via_param(self): + """Test setting expected frequency via ros2 param set.""" + time.sleep(2.0) + + freq_param = make_freq_param(TEST_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + time.sleep(1.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Expected diagnostics after setting frequency param' + ) + + def test_change_tolerance_via_param(self): + """Test changing tolerance via ros2 param set.""" + time.sleep(1.0) + + tol_param = make_tol_param(TEST_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, 20.0) + self.assertTrue(success, f'Failed to set {tol_param}') + + time.sleep(0.5) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=2, timeout_sec=5.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 2, + 'Topic should still be monitored after tolerance change' + ) + + def test_verify_params_with_get(self): + """Test that ros2 param get returns the values we set.""" + time.sleep(1.0) + + # Set specific values + freq_param = make_freq_param(TEST_TOPIC) + tol_param = make_tol_param(TEST_TOPIC) + expected_freq = 42.5 + expected_tol = 15.0 + + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, expected_freq) + self.assertTrue(success, f'Failed to set {freq_param}') + + success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, expected_tol) + self.assertTrue(success, f'Failed to set {tol_param}') + + time.sleep(0.5) + + # Verify with ros2 param get + success, actual_freq = run_ros2_param_get(MONITOR_NODE_NAME, freq_param) + self.assertTrue(success, f'Failed to get {freq_param}') + self.assertAlmostEqual( + actual_freq, expected_freq, places=1, + msg=f'Frequency mismatch: expected {expected_freq}, got {actual_freq}' + ) + + success, actual_tol = run_ros2_param_get(MONITOR_NODE_NAME, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, expected_tol, places=1, + msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py new file mode 100644 index 0000000..df1d89a --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: only expected_frequency specified, tolerance defaults to 5%.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +import launch +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/freq_only_topic' +TEST_FREQUENCY = 50.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test with only expected_frequency specified.""" + topic_configs = { + TEST_TOPIC: { + 'expected_frequency': TEST_FREQUENCY + # No tolerance - should default to 5% + } + } + + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[], + topic_configs=topic_configs + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_freq_only' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestFrequencyOnlyParameter(unittest.TestCase): + """Test that only specifying frequency works (tolerance defaults).""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('freq_only_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_frequency_only_uses_default_tolerance(self): + """Test that specifying only frequency uses default tolerance.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + f'Expected at least 3 diagnostics, got {len(received_diagnostics)}' + ) + + has_valid_rate = False + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + has_valid_rate = True + break + except ValueError: + continue + if has_valid_rate: + break + + self.assertTrue(has_valid_rate, 'Should have valid frame rate with default tolerance') + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py new file mode 100644 index 0000000..1875516 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: add new topic to monitoring via ros2 param set.""" + +import subprocess +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +from greenwave_monitor.ui_adaptor import ( + FREQ_SUFFIX, + TOPIC_PARAM_PREFIX, +) +import launch +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +NEW_TOPIC = '/new_dynamic_topic' +TEST_FREQUENCY = 50.0 + + +def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: + """Run ros2 param set command and return success status.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] + try: + result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) + return result.returncode == 0 + except subprocess.TimeoutExpired: + return False + + +def make_freq_param(topic: str) -> str: + """Build frequency parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' + + +@pytest.mark.launch_test +def generate_test_description(): + """Test adding a new topic via ros2 param set.""" + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[''], # Empty - no initial topics + topic_configs={} + ) + + publisher = create_minimal_publisher( + NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestAddNewTopicViaParam(unittest.TestCase): + """Test adding a new topic to monitoring via ros2 param set.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('new_topic_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_add_new_topic_via_frequency_param(self): + """Test that setting frequency param for new topic starts monitoring.""" + time.sleep(2.0) + + initial_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(initial_diagnostics), 0, + 'Topic should not be monitored initially' + ) + + freq_param = make_freq_param(NEW_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Should monitor new topic after setting frequency param' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py new file mode 100644 index 0000000..ddc8f08 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: only tolerance specified - should NOT start monitoring.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +from greenwave_monitor.ui_adaptor import ( + TOL_SUFFIX, + TOPIC_PARAM_PREFIX, +) +import launch +import launch_ros.actions +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/tol_only_topic' +TEST_FREQUENCY = 50.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test with only tolerance specified (should not monitor).""" + params = { + 'topics': [''], + f'{TOPIC_PARAM_PREFIX}{TEST_TOPIC}{TOL_SUFFIX}': 15.0 + } + + ros2_monitor_node = launch_ros.actions.Node( + package='greenwave_monitor', + executable='greenwave_monitor', + name=MONITOR_NODE_NAME, + namespace=MONITOR_NODE_NAMESPACE, + parameters=[params], + output='screen' + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_tol_only' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestToleranceOnlyParameter(unittest.TestCase): + """Test that only specifying tolerance does NOT start monitoring.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('tol_only_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_tolerance_only_does_not_monitor(self): + """Test that specifying only tolerance does not start monitoring.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=3.0 + ) + + self.assertEqual( + len(received_diagnostics), 0, + f'Should not monitor topic with only tolerance set, got {len(received_diagnostics)}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py new file mode 100644 index 0000000..6c1d7c5 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: load topic configuration from YAML parameter file.""" + +import os +import tempfile +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +import launch +import launch_ros.actions +import launch_testing +import pytest +import rclpy +from rclpy.node import Node + + +YAML_TOPIC = '/yaml_config_topic' +TEST_FREQUENCY = 50.0 +TEST_TOLERANCE = 10.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test loading parameters from a YAML file.""" + # Write YAML manually with proper quoting (yaml.dump doesn't quote keys with dots) + # Use /** to apply to all nodes, or use full namespace path + yaml_content = ( + f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n' + f' ros__parameters:\n' + f' topics:\n' + f" - ''\n" + f' "topics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n' + f' "topics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n' + ) + + yaml_dir = tempfile.mkdtemp() + yaml_path = os.path.join(yaml_dir, 'test_params.yaml') + with open(yaml_path, 'w') as f: + f.write(yaml_content) + + ros2_monitor_node = launch_ros.actions.Node( + package='greenwave_monitor', + executable='greenwave_monitor', + name=MONITOR_NODE_NAME, + namespace=MONITOR_NODE_NAMESPACE, + parameters=[yaml_path], + output='screen' + ) + + publisher = create_minimal_publisher( + YAML_TOPIC, TEST_FREQUENCY, 'imu', '_yaml' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestYamlParameterFile(unittest.TestCase): + """Test loading topic configuration from YAML parameter file.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('yaml_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_topic_configured_via_yaml(self): + """Test that topic is monitored when configured via YAML file.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, YAML_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Expected diagnostics from YAML-configured topic' + ) + + has_valid_rate = False + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + has_valid_rate = True + break + except ValueError: + continue + if has_valid_rate: + break + + self.assertTrue(has_valid_rate, 'Should have valid frame rate from YAML config') + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py new file mode 100644 index 0000000..4add93f --- /dev/null +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for parameter-based topic configuration - both frequency and tolerance.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE +) +import launch +import launch_testing +from launch_testing import post_shutdown_test +from launch_testing.asserts import assertExitCodes +import pytest +import rclpy +from rclpy.node import Node + + +TEST_TOPIC = '/param_test_topic' +TEST_FREQUENCY = 50.0 +TEST_TOLERANCE = 10.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Generate launch description with both frequency and tolerance set.""" + topic_configs = { + TEST_TOPIC: { + 'expected_frequency': TEST_FREQUENCY, + 'tolerance': TEST_TOLERANCE + } + } + + ros2_monitor_node = create_monitor_node( + namespace=MONITOR_NODE_NAMESPACE, + node_name=MONITOR_NODE_NAME, + topics=[], + topic_configs=topic_configs + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_param_test' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +@post_shutdown_test() +class TestTopicParametersPostShutdown(unittest.TestCase): + """Post-shutdown tests.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('shutdown_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_node_shutdown(self, proc_info): + """Test that the node shuts down correctly.""" + available_nodes = self.test_node.get_node_names() + self.assertNotIn(MONITOR_NODE_NAME, available_nodes) + assertExitCodes(proc_info, allowable_exit_codes=[0]) + + +class TestTopicParameters(unittest.TestCase): + """Tests for parameter-based topic configuration.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS2 and create test node.""" + rclpy.init() + cls.test_node = Node('topic_params_test_node', namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS2.""" + cls.test_node.destroy_node() + rclpy.shutdown() + + def test_topic_configured_via_parameters(self): + """Test that topic is monitored when configured via parameters.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + f'Expected at least 3 diagnostics for {TEST_TOPIC}, got {len(received_diagnostics)}' + ) + + # Verify valid frame rate exists + best_status = None + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + frame_rate = float(kv.value) + if frame_rate > 0: + best_status = status + break + except ValueError: + continue + if best_status: + break + + self.assertIsNotNone( + best_status, + 'Should have received diagnostics with valid frame_rate_node' + ) + + frame_rate_node = None + for kv in best_status.values: + if kv.key == 'frame_rate_node': + frame_rate_node = float(kv.value) + break + + self.assertIsNotNone(frame_rate_node) + tolerance = TEST_FREQUENCY * 0.5 + self.assertAlmostEqual( + frame_rate_node, TEST_FREQUENCY, delta=tolerance, + msg=f'Frame rate {frame_rate_node} not within {tolerance} of {TEST_FREQUENCY}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index 2864f97..3aca226 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -132,10 +132,18 @@ def tearDown(self): if hasattr(self, 'diagnostics_monitor'): # Clean up ROS components try: + timer = self.diagnostics_monitor._initial_params_timer + if timer is not None: + timer.cancel() + self.test_node.destroy_timer(timer) self.test_node.destroy_subscription(self.diagnostics_monitor.subscription) + self.test_node.destroy_subscription( + self.diagnostics_monitor.param_events_subscription) self.test_node.destroy_client(self.diagnostics_monitor.manage_topic_client) self.test_node.destroy_client( self.diagnostics_monitor.set_expected_frequency_client) + self.test_node.destroy_client(self.diagnostics_monitor.list_params_client) + self.test_node.destroy_client(self.diagnostics_monitor.get_params_client) except Exception: pass # Ignore cleanup errors From 596cc53868700eaa04e0231e3e10963deed74021 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 10:28:53 -0800 Subject: [PATCH 02/33] Add tests for nested YAML and don't specify the topics parameter every time Signed-off-by: Blake McHale --- greenwave_monitor/examples/example.launch.py | 6 +-- .../greenwave_monitor/test_utils.py | 17 +++---- .../test/parameters/test_param_dynamic.py | 42 +++++++++++++++-- .../test/parameters/test_param_freq_only.py | 4 -- .../test/parameters/test_param_new_topic.py | 7 +-- .../test/parameters/test_param_tol_only.py | 1 - .../test/parameters/test_param_yaml.py | 47 +++++++++++++++++-- .../test/parameters/test_topic_parameters.py | 3 -- 8 files changed, 94 insertions(+), 33 deletions(-) diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index 6f05710..c84b13a 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -53,9 +53,9 @@ def generate_launch_description(): parameters=[ { 'topics': { - '/imu_topic': {'expected_frequency': 100.0}, - '/image_topic': {'expected_frequency': 30.0}, - '/string_topic': {'expected_frequency': 1000.0} + '/imu_topic': {'expected_frequency': 100.0, 'tolerance': 5.0}, + '/image_topic': {'expected_frequency': 30.0, 'tolerance': 5.0}, + '/string_topic': {'expected_frequency': 1000.0, 'tolerance': 5.0} }, } ] diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 67d2d87..ac79bdd 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -73,14 +73,15 @@ def create_monitor_node(namespace: str = MONITOR_NODE_NAMESPACE, topics: List[str] = None, topic_configs: dict = None): """Create a greenwave_monitor node for testing.""" - if topics is None: - topics = ['/test_topic'] - - # Ensure topics list has at least one element (even if empty string) - if not topics: - topics = [''] - - params = {'topics': topics} + params = {} + + # Only add topics param if explicitly provided or no topic_configs + if topics is not None: + if not topics: + topics = [''] + params['topics'] = topics + elif not topic_configs: + params['topics'] = ['/test_topic'] if topic_configs: for topic, config in topic_configs.items(): diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 392b950..6b130de 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -43,6 +43,7 @@ TEST_TOPIC = '/dynamic_param_topic' +NEW_TOPIC = '/new_param_topic' TEST_FREQUENCY = 30.0 @@ -89,20 +90,23 @@ def make_tol_param(topic: str) -> str: def generate_test_description(): """Test dynamic parameter changes via ros2 param set.""" ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[TEST_TOPIC], # Topic exists but no expected frequency - topic_configs={} + topics=[TEST_TOPIC] # Topic exists but no expected frequency ) publisher = create_minimal_publisher( TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' ) + # Publisher for topic not initially monitored + new_topic_publisher = create_minimal_publisher( + NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' + ) + return ( launch.LaunchDescription([ ros2_monitor_node, publisher, + new_topic_publisher, launch_testing.actions.ReadyToTest() ]), {} ) @@ -194,6 +198,36 @@ def test_verify_params_with_get(self): msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' ) + def test_add_new_topic_via_param(self): + """Test that setting frequency param for unmonitored topic starts monitoring.""" + time.sleep(1.0) + + # Verify topic is not initially monitored + initial_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(initial_diagnostics), 0, + f'{NEW_TOPIC} should not be monitored initially' + ) + + # Set expected frequency for the new topic + freq_param = make_freq_param(NEW_TOPIC) + success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + time.sleep(2.0) + + # Verify topic is now monitored + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + f'{NEW_TOPIC} should be monitored after setting frequency param' + ) + if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index df1d89a..805708a 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -26,7 +26,6 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE ) import launch @@ -51,9 +50,6 @@ def generate_test_description(): } ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[], topic_configs=topic_configs ) diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 1875516..6d8ac15 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -64,12 +64,7 @@ def make_freq_param(topic: str) -> str: @pytest.mark.launch_test def generate_test_description(): """Test adding a new topic via ros2 param set.""" - ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[''], # Empty - no initial topics - topic_configs={} - ) + ros2_monitor_node = create_monitor_node() publisher = create_minimal_publisher( NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index ddc8f08..f99e7b3 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -48,7 +48,6 @@ def generate_test_description(): """Test with only tolerance specified (should not monitor).""" params = { - 'topics': [''], f'{TOPIC_PARAM_PREFIX}{TEST_TOPIC}{TOL_SUFFIX}': 15.0 } diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 6c1d7c5..08d070e 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -39,22 +39,28 @@ YAML_TOPIC = '/yaml_config_topic' +NESTED_TOPIC = '/nested_yaml_topic' TEST_FREQUENCY = 50.0 +NESTED_FREQUENCY = 25.0 TEST_TOLERANCE = 10.0 @pytest.mark.launch_test def generate_test_description(): """Test loading parameters from a YAML file.""" - # Write YAML manually with proper quoting (yaml.dump doesn't quote keys with dots) - # Use /** to apply to all nodes, or use full namespace path + # Write YAML manually - demonstrates both flat dotted keys and nested dict formats + # Use full namespace path for node parameters yaml_content = ( f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n' f' ros__parameters:\n' - f' topics:\n' - f" - ''\n" + f' # Flat dotted key format (requires quotes)\n' f' "topics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n' f' "topics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n' + f' # Nested dictionary format\n' + f' topics:\n' + f' {NESTED_TOPIC}:\n' + f' expected_frequency: {NESTED_FREQUENCY}\n' + f' tolerance: {TEST_TOLERANCE}\n' ) yaml_dir = tempfile.mkdtemp() @@ -75,10 +81,15 @@ def generate_test_description(): YAML_TOPIC, TEST_FREQUENCY, 'imu', '_yaml' ) + nested_publisher = create_minimal_publisher( + NESTED_TOPIC, NESTED_FREQUENCY, 'imu', '_nested_yaml' + ) + return ( launch.LaunchDescription([ ros2_monitor_node, publisher, + nested_publisher, launch_testing.actions.ReadyToTest() ]), {} ) @@ -127,6 +138,34 @@ def test_topic_configured_via_yaml(self): self.assertTrue(has_valid_rate, 'Should have valid frame rate from YAML config') + def test_nested_dict_topic_configured_via_yaml(self): + """Test that topic configured via nested YAML dict is monitored.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, NESTED_TOPIC, expected_count=3, timeout_sec=10.0 + ) + + self.assertGreaterEqual( + len(received_diagnostics), 3, + 'Expected diagnostics from nested YAML-configured topic' + ) + + has_valid_rate = False + for status in received_diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + has_valid_rate = True + break + except ValueError: + continue + if has_valid_rate: + break + + self.assertTrue(has_valid_rate, 'Should have valid frame rate from nested YAML config') + if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 4add93f..7c9c0cf 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -54,9 +54,6 @@ def generate_test_description(): } ros2_monitor_node = create_monitor_node( - namespace=MONITOR_NODE_NAMESPACE, - node_name=MONITOR_NODE_NAME, - topics=[], topic_configs=topic_configs ) From 2f319690435d86ebb38cbbf2d2baf5f22e1eaf0a Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 11:14:29 -0800 Subject: [PATCH 03/33] More common functions and use rclpy instead of subprocess for interfacing with parameters Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 83 ++++++++++++++ .../test/parameters/test_param_dynamic.py | 103 ++---------------- .../test/parameters/test_param_freq_only.py | 22 +--- .../test/parameters/test_param_new_topic.py | 28 +---- .../test/parameters/test_param_tol_only.py | 9 +- .../test/parameters/test_param_yaml.py | 41 ++----- .../test/parameters/test_topic_parameters.py | 2 +- 7 files changed, 119 insertions(+), 169 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index ac79bdd..1155357 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -29,6 +29,8 @@ ) from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency import launch_ros +from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue +from rcl_interfaces.srv import GetParameters, SetParameters import rclpy from rclpy.node import Node @@ -52,6 +54,87 @@ MONITOR_NODE_NAMESPACE = 'test_namespace' +def make_freq_param(topic: str) -> str: + """Build frequency parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' + + +def make_tol_param(topic: str) -> str: + """Build tolerance parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' + + +def set_parameter(test_node: Node, param_name: str, value: float, + node_name: str = MONITOR_NODE_NAME) -> bool: + """Set a parameter on the monitor node using rclpy service client.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + service_name = f'{full_node_name}/set_parameters' + + client = test_node.create_client(SetParameters, service_name) + if not client.wait_for_service(timeout_sec=5.0): + return False + + param = Parameter() + param.name = param_name + param.value = ParameterValue() + param.value.type = ParameterType.PARAMETER_DOUBLE + param.value.double_value = float(value) + + request = SetParameters.Request() + request.parameters = [param] + + future = client.call_async(request) + rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0) + + test_node.destroy_client(client) + + if future.result() is None: + return False + return all(r.successful for r in future.result().results) + + +def get_parameter(test_node: Node, param_name: str, + node_name: str = MONITOR_NODE_NAME) -> Tuple[bool, Optional[float]]: + """Get a parameter from the monitor node using rclpy service client.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + service_name = f'{full_node_name}/get_parameters' + + client = test_node.create_client(GetParameters, service_name) + if not client.wait_for_service(timeout_sec=5.0): + return False, None + + request = GetParameters.Request() + request.names = [param_name] + + future = client.call_async(request) + rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0) + + test_node.destroy_client(client) + + if future.result() is None or not future.result().values: + return False, None + + param_value = future.result().values[0] + if param_value.type == ParameterType.PARAMETER_DOUBLE: + return True, param_value.double_value + elif param_value.type == ParameterType.PARAMETER_INTEGER: + return True, float(param_value.integer_value) + return False, None + + +def has_valid_frame_rate(diagnostics: List[DiagnosticStatus]) -> bool: + """Check if any diagnostic has a valid (positive) frame_rate_node value.""" + for status in diagnostics: + for kv in status.values: + if kv.key == 'frame_rate_node': + try: + if float(kv.value) > 0: + return True + except ValueError: + continue + return False + + def create_minimal_publisher( topic: str, frequency_hz: float, message_type: str, id_suffix: str = ''): """Create a minimal publisher node with the given parameters.""" diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 6b130de..c625519 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -19,7 +19,6 @@ """Test: dynamic parameter changes via ros2 param set.""" -import subprocess import time import unittest @@ -27,13 +26,11 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE -) -from greenwave_monitor.ui_adaptor import ( - FREQ_SUFFIX, - TOL_SUFFIX, - TOPIC_PARAM_PREFIX, + get_parameter, + make_freq_param, + make_tol_param, + MONITOR_NODE_NAMESPACE, + set_parameter, ) import launch import launch_testing @@ -43,49 +40,9 @@ TEST_TOPIC = '/dynamic_param_topic' -NEW_TOPIC = '/new_param_topic' TEST_FREQUENCY = 30.0 -def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: - """Run ros2 param set command and return success status.""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' - cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] - try: - result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) - return result.returncode == 0 - except subprocess.TimeoutExpired: - return False - - -def run_ros2_param_get(node_name: str, param_name: str) -> tuple[bool, float | None]: - """Run ros2 param get command and return (success, value).""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' - cmd = ['ros2', 'param', 'get', full_node_name, param_name] - try: - result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) - if result.returncode != 0: - return False, None - # Parse output like "Double value is: 30.0" or "Integer value is: 30" - output = result.stdout.strip() - if 'value is:' in output: - value_str = output.split('value is:')[1].strip() - return True, float(value_str) - return False, None - except (subprocess.TimeoutExpired, ValueError): - return False, None - - -def make_freq_param(topic: str) -> str: - """Build frequency parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' - - -def make_tol_param(topic: str) -> str: - """Build tolerance parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' - - @pytest.mark.launch_test def generate_test_description(): """Test dynamic parameter changes via ros2 param set.""" @@ -97,16 +54,10 @@ def generate_test_description(): TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' ) - # Publisher for topic not initially monitored - new_topic_publisher = create_minimal_publisher( - NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' - ) - return ( launch.LaunchDescription([ ros2_monitor_node, publisher, - new_topic_publisher, launch_testing.actions.ReadyToTest() ]), {} ) @@ -132,7 +83,7 @@ def test_set_expected_frequency_via_param(self): time.sleep(2.0) freq_param = make_freq_param(TEST_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') time.sleep(1.0) @@ -151,7 +102,7 @@ def test_change_tolerance_via_param(self): time.sleep(1.0) tol_param = make_tol_param(TEST_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, 20.0) + success = set_parameter(self.test_node, tol_param, 20.0) self.assertTrue(success, f'Failed to set {tol_param}') time.sleep(0.5) @@ -175,59 +126,29 @@ def test_verify_params_with_get(self): expected_freq = 42.5 expected_tol = 15.0 - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, expected_freq) + success = set_parameter(self.test_node, freq_param, expected_freq) self.assertTrue(success, f'Failed to set {freq_param}') - success = run_ros2_param_set(MONITOR_NODE_NAME, tol_param, expected_tol) + success = set_parameter(self.test_node, tol_param, expected_tol) self.assertTrue(success, f'Failed to set {tol_param}') time.sleep(0.5) - # Verify with ros2 param get - success, actual_freq = run_ros2_param_get(MONITOR_NODE_NAME, freq_param) + # Verify with get_parameter + success, actual_freq = get_parameter(self.test_node, freq_param) self.assertTrue(success, f'Failed to get {freq_param}') self.assertAlmostEqual( actual_freq, expected_freq, places=1, msg=f'Frequency mismatch: expected {expected_freq}, got {actual_freq}' ) - success, actual_tol = run_ros2_param_get(MONITOR_NODE_NAME, tol_param) + success, actual_tol = get_parameter(self.test_node, tol_param) self.assertTrue(success, f'Failed to get {tol_param}') self.assertAlmostEqual( actual_tol, expected_tol, places=1, msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' ) - def test_add_new_topic_via_param(self): - """Test that setting frequency param for unmonitored topic starts monitoring.""" - time.sleep(1.0) - - # Verify topic is not initially monitored - initial_diagnostics = collect_diagnostics_for_topic( - self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0 - ) - self.assertEqual( - len(initial_diagnostics), 0, - f'{NEW_TOPIC} should not be monitored initially' - ) - - # Set expected frequency for the new topic - freq_param = make_freq_param(NEW_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) - self.assertTrue(success, f'Failed to set {freq_param}') - - time.sleep(2.0) - - # Verify topic is now monitored - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - f'{NEW_TOPIC} should be monitored after setting frequency param' - ) - if __name__ == '__main__': unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index 805708a..c770b92 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -26,7 +26,8 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAMESPACE + has_valid_frame_rate, + MONITOR_NODE_NAMESPACE, ) import launch import launch_testing @@ -93,21 +94,10 @@ def test_frequency_only_uses_default_tolerance(self): len(received_diagnostics), 3, f'Expected at least 3 diagnostics, got {len(received_diagnostics)}' ) - - has_valid_rate = False - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - has_valid_rate = True - break - except ValueError: - continue - if has_valid_rate: - break - - self.assertTrue(has_valid_rate, 'Should have valid frame rate with default tolerance') + self.assertTrue( + has_valid_frame_rate(received_diagnostics), + 'Should have valid frame rate with default tolerance' + ) if __name__ == '__main__': diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 6d8ac15..297a448 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -19,7 +19,6 @@ """Test: add new topic to monitoring via ros2 param set.""" -import subprocess import time import unittest @@ -27,12 +26,9 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE -) -from greenwave_monitor.ui_adaptor import ( - FREQ_SUFFIX, - TOPIC_PARAM_PREFIX, + make_freq_param, + MONITOR_NODE_NAMESPACE, + set_parameter, ) import launch import launch_testing @@ -45,22 +41,6 @@ TEST_FREQUENCY = 50.0 -def run_ros2_param_set(node_name: str, param_name: str, value: float) -> bool: - """Run ros2 param set command and return success status.""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' - cmd = ['ros2', 'param', 'set', full_node_name, param_name, str(value)] - try: - result = subprocess.run(cmd, capture_output=True, text=True, timeout=10.0) - return result.returncode == 0 - except subprocess.TimeoutExpired: - return False - - -def make_freq_param(topic: str) -> str: - """Build frequency parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' - - @pytest.mark.launch_test def generate_test_description(): """Test adding a new topic via ros2 param set.""" @@ -107,7 +87,7 @@ def test_add_new_topic_via_frequency_param(self): ) freq_param = make_freq_param(NEW_TOPIC) - success = run_ros2_param_set(MONITOR_NODE_NAME, freq_param, TEST_FREQUENCY) + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') time.sleep(2.0) diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index f99e7b3..4e87d9d 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -25,12 +25,9 @@ from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, + make_tol_param, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE -) -from greenwave_monitor.ui_adaptor import ( - TOL_SUFFIX, - TOPIC_PARAM_PREFIX, + MONITOR_NODE_NAMESPACE, ) import launch import launch_ros.actions @@ -48,7 +45,7 @@ def generate_test_description(): """Test with only tolerance specified (should not monitor).""" params = { - f'{TOPIC_PARAM_PREFIX}{TEST_TOPIC}{TOL_SUFFIX}': 15.0 + make_tol_param(TEST_TOPIC): 15.0 } ros2_monitor_node = launch_ros.actions.Node( diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 08d070e..b295d45 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -27,8 +27,9 @@ from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, + has_valid_frame_rate, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE + MONITOR_NODE_NAMESPACE, ) import launch import launch_ros.actions @@ -122,21 +123,10 @@ def test_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from YAML-configured topic' ) - - has_valid_rate = False - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - has_valid_rate = True - break - except ValueError: - continue - if has_valid_rate: - break - - self.assertTrue(has_valid_rate, 'Should have valid frame rate from YAML config') + self.assertTrue( + has_valid_frame_rate(received_diagnostics), + 'Should have valid frame rate from YAML config' + ) def test_nested_dict_topic_configured_via_yaml(self): """Test that topic configured via nested YAML dict is monitored.""" @@ -150,21 +140,10 @@ def test_nested_dict_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from nested YAML-configured topic' ) - - has_valid_rate = False - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - has_valid_rate = True - break - except ValueError: - continue - if has_valid_rate: - break - - self.assertTrue(has_valid_rate, 'Should have valid frame rate from nested YAML config') + self.assertTrue( + has_valid_frame_rate(received_diagnostics), + 'Should have valid frame rate from nested YAML config' + ) if __name__ == '__main__': diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 7c9c0cf..ce2fece 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -27,7 +27,7 @@ create_minimal_publisher, create_monitor_node, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE + MONITOR_NODE_NAMESPACE, ) import launch import launch_testing From e78aca32e37f23d6aaf3923eb0ea5d0dfb4b5bab Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 11:25:23 -0800 Subject: [PATCH 04/33] Fix cpplint Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 86183d9..b15da24 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -67,13 +67,11 @@ TopicParamInfo parse_topic_param_name(const std::string & param_name) const size_t tol_suffix_len = strlen(kTolSuffix); if (topic_and_field.length() > freq_suffix_len && - topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) - { + topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) { info.topic_name = topic_and_field.substr(0, topic_and_field.length() - freq_suffix_len); info.field = TopicParamField::kFrequency; } else if (topic_and_field.length() > tol_suffix_len && - topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) - { + topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) { info.topic_name = topic_and_field.substr(0, topic_and_field.length() - tol_suffix_len); info.field = TopicParamField::kTolerance; } From b891a6b8fb11a69f04b109e97b20dada04c7fc16 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 13:00:25 -0800 Subject: [PATCH 05/33] Fix lint Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 16 +++-- .../test/parameters/test_param_dynamic.py | 65 ++++++++++++------- 2 files changed, 52 insertions(+), 29 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index b15da24..20dd4ff 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -65,14 +65,18 @@ TopicParamInfo parse_topic_param_name(const std::string & param_name) const size_t freq_suffix_len = strlen(kFreqSuffix); const size_t tol_suffix_len = strlen(kTolSuffix); + const size_t len = topic_and_field.length(); - if (topic_and_field.length() > freq_suffix_len && - topic_and_field.rfind(kFreqSuffix) == topic_and_field.length() - freq_suffix_len) { - info.topic_name = topic_and_field.substr(0, topic_and_field.length() - freq_suffix_len); + bool is_freq = len > freq_suffix_len && + topic_and_field.rfind(kFreqSuffix) == len - freq_suffix_len; + bool is_tol = len > tol_suffix_len && + topic_and_field.rfind(kTolSuffix) == len - tol_suffix_len; + + if (is_freq) { + info.topic_name = topic_and_field.substr(0, len - freq_suffix_len); info.field = TopicParamField::kFrequency; - } else if (topic_and_field.length() > tol_suffix_len && - topic_and_field.rfind(kTolSuffix) == topic_and_field.length() - tol_suffix_len) { - info.topic_name = topic_and_field.substr(0, topic_and_field.length() - tol_suffix_len); + } else if (is_tol) { + info.topic_name = topic_and_field.substr(0, len - tol_suffix_len); info.field = TopicParamField::kTolerance; } diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index c625519..4739edb 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -41,6 +41,8 @@ TEST_TOPIC = '/dynamic_param_topic' TEST_FREQUENCY = 30.0 +TEST_TOLERANCE = 20.0 +NONEXISTENT_TOPIC = '/topic_that_does_not_exist' @pytest.mark.launch_test @@ -82,6 +84,15 @@ def test_set_expected_frequency_via_param(self): """Test setting expected frequency via ros2 param set.""" time.sleep(2.0) + # Verify topic is not monitored before setting frequency + initial_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(initial_diagnostics), 0, + f'{TEST_TOPIC} should not be monitored before setting frequency' + ) + freq_param = make_freq_param(TEST_TOPIC) success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') @@ -97,12 +108,20 @@ def test_set_expected_frequency_via_param(self): 'Expected diagnostics after setting frequency param' ) + # Verify parameter value + success, actual_freq = get_parameter(self.test_node, freq_param) + self.assertTrue(success, f'Failed to get {freq_param}') + self.assertAlmostEqual( + actual_freq, TEST_FREQUENCY, places=1, + msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' + ) + def test_change_tolerance_via_param(self): """Test changing tolerance via ros2 param set.""" time.sleep(1.0) tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter(self.test_node, tol_param, 20.0) + success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) self.assertTrue(success, f'Failed to set {tol_param}') time.sleep(0.5) @@ -116,37 +135,37 @@ def test_change_tolerance_via_param(self): 'Topic should still be monitored after tolerance change' ) - def test_verify_params_with_get(self): - """Test that ros2 param get returns the values we set.""" - time.sleep(1.0) + # Verify parameter value + success, actual_tol = get_parameter(self.test_node, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, TEST_TOLERANCE, places=1, + msg=f'Tolerance mismatch: expected {TEST_TOLERANCE}, got {actual_tol}' + ) - # Set specific values - freq_param = make_freq_param(TEST_TOPIC) - tol_param = make_tol_param(TEST_TOPIC) - expected_freq = 42.5 - expected_tol = 15.0 + def test_set_frequency_for_nonexistent_topic(self): + """Test setting expected frequency for a topic that does not exist.""" + time.sleep(1.0) - success = set_parameter(self.test_node, freq_param, expected_freq) + freq_param = make_freq_param(NONEXISTENT_TOPIC) + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') - success = set_parameter(self.test_node, tol_param, expected_tol) - self.assertTrue(success, f'Failed to set {tol_param}') - - time.sleep(0.5) - - # Verify with get_parameter + # Verify parameter was set success, actual_freq = get_parameter(self.test_node, freq_param) self.assertTrue(success, f'Failed to get {freq_param}') self.assertAlmostEqual( - actual_freq, expected_freq, places=1, - msg=f'Frequency mismatch: expected {expected_freq}, got {actual_freq}' + actual_freq, TEST_FREQUENCY, places=1, + msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' ) - success, actual_tol = get_parameter(self.test_node, tol_param) - self.assertTrue(success, f'Failed to get {tol_param}') - self.assertAlmostEqual( - actual_tol, expected_tol, places=1, - msg=f'Tolerance mismatch: expected {expected_tol}, got {actual_tol}' + # Topic should not appear in diagnostics since it doesn't exist + diagnostics = collect_diagnostics_for_topic( + self.test_node, NONEXISTENT_TOPIC, expected_count=1, timeout_sec=3.0 + ) + self.assertEqual( + len(diagnostics), 0, + f'{NONEXISTENT_TOPIC} should not appear in diagnostics' ) From eb488e384f39e62e4daebb7f33f33fc2443814d7 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 14:01:02 -0800 Subject: [PATCH 06/33] Fix tests Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 18 +--- .../include/greenwave_monitor.hpp | 6 ++ greenwave_monitor/src/greenwave_monitor.cpp | 15 ++++ .../test/parameters/test_param_dynamic.py | 88 ++++++++++++------- .../test/parameters/test_param_freq_only.py | 9 +- .../test/parameters/test_param_yaml.py | 16 ++-- .../test/parameters/test_topic_parameters.py | 29 ++---- 7 files changed, 100 insertions(+), 81 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 1155357..e9c4324 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -65,7 +65,8 @@ def make_tol_param(topic: str) -> str: def set_parameter(test_node: Node, param_name: str, value: float, - node_name: str = MONITOR_NODE_NAME) -> bool: + node_name: str = MONITOR_NODE_NAME, + timeout_sec: float = 10.0) -> bool: """Set a parameter on the monitor node using rclpy service client.""" full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' service_name = f'{full_node_name}/set_parameters' @@ -84,7 +85,7 @@ def set_parameter(test_node: Node, param_name: str, value: float, request.parameters = [param] future = client.call_async(request) - rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0) + rclpy.spin_until_future_complete(test_node, future, timeout_sec=timeout_sec) test_node.destroy_client(client) @@ -122,19 +123,6 @@ def get_parameter(test_node: Node, param_name: str, return False, None -def has_valid_frame_rate(diagnostics: List[DiagnosticStatus]) -> bool: - """Check if any diagnostic has a valid (positive) frame_rate_node value.""" - for status in diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - if float(kv.value) > 0: - return True - except ValueError: - continue - return False - - def create_minimal_publisher( topic: str, frequency_hz: float, message_type: str, id_suffix: str = ''): """Create a minimal publisher node with the given parameters.""" diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index c736639..496fe9e 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -109,4 +110,9 @@ class GreenwaveMonitor : public rclcpp::Node std::map pending_topic_configs_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // Mutex protecting message_diagnostics_, subscriptions_, and pending_topic_configs_ + mutable std::mutex state_mutex_; + // Flag to skip parameter callback when updating params internally (avoids redundant work) + bool updating_params_internally_ = false; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 20dd4ff..0e4a005 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -192,11 +192,13 @@ void GreenwaveMonitor::topic_callback( const std::string & topic, const std::string & type) { auto msg_timestamp = GetTimestampFromSerializedMessage(msg, type); + std::lock_guard lock(state_mutex_); message_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); } void GreenwaveMonitor::timer_callback() { + std::lock_guard lock(state_mutex_); RCLCPP_INFO(this->get_logger(), "===================================================="); if (message_diagnostics_.empty()) { RCLCPP_INFO(this->get_logger(), "No topics to monitor"); @@ -217,6 +219,7 @@ void GreenwaveMonitor::handle_manage_topic( const std::shared_ptr request, std::shared_ptr response) { + std::lock_guard lock(state_mutex_); if (request->add_topic) { response->success = add_topic(request->topic_name, response->message); } else { @@ -228,6 +231,7 @@ void GreenwaveMonitor::handle_set_expected_frequency( const std::shared_ptr request, std::shared_ptr response) { + std::lock_guard lock(state_mutex_); auto it = message_diagnostics_.find(request->topic_name); if (it == message_diagnostics_.end()) { @@ -300,8 +304,10 @@ bool GreenwaveMonitor::set_topic_expected_frequency( // Sync parameters with the new values if (update_parameters) { + updating_params_internally_ = true; declare_or_set_parameter(make_freq_param_name(topic_name), expected_hz); declare_or_set_parameter(make_tol_param_name(topic_name), tolerance_percent); + updating_params_internally_ = false; } message = "Successfully set expected frequency for topic '" + @@ -316,6 +322,13 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( rcl_interfaces::msg::SetParametersResult result; result.successful = true; + // Skip if updating from within the node (avoids redundant work and deadlock) + if (updating_params_internally_) { + return result; + } + + std::lock_guard lock(state_mutex_); + for (const auto & param : parameters) { auto info = parse_topic_param_name(param.get_name()); if (info.field == TopicParamField::kNone || info.topic_name.empty()) { @@ -403,6 +416,8 @@ void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_ void GreenwaveMonitor::load_topic_parameters_from_overrides() { + std::lock_guard lock(state_mutex_); + // Parameters are automatically declared from overrides due to NodeOptions setting. // List all parameters and filter by prefix manually (list_parameters prefix matching // can be unreliable with deeply nested parameter names). diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 4739edb..7b7f2a0 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -48,9 +48,7 @@ @pytest.mark.launch_test def generate_test_description(): """Test dynamic parameter changes via ros2 param set.""" - ros2_monitor_node = create_monitor_node( - topics=[TEST_TOPIC] # Topic exists but no expected frequency - ) + ros2_monitor_node = create_monitor_node() publisher = create_minimal_publisher( TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' @@ -80,35 +78,46 @@ def tearDownClass(cls): cls.test_node.destroy_node() rclpy.shutdown() - def test_set_expected_frequency_via_param(self): - """Test setting expected frequency via ros2 param set.""" + def test_set_parameters(self): + """Test setting frequency and tolerance parameters in sequence.""" time.sleep(2.0) - # Verify topic is not monitored before setting frequency + freq_param = make_freq_param(TEST_TOPIC) + tol_param = make_tol_param(TEST_TOPIC) + + # 1. Verify topic is not monitored initially initial_diagnostics = collect_diagnostics_for_topic( self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 ) self.assertEqual( len(initial_diagnostics), 0, - f'{TEST_TOPIC} should not be monitored before setting frequency' + f'{TEST_TOPIC} should not be monitored initially' ) - freq_param = make_freq_param(TEST_TOPIC) - success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) - self.assertTrue(success, f'Failed to set {freq_param}') - - time.sleep(1.0) + # 2. Set tolerance before frequency - topic should remain unmonitored + success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) + self.assertTrue(success, f'Failed to set {tol_param}') - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + success, actual_tol = get_parameter(self.test_node, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, TEST_TOLERANCE, places=1, + msg=f'Tolerance mismatch: expected {TEST_TOLERANCE}, got {actual_tol}' ) - self.assertGreaterEqual( - len(received_diagnostics), 3, - 'Expected diagnostics after setting frequency param' + time.sleep(1.0) + diagnostics_after_tol = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 + ) + self.assertEqual( + len(diagnostics_after_tol), 0, + f'{TEST_TOPIC} should remain unmonitored after setting only tolerance' ) - # Verify parameter value + # 3. Set frequency - topic should become monitored + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + success, actual_freq = get_parameter(self.test_node, freq_param) self.assertTrue(success, f'Failed to get {freq_param}') self.assertAlmostEqual( @@ -116,31 +125,42 @@ def test_set_expected_frequency_via_param(self): msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' ) - def test_change_tolerance_via_param(self): - """Test changing tolerance via ros2 param set.""" time.sleep(1.0) + diagnostics_after_freq = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics_after_freq), 3, + 'Expected diagnostics after setting frequency param' + ) - tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) - self.assertTrue(success, f'Failed to set {tol_param}') + # 4. Set tolerance to 0.0 - should cause diagnostics to show error + success = set_parameter(self.test_node, tol_param, 0.0) + self.assertTrue(success, f'Failed to set {tol_param} to 0.0') - time.sleep(0.5) + success, actual_tol = get_parameter(self.test_node, tol_param) + self.assertTrue(success, f'Failed to get {tol_param}') + self.assertAlmostEqual( + actual_tol, 0.0, places=1, + msg=f'Tolerance mismatch: expected 0.0, got {actual_tol}' + ) - received_diagnostics = collect_diagnostics_for_topic( + time.sleep(2.0) + diagnostics_with_zero_tol = collect_diagnostics_for_topic( self.test_node, TEST_TOPIC, expected_count=2, timeout_sec=5.0 ) - self.assertGreaterEqual( - len(received_diagnostics), 2, - 'Topic should still be monitored after tolerance change' + len(diagnostics_with_zero_tol), 2, + 'Topic should still be monitored with zero tolerance' ) - # Verify parameter value - success, actual_tol = get_parameter(self.test_node, tol_param) - self.assertTrue(success, f'Failed to get {tol_param}') - self.assertAlmostEqual( - actual_tol, TEST_TOLERANCE, places=1, - msg=f'Tolerance mismatch: expected {TEST_TOLERANCE}, got {actual_tol}' + # Check that at least one diagnostic has ERROR level (frequency outside 0% tolerance) + has_error = any( + d.level != 0 for d in diagnostics_with_zero_tol + ) + self.assertTrue( + has_error, + 'Expected ERROR diagnostics with 0% tolerance' ) def test_set_frequency_for_nonexistent_topic(self): diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index c770b92..beb22cc 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -26,7 +26,7 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, - has_valid_frame_rate, + find_best_diagnostic, MONITOR_NODE_NAMESPACE, ) import launch @@ -94,8 +94,11 @@ def test_frequency_only_uses_default_tolerance(self): len(received_diagnostics), 3, f'Expected at least 3 diagnostics, got {len(received_diagnostics)}' ) - self.assertTrue( - has_valid_frame_rate(received_diagnostics), + best_status, _ = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, 'Should have valid frame rate with default tolerance' ) diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index b295d45..4c3d47d 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -27,7 +27,7 @@ from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, - has_valid_frame_rate, + find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, ) @@ -123,8 +123,11 @@ def test_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from YAML-configured topic' ) - self.assertTrue( - has_valid_frame_rate(received_diagnostics), + best_status, _ = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, 'Should have valid frame rate from YAML config' ) @@ -140,8 +143,11 @@ def test_nested_dict_topic_configured_via_yaml(self): len(received_diagnostics), 3, 'Expected diagnostics from nested YAML-configured topic' ) - self.assertTrue( - has_valid_frame_rate(received_diagnostics), + best_status, _ = find_best_diagnostic( + received_diagnostics, NESTED_FREQUENCY, 'imu' + ) + self.assertIsNotNone( + best_status, 'Should have valid frame rate from nested YAML config' ) diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index ce2fece..5b1e4a6 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -26,6 +26,7 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, + find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, ) @@ -120,34 +121,14 @@ def test_topic_configured_via_parameters(self): len(received_diagnostics), 3, f'Expected at least 3 diagnostics for {TEST_TOPIC}, got {len(received_diagnostics)}' ) - - # Verify valid frame rate exists - best_status = None - for status in received_diagnostics: - for kv in status.values: - if kv.key == 'frame_rate_node': - try: - frame_rate = float(kv.value) - if frame_rate > 0: - best_status = status - break - except ValueError: - continue - if best_status: - break - + best_status, best_values = find_best_diagnostic( + received_diagnostics, TEST_FREQUENCY, 'imu' + ) self.assertIsNotNone( best_status, 'Should have received diagnostics with valid frame_rate_node' ) - - frame_rate_node = None - for kv in best_status.values: - if kv.key == 'frame_rate_node': - frame_rate_node = float(kv.value) - break - - self.assertIsNotNone(frame_rate_node) + frame_rate_node = best_values[0] tolerance = TEST_FREQUENCY * 0.5 self.assertAlmostEqual( frame_rate_node, TEST_FREQUENCY, delta=tolerance, From 4fdc70cc90168a91a6a71baef16ef1e143571c10 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 15:16:02 -0800 Subject: [PATCH 07/33] Don't maintain pending_topic_configs_ Signed-off-by: Blake McHale --- .../include/greenwave_monitor.hpp | 6 +- greenwave_monitor/src/greenwave_monitor.cpp | 87 ++++++++----------- 2 files changed, 38 insertions(+), 55 deletions(-) diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 496fe9e..e28d3f6 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -76,7 +75,7 @@ class GreenwaveMonitor : public rclcpp::Node rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & parameters); - void apply_topic_config_if_complete(const std::string & topic_name); + void apply_topic_config(const std::string & topic_name, const TopicConfig & incoming); void load_topic_parameters_from_overrides(); @@ -108,11 +107,8 @@ class GreenwaveMonitor : public rclcpp::Node rclcpp::Service::SharedPtr set_expected_frequency_service_; - std::map pending_topic_configs_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - // Mutex protecting message_diagnostics_, subscriptions_, and pending_topic_configs_ - mutable std::mutex state_mutex_; // Flag to skip parameter callback when updating params internally (avoids redundant work) bool updating_params_internally_ = false; }; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 0e4a005..7716826 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -192,13 +192,11 @@ void GreenwaveMonitor::topic_callback( const std::string & topic, const std::string & type) { auto msg_timestamp = GetTimestampFromSerializedMessage(msg, type); - std::lock_guard lock(state_mutex_); message_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count()); } void GreenwaveMonitor::timer_callback() { - std::lock_guard lock(state_mutex_); RCLCPP_INFO(this->get_logger(), "===================================================="); if (message_diagnostics_.empty()) { RCLCPP_INFO(this->get_logger(), "No topics to monitor"); @@ -219,7 +217,6 @@ void GreenwaveMonitor::handle_manage_topic( const std::shared_ptr request, std::shared_ptr response) { - std::lock_guard lock(state_mutex_); if (request->add_topic) { response->success = add_topic(request->topic_name, response->message); } else { @@ -231,7 +228,6 @@ void GreenwaveMonitor::handle_set_expected_frequency( const std::shared_ptr request, std::shared_ptr response) { - std::lock_guard lock(state_mutex_); auto it = message_diagnostics_.find(request->topic_name); if (it == message_diagnostics_.end()) { @@ -327,7 +323,8 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( return result; } - std::lock_guard lock(state_mutex_); + // Build a local map of incoming topic configs from this callback + std::map incoming_configs; for (const auto & param : parameters) { auto info = parse_topic_param_name(param.get_name()); @@ -345,7 +342,7 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( } double value = value_opt.value(); - TopicConfig & config = pending_topic_configs_[info.topic_name]; + TopicConfig & config = incoming_configs[info.topic_name]; if (info.field == TopicParamField::kFrequency) { config.expected_frequency = value; @@ -357,38 +354,35 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( this->get_logger(), "Parameter set: %s for topic '%s' = %.2f %s", get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); + } - apply_topic_config_if_complete(info.topic_name); + // Apply configs for each topic affected by this parameter change + for (const auto & [topic_name, incoming] : incoming_configs) { + apply_topic_config(topic_name, incoming); } return result; } -void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_name) +void GreenwaveMonitor::apply_topic_config( + const std::string & topic_name, const TopicConfig & incoming) { - auto it = pending_topic_configs_.find(topic_name); - if (it == pending_topic_configs_.end()) { - return; - } - - const TopicConfig & config = it->second; - - // Get expected frequency from pending config or existing parameter + // Get expected frequency: prefer incoming, fall back to existing parameter double expected_freq = 0.0; - if (config.expected_frequency.has_value()) { - expected_freq = config.expected_frequency.value(); + if (incoming.expected_frequency.has_value()) { + expected_freq = incoming.expected_frequency.value(); } else { auto freq_opt = get_numeric_parameter(make_freq_param_name(topic_name)); if (freq_opt.has_value()) { expected_freq = freq_opt.value(); } else { - // No frequency available, nothing to do + // No frequency available yet, nothing to do return; } } - // Get tolerance from pending config, existing parameter, or default - double tolerance = config.tolerance.value_or( + // Get tolerance: prefer incoming, then existing parameter, then default + double tolerance = incoming.tolerance.value_or( get_numeric_parameter(make_tol_param_name(topic_name)).value_or(kDefaultTolerancePercent) ); @@ -410,20 +404,19 @@ void GreenwaveMonitor::apply_topic_config_if_complete(const std::string & topic_ "Use manage_topic service to add the topic first.", topic_name.c_str(), message.c_str()); } - - pending_topic_configs_.erase(it); } void GreenwaveMonitor::load_topic_parameters_from_overrides() { - std::lock_guard lock(state_mutex_); - // Parameters are automatically declared from overrides due to NodeOptions setting. // List all parameters and filter by prefix manually (list_parameters prefix matching // can be unreliable with deeply nested parameter names). auto all_params = this->list_parameters( {}, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); + // Build a local map of topic configs from startup parameters + std::map startup_configs; + for (const auto & name : all_params.names) { auto info = parse_topic_param_name(name); if (info.field == TopicParamField::kNone || info.topic_name.empty()) { @@ -436,7 +429,7 @@ void GreenwaveMonitor::load_topic_parameters_from_overrides() } double value = value_opt.value(); - TopicConfig & config = pending_topic_configs_[info.topic_name]; + TopicConfig & config = startup_configs[info.topic_name]; if (info.field == TopicParamField::kFrequency) { config.expected_frequency = value; @@ -450,32 +443,26 @@ void GreenwaveMonitor::load_topic_parameters_from_overrides() get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); } - // Apply all complete configs - at startup we can add topics if needed - std::vector topics_to_apply; - for (const auto & [topic, config] : pending_topic_configs_) { + // Apply all configs that have frequency set + for (const auto & [topic, config] : startup_configs) { if (config.expected_frequency.has_value()) { - topics_to_apply.push_back(topic); - } - } - for (const auto & topic : topics_to_apply) { - const TopicConfig & config = pending_topic_configs_[topic]; - double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); - - std::string message; - bool success = set_topic_expected_frequency( - topic, - config.expected_frequency.value(), - tolerance, - true, // add topic if missing - safe at startup - message, - false); // don't update parameters - - if (success) { - RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); - } else { - RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); + double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); + + std::string message; + bool success = set_topic_expected_frequency( + topic, + config.expected_frequency.value(), + tolerance, + true, // add topic if missing - safe at startup + message, + false); // don't update parameters + + if (success) { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } else { + RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); + } } - pending_topic_configs_.erase(topic); } } From f715325f0cbfc09a010823030d8b70fb6d162be2 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 15:50:06 -0800 Subject: [PATCH 08/33] Use same base class for RosNodeTestCase Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 36 +++++++++++++++++++ .../test/parameters/test_param_dynamic.py | 19 ++-------- .../test/parameters/test_param_freq_only.py | 19 ++-------- .../test/parameters/test_param_new_topic.py | 19 ++-------- .../test/parameters/test_param_tol_only.py | 18 ++-------- .../test/parameters/test_param_yaml.py | 18 ++-------- .../test/parameters/test_topic_parameters.py | 31 ++++------------ 7 files changed, 57 insertions(+), 103 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index e9c4324..20ec372 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -17,9 +17,11 @@ # # SPDX-License-Identifier: Apache-2.0 +from abc import ABC import math import time from typing import List, Optional, Tuple +import unittest from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from greenwave_monitor.ui_adaptor import ( @@ -367,3 +369,37 @@ def create_service_clients(node: Node, namespace: str = MONITOR_NODE_NAMESPACE, ) return manage_topic_client, set_frequency_client + + +class RosNodeTestCase(unittest.TestCase, ABC): + """ + Abstract base class for ROS 2 launch tests that need a test node. + + Subclasses must define the TEST_NODE_NAME class attribute to specify + the unique name for the test node. + + Example: + class TestMyFeature(RosNodeTestCase): + TEST_NODE_NAME = 'my_feature_test_node' + + def test_something(self): + # self.test_node is available + ... + """ + + TEST_NODE_NAME: str = None + + @classmethod + def setUpClass(cls): + """Initialize ROS 2 and create test node.""" + if cls.TEST_NODE_NAME is None: + raise ValueError( + f'{cls.__name__} must define TEST_NODE_NAME class attribute') + rclpy.init() + cls.test_node = Node(cls.TEST_NODE_NAME, namespace=MONITOR_NODE_NAMESPACE) + + @classmethod + def tearDownClass(cls): + """Clean up ROS 2.""" + cls.test_node.destroy_node() + rclpy.shutdown() diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 7b7f2a0..891fa17 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -20,7 +20,6 @@ """Test: dynamic parameter changes via ros2 param set.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -29,14 +28,12 @@ get_parameter, make_freq_param, make_tol_param, - MONITOR_NODE_NAMESPACE, + RosNodeTestCase, set_parameter, ) import launch import launch_testing import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/dynamic_param_topic' @@ -63,20 +60,10 @@ def generate_test_description(): ) -class TestDynamicParameterChanges(unittest.TestCase): +class TestDynamicParameterChanges(RosNodeTestCase): """Test changing parameters dynamically via ros2 param set.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('dynamic_param_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'dynamic_param_test_node' def test_set_parameters(self): """Test setting frequency and tolerance parameters in sequence.""" diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index beb22cc..0744c1b 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -20,20 +20,17 @@ """Test: only expected_frequency specified, tolerance defaults to 5%.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, find_best_diagnostic, - MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_testing import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/freq_only_topic' @@ -67,20 +64,10 @@ def generate_test_description(): ) -class TestFrequencyOnlyParameter(unittest.TestCase): +class TestFrequencyOnlyParameter(RosNodeTestCase): """Test that only specifying frequency works (tolerance defaults).""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('freq_only_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'freq_only_test_node' def test_frequency_only_uses_default_tolerance(self): """Test that specifying only frequency uses default tolerance.""" diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 297a448..b5a8233 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -20,21 +20,18 @@ """Test: add new topic to monitoring via ros2 param set.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, make_freq_param, - MONITOR_NODE_NAMESPACE, + RosNodeTestCase, set_parameter, ) import launch import launch_testing import pytest -import rclpy -from rclpy.node import Node NEW_TOPIC = '/new_dynamic_topic' @@ -59,20 +56,10 @@ def generate_test_description(): ) -class TestAddNewTopicViaParam(unittest.TestCase): +class TestAddNewTopicViaParam(RosNodeTestCase): """Test adding a new topic to monitoring via ros2 param set.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('new_topic_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'new_topic_test_node' def test_add_new_topic_via_frequency_param(self): """Test that setting frequency param for new topic starts monitoring.""" diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index 4e87d9d..8060890 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -20,7 +20,6 @@ """Test: only tolerance specified - should NOT start monitoring.""" import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -28,13 +27,12 @@ make_tol_param, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_ros.actions import launch_testing import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/tol_only_topic' @@ -70,20 +68,10 @@ def generate_test_description(): ) -class TestToleranceOnlyParameter(unittest.TestCase): +class TestToleranceOnlyParameter(RosNodeTestCase): """Test that only specifying tolerance does NOT start monitoring.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('tol_only_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'tol_only_test_node' def test_tolerance_only_does_not_monitor(self): """Test that specifying only tolerance does not start monitoring.""" diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 4c3d47d..630895e 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -22,7 +22,6 @@ import os import tempfile import time -import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -30,13 +29,12 @@ find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_ros.actions import launch_testing import pytest -import rclpy -from rclpy.node import Node YAML_TOPIC = '/yaml_config_topic' @@ -96,20 +94,10 @@ def generate_test_description(): ) -class TestYamlParameterFile(unittest.TestCase): +class TestYamlParameterFile(RosNodeTestCase): """Test loading topic configuration from YAML parameter file.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('yaml_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'yaml_test_node' def test_topic_configured_via_yaml(self): """Test that topic is monitored when configured via YAML file.""" diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 5b1e4a6..0995bec 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -29,6 +29,7 @@ find_best_diagnostic, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, + RosNodeTestCase, ) import launch import launch_testing @@ -72,20 +73,10 @@ def generate_test_description(): @post_shutdown_test() -class TestTopicParametersPostShutdown(unittest.TestCase): +class TestTopicParametersPostShutdown(RosNodeTestCase): """Post-shutdown tests.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('shutdown_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'shutdown_test_node' def test_node_shutdown(self, proc_info): """Test that the node shuts down correctly.""" @@ -94,20 +85,10 @@ def test_node_shutdown(self, proc_info): assertExitCodes(proc_info, allowable_exit_codes=[0]) -class TestTopicParameters(unittest.TestCase): +class TestTopicParameters(RosNodeTestCase): """Tests for parameter-based topic configuration.""" - @classmethod - def setUpClass(cls): - """Initialize ROS2 and create test node.""" - rclpy.init() - cls.test_node = Node('topic_params_test_node', namespace=MONITOR_NODE_NAMESPACE) - - @classmethod - def tearDownClass(cls): - """Clean up ROS2.""" - cls.test_node.destroy_node() - rclpy.shutdown() + TEST_NODE_NAME = 'topic_params_test_node' def test_topic_configured_via_parameters(self): """Test that topic is monitored when configured via parameters.""" @@ -129,7 +110,7 @@ def test_topic_configured_via_parameters(self): 'Should have received diagnostics with valid frame_rate_node' ) frame_rate_node = best_values[0] - tolerance = TEST_FREQUENCY * 0.5 + tolerance = TEST_FREQUENCY * TEST_TOLERANCE / 100.0 self.assertAlmostEqual( frame_rate_node, TEST_FREQUENCY, delta=tolerance, msg=f'Frame rate {frame_rate_node} not within {tolerance} of {TEST_FREQUENCY}' From 31e91480d1a3eab14fb9811d018c1d0342cfa5ef Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 15:59:00 -0800 Subject: [PATCH 09/33] Update copyrights Signed-off-by: Blake McHale --- greenwave_monitor/examples/example.launch.py | 2 +- greenwave_monitor/greenwave_monitor/test_utils.py | 2 +- greenwave_monitor/greenwave_monitor/ui_adaptor.py | 2 +- greenwave_monitor/include/greenwave_monitor.hpp | 2 +- greenwave_monitor/src/greenwave_monitor.cpp | 2 +- greenwave_monitor/test/parameters/test_param_dynamic.py | 2 +- greenwave_monitor/test/parameters/test_param_freq_only.py | 2 +- greenwave_monitor/test/parameters/test_param_new_topic.py | 2 +- greenwave_monitor/test/parameters/test_param_tol_only.py | 2 +- greenwave_monitor/test/parameters/test_param_yaml.py | 2 +- greenwave_monitor/test/parameters/test_topic_parameters.py | 2 +- 11 files changed, 11 insertions(+), 11 deletions(-) diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index c84b13a..92bb6d2 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2025, NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 20ec372..e351908 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2024-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 2e7ba8d..09f923c 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index e28d3f6..ce854fb 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 7716826..5c202fc 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 891fa17..74565ff 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index 0744c1b..7e6751e 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index b5a8233..2a8c0fe 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index 8060890..e05ebb8 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 630895e..3cf9c51 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index 0995bec..d3f210b 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From 88ddcd24ddf95341d7a2e1bf40307a255ccea3c2 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 16:31:13 -0800 Subject: [PATCH 10/33] More tests and linting Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 8 -- .../test/parameters/test_param_dynamic.py | 56 +++++++- .../test/parameters/test_param_freq_only.py | 1 + .../parameters/test_param_multiple_topics.py | 124 ++++++++++++++++++ .../test/parameters/test_param_new_topic.py | 1 + .../test/parameters/test_param_tol_only.py | 1 + .../test/parameters/test_param_yaml.py | 1 + .../test/parameters/test_topic_parameters.py | 3 - 8 files changed, 183 insertions(+), 12 deletions(-) create mode 100644 greenwave_monitor/test/parameters/test_param_multiple_topics.py diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index e351908..dc3c274 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -377,14 +377,6 @@ class RosNodeTestCase(unittest.TestCase, ABC): Subclasses must define the TEST_NODE_NAME class attribute to specify the unique name for the test node. - - Example: - class TestMyFeature(RosNodeTestCase): - TEST_NODE_NAME = 'my_feature_test_node' - - def test_something(self): - # self.test_node is available - ... """ TEST_NODE_NAME: str = None diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 74565ff..6e157c5 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -20,6 +20,7 @@ """Test: dynamic parameter changes via ros2 param set.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, @@ -143,13 +144,66 @@ def test_set_parameters(self): # Check that at least one diagnostic has ERROR level (frequency outside 0% tolerance) has_error = any( - d.level != 0 for d in diagnostics_with_zero_tol + ord(d.level) != 0 for d in diagnostics_with_zero_tol ) self.assertTrue( has_error, 'Expected ERROR diagnostics with 0% tolerance' ) + # Reset tolerance to 10% - should no longer error + success = set_parameter(self.test_node, tol_param, 10.0) + self.assertTrue(success, f'Failed to reset {tol_param}') + + # Wait for diagnostics to stabilize after tolerance change + time.sleep(3.0) + diagnostics_after_reset = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics_after_reset), 3, + 'Expected diagnostics after resetting tolerance' + ) + + # Verify most recent diagnostic is OK after resetting tolerance + last_diagnostic = diagnostics_after_reset[-1] + self.assertEqual( + ord(last_diagnostic.level), 0, + 'Expected OK diagnostic after resetting tolerance to 10%' + ) + + # 5. Update expected frequency to mismatched value - should cause error + # Publisher is still at 30 Hz, tolerance is 10%, but we set expected to 1 Hz + mismatched_frequency = 1.0 + success = set_parameter(self.test_node, freq_param, mismatched_frequency) + self.assertTrue(success, f'Failed to update {freq_param}') + + success, actual_freq = get_parameter(self.test_node, freq_param) + self.assertTrue(success, f'Failed to get updated {freq_param}') + self.assertAlmostEqual( + actual_freq, mismatched_frequency, places=1, + msg=f'Frequency mismatch: expected {mismatched_frequency}, got {actual_freq}' + ) + + time.sleep(2.0) + diagnostics_mismatched = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics_mismatched), 3, + 'Should still receive diagnostics after frequency update' + ) + + # Verify diagnostics show error due to frequency mismatch + has_error = any( + ord(d.level) != 0 for d in diagnostics_mismatched + ) + self.assertTrue( + has_error, + 'Expected ERROR diagnostics when actual frequency (30 Hz) ' + 'does not match expected (1 Hz)' + ) + def test_set_frequency_for_nonexistent_topic(self): """Test setting expected frequency for a topic that does not exist.""" time.sleep(1.0) diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py index 7e6751e..578f427 100644 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ b/greenwave_monitor/test/parameters/test_param_freq_only.py @@ -20,6 +20,7 @@ """Test: only expected_frequency specified, tolerance defaults to 5%.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_param_multiple_topics.py b/greenwave_monitor/test/parameters/test_param_multiple_topics.py new file mode 100644 index 0000000..d9a93a0 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_multiple_topics.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: multiple topics configured via parameters at startup.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + create_monitor_node, + find_best_diagnostic, + RosNodeTestCase, +) +import launch +import launch_testing +import pytest + + +TOPIC_1 = '/multi_topic_1' +TOPIC_2 = '/multi_topic_2' +TOPIC_3 = '/multi_topic_3' +FREQUENCY_1 = 10.0 +FREQUENCY_2 = 25.0 +FREQUENCY_3 = 50.0 +TOLERANCE = 20.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test multiple topics configured via parameters.""" + topic_configs = { + TOPIC_1: { + 'expected_frequency': FREQUENCY_1, + 'tolerance': TOLERANCE + }, + TOPIC_2: { + 'expected_frequency': FREQUENCY_2, + 'tolerance': TOLERANCE + }, + TOPIC_3: { + 'expected_frequency': FREQUENCY_3, + 'tolerance': TOLERANCE + } + } + + ros2_monitor_node = create_monitor_node(topic_configs=topic_configs) + + publisher_1 = create_minimal_publisher(TOPIC_1, FREQUENCY_1, 'imu', '_multi_1') + publisher_2 = create_minimal_publisher(TOPIC_2, FREQUENCY_2, 'imu', '_multi_2') + publisher_3 = create_minimal_publisher(TOPIC_3, FREQUENCY_3, 'imu', '_multi_3') + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher_1, + publisher_2, + publisher_3, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestMultipleTopicsViaParameters(RosNodeTestCase): + """Test that multiple topics can be configured via parameters.""" + + TEST_NODE_NAME = 'multiple_topics_test_node' + + def test_all_topics_monitored(self): + """Test that all configured topics are monitored.""" + time.sleep(2.0) + + topics_to_check = [ + (TOPIC_1, FREQUENCY_1), + (TOPIC_2, FREQUENCY_2), + (TOPIC_3, FREQUENCY_3), + ] + + for topic, expected_freq in topics_to_check: + with self.subTest(topic=topic): + diagnostics = collect_diagnostics_for_topic( + self.test_node, topic, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics), 3, + f'Expected at least 3 diagnostics for {topic}' + ) + + best_status, best_values = find_best_diagnostic( + diagnostics, expected_freq, 'imu' + ) + self.assertIsNotNone( + best_status, + f'Should have valid diagnostics for {topic}' + ) + + frame_rate = best_values[0] + tolerance_hz = expected_freq * TOLERANCE / 100.0 + self.assertAlmostEqual( + frame_rate, expected_freq, delta=tolerance_hz, + msg=f'{topic}: frame rate {frame_rate} not within ' + f'{tolerance_hz} of expected {expected_freq}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 2a8c0fe..7a0048b 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -20,6 +20,7 @@ """Test: add new topic to monitoring via ros2 param set.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index e05ebb8..a90a634 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -20,6 +20,7 @@ """Test: only tolerance specified - should NOT start monitoring.""" import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 3cf9c51..3a7c049 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -22,6 +22,7 @@ import os import tempfile import time +import unittest from greenwave_monitor.test_utils import ( collect_diagnostics_for_topic, diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py index d3f210b..4901635 100644 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ b/greenwave_monitor/test/parameters/test_topic_parameters.py @@ -28,7 +28,6 @@ create_monitor_node, find_best_diagnostic, MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE, RosNodeTestCase, ) import launch @@ -36,8 +35,6 @@ from launch_testing import post_shutdown_test from launch_testing.asserts import assertExitCodes import pytest -import rclpy -from rclpy.node import Node TEST_TOPIC = '/param_test_topic' From ad2aacb6fd01860db384111f756da42503ed87d3 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 2 Jan 2026 16:49:31 -0800 Subject: [PATCH 11/33] Cover when topics is specified alongside its subfields Signed-off-by: Blake McHale --- .../parameters/test_param_multiple_topics.py | 63 ++++++++++++++++++- 1 file changed, 62 insertions(+), 1 deletion(-) diff --git a/greenwave_monitor/test/parameters/test_param_multiple_topics.py b/greenwave_monitor/test/parameters/test_param_multiple_topics.py index d9a93a0..b351b33 100644 --- a/greenwave_monitor/test/parameters/test_param_multiple_topics.py +++ b/greenwave_monitor/test/parameters/test_param_multiple_topics.py @@ -34,6 +34,7 @@ import pytest +# Topics with expected frequencies configured TOPIC_1 = '/multi_topic_1' TOPIC_2 = '/multi_topic_2' TOPIC_3 = '/multi_topic_3' @@ -42,10 +43,16 @@ FREQUENCY_3 = 50.0 TOLERANCE = 20.0 +# Topics specified as list only (no expected frequency) +TOPIC_LIST_1 = '/multi_topic_list_1' +TOPIC_LIST_2 = '/multi_topic_list_2' +LIST_PUBLISHER_FREQ = 30.0 + @pytest.mark.launch_test def generate_test_description(): """Test multiple topics configured via parameters.""" + # Topics with frequency/tolerance configs topic_configs = { TOPIC_1: { 'expected_frequency': FREQUENCY_1, @@ -61,11 +68,22 @@ def generate_test_description(): } } - ros2_monitor_node = create_monitor_node(topic_configs=topic_configs) + # Also include topics specified as simple list (no frequencies) + topics_list = [TOPIC_LIST_1, TOPIC_LIST_2] + + ros2_monitor_node = create_monitor_node( + topics=topics_list, + topic_configs=topic_configs + ) publisher_1 = create_minimal_publisher(TOPIC_1, FREQUENCY_1, 'imu', '_multi_1') publisher_2 = create_minimal_publisher(TOPIC_2, FREQUENCY_2, 'imu', '_multi_2') publisher_3 = create_minimal_publisher(TOPIC_3, FREQUENCY_3, 'imu', '_multi_3') + # Publishers for topics without expected frequencies + publisher_list_1 = create_minimal_publisher( + TOPIC_LIST_1, LIST_PUBLISHER_FREQ, 'imu', '_list_1') + publisher_list_2 = create_minimal_publisher( + TOPIC_LIST_2, LIST_PUBLISHER_FREQ, 'imu', '_list_2') return ( launch.LaunchDescription([ @@ -73,6 +91,8 @@ def generate_test_description(): publisher_1, publisher_2, publisher_3, + publisher_list_1, + publisher_list_2, launch_testing.actions.ReadyToTest() ]), {} ) @@ -119,6 +139,47 @@ def test_all_topics_monitored(self): f'{tolerance_hz} of expected {expected_freq}' ) + def test_topics_list_monitored_without_expected_frequency(self): + """Test topics in list are monitored but show no expected frequency.""" + time.sleep(2.0) + + for topic in [TOPIC_LIST_1, TOPIC_LIST_2]: + with self.subTest(topic=topic): + diagnostics = collect_diagnostics_for_topic( + self.test_node, topic, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics), 3, + f'Expected at least 3 diagnostics for {topic}' + ) + + # Verify expected_frequency is 0.0 or not present (not configured) + last_diag = diagnostics[-1] + expected_freq_value = None + frame_rate_value = None + for kv in last_diag.values: + if kv.key == 'expected_frequency': + expected_freq_value = float(kv.value) + elif kv.key == 'frame_rate_node': + frame_rate_value = float(kv.value) + + # When not configured, expected_frequency is either not present or 0.0 + self.assertTrue( + expected_freq_value is None or expected_freq_value == 0.0, + f'{topic}: expected_frequency should be None or 0.0, ' + f'got {expected_freq_value}' + ) + + # Verify frame rate is being reported (topic is monitored) + self.assertIsNotNone( + frame_rate_value, + f'{topic}: should have frame_rate_node in diagnostics' + ) + self.assertGreater( + frame_rate_value, 0.0, + f'{topic}: frame_rate_node should be > 0' + ) + if __name__ == '__main__': unittest.main() From 61c898f63c35657054d175fedaf1ae5a0fecd746 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 5 Jan 2026 10:00:49 -0800 Subject: [PATCH 12/33] Move code inline and add param delete Signed-off-by: Blake McHale --- .../greenwave_monitor/ui_adaptor.py | 14 ++ .../include/greenwave_monitor.hpp | 8 +- greenwave_monitor/src/greenwave_monitor.cpp | 193 ++++++++++-------- 3 files changed, 128 insertions(+), 87 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 09f923c..2047f00 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -349,6 +349,20 @@ def _on_parameter_event(self, msg: ParameterEvent): 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 == TopicParamField.NONE: + continue + + with self.data_lock: + if field == TopicParamField.FREQUENCY: + self.expected_frequencies.pop(topic_name, None) + elif field == TopicParamField.TOLERANCE: + current = self.expected_frequencies.get(topic_name) + if current and current[0] > 0: + self.expected_frequencies[topic_name] = ( + current[0], DEFAULT_TOLERANCE_PERCENT) + def toggle_topic_monitoring(self, topic_name: str): """Toggle monitoring for a topic.""" if not self.manage_topic_client.wait_for_service(timeout_sec=1.0): diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index ce854fb..0b698ba 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -26,6 +26,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "std_msgs/msg/string.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" @@ -72,11 +73,15 @@ class GreenwaveMonitor : public rclcpp::Node std::string & message, bool update_parameters = true); + // Callback for this nodes parameter changes + additions rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & parameters); - void apply_topic_config(const std::string & topic_name, const TopicConfig & incoming); + // Callback for handling deletions of parameters (across all nodes) + void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg); + // Read all parameters from node at startup and ensure they are applied. on_paramter_change + // is not called at startup. void load_topic_parameters_from_overrides(); std::optional get_numeric_parameter(const std::string & param_name); @@ -108,6 +113,7 @@ class GreenwaveMonitor : public rclcpp::Node set_expected_frequency_service_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + rclcpp::Subscription::SharedPtr param_event_subscription_; // Flag to skip parameter callback when updating params internally (avoids redundant work) bool updating_params_internally_ = false; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 5c202fc..b9bc598 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -17,10 +17,7 @@ #include "greenwave_monitor.hpp" -#include #include -#include -#include #include "rcl_interfaces/srv/list_parameters.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" @@ -94,29 +91,6 @@ std::optional param_to_double(const rclcpp::Parameter & param) return std::nullopt; } -const char * get_field_name(TopicParamField field) -{ - if (field == TopicParamField::kNone) { - return "none"; - } else if (field == TopicParamField::kFrequency) { - return "expected_frequency"; - } else if (field == TopicParamField::kTolerance) { - return "tolerance"; - } - return "unknown"; -} - -const char * get_field_unit(TopicParamField field) -{ - if (field == TopicParamField::kNone) { - return ""; - } else if (field == TopicParamField::kFrequency) { - return "Hz"; - } else if (field == TopicParamField::kTolerance) { - return "%"; - } - return "unknown"; -} } // namespace GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) @@ -149,6 +123,11 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) param_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&GreenwaveMonitor::on_parameter_change, this, std::placeholders::_1)); + // Subscribe to parameter events to handle parameter deletions + param_event_subscription_ = this->create_subscription( + "/parameter_events", 10, + std::bind(&GreenwaveMonitor::on_parameter_event, this, std::placeholders::_1)); + // Process any topic parameters passed at startup load_topic_parameters_from_overrides(); @@ -272,6 +251,15 @@ bool GreenwaveMonitor::set_topic_expected_frequency( std::string & message, bool update_parameters) { + if (expected_hz <= 0.0) { + message = "Invalid expected frequency, must be set to a positive value"; + return false; + } + if (tolerance_percent < 0.0) { + message = "Invalid tolerance, must be a non-negative percentage"; + return false; + } + auto it = message_diagnostics_.find(topic_name); if (it == message_diagnostics_.end()) { @@ -286,15 +274,6 @@ bool GreenwaveMonitor::set_topic_expected_frequency( it = message_diagnostics_.find(topic_name); } - if (expected_hz <= 0.0) { - message = "Invalid expected frequency, must be set to a positive value"; - return false; - } - if (tolerance_percent < 0.0) { - message = "Invalid tolerance, must be a non-negative percentage"; - return false; - } - message_diagnostics::MessageDiagnostics & msg_diagnostics_obj = *(it->second); msg_diagnostics_obj.setExpectedDt(expected_hz, tolerance_percent); @@ -323,9 +302,11 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( return result; } - // Build a local map of incoming topic configs from this callback + // Collect validation errors and valid configs + std::vector errors; std::map incoming_configs; + // Construct expected frequency and tolerance pairs from parameter changes for (const auto & param : parameters) { auto info = parse_topic_param_name(param.get_name()); if (info.field == TopicParamField::kNone || info.topic_name.empty()) { @@ -345,64 +326,101 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( TopicConfig & config = incoming_configs[info.topic_name]; if (info.field == TopicParamField::kFrequency) { + if (value <= 0.0) { + errors.push_back( + param.get_name() + ": Invalid frequency, must be positive"); + continue; + } config.expected_frequency = value; } else { + if (value < 0.0) { + errors.push_back( + param.get_name() + ": Invalid tolerance, must be non-negative"); + continue; + } config.tolerance = value; } - - RCLCPP_INFO( - this->get_logger(), - "Parameter set: %s for topic '%s' = %.2f %s", - get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); } - // Apply configs for each topic affected by this parameter change + // Iterate over incoming configs and set expected frequencies/tolerances for (const auto & [topic_name, incoming] : incoming_configs) { - apply_topic_config(topic_name, incoming); + // Get expected frequency: prefer incoming, fall back to existing parameter + double expected_freq = 0.0; + if (incoming.expected_frequency.has_value()) { + expected_freq = incoming.expected_frequency.value(); + } else { + auto freq_opt = get_numeric_parameter(make_freq_param_name(topic_name)); + if (freq_opt.has_value()) { + expected_freq = freq_opt.value(); + } else { + // Tolerance set without frequency - nothing to apply yet + continue; + } + } + + // Get tolerance: prefer incoming, then existing parameter, then default + double tolerance = incoming.tolerance.value_or( + get_numeric_parameter(make_tol_param_name(topic_name)).value_or(kDefaultTolerancePercent) + ); + + std::string message; + bool success = set_topic_expected_frequency( + topic_name, + expected_freq, + tolerance, + true, + message, + false); // don't update parameters - called from parameter change + + // Log warning if the topic is not up yet or an error occurs while trying to monitor it + // Still errors if parameter is invalid value since that is redundantly checked earlier + if (!success) { + RCLCPP_WARN( + this->get_logger(), + "Could not apply monitoring config for topic '%s': %s", + topic_name.c_str(), message.c_str()); + } } return result; } -void GreenwaveMonitor::apply_topic_config( - const std::string & topic_name, const TopicConfig & incoming) +void GreenwaveMonitor::on_parameter_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) { - // Get expected frequency: prefer incoming, fall back to existing parameter - double expected_freq = 0.0; - if (incoming.expected_frequency.has_value()) { - expected_freq = incoming.expected_frequency.value(); - } else { - auto freq_opt = get_numeric_parameter(make_freq_param_name(topic_name)); - if (freq_opt.has_value()) { - expected_freq = freq_opt.value(); - } else { - // No frequency available yet, nothing to do - return; - } + // Only process events from this node + if (msg->node != this->get_fully_qualified_name()) { + return; } - // Get tolerance: prefer incoming, then existing parameter, then default - double tolerance = incoming.tolerance.value_or( - get_numeric_parameter(make_tol_param_name(topic_name)).value_or(kDefaultTolerancePercent) - ); + for (const auto & param : msg->deleted_parameters) { + auto info = parse_topic_param_name(param.name); + if (info.field == TopicParamField::kNone || info.topic_name.empty()) { + continue; + } - std::string message; - bool success = set_topic_expected_frequency( - topic_name, - expected_freq, - tolerance, - true, - message, - false); // don't update parameters - called from parameter change + auto it = message_diagnostics_.find(info.topic_name); + if (it == message_diagnostics_.end()) { + continue; + } - if (success) { - RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); - } else { - RCLCPP_WARN( - this->get_logger(), - "Could not apply config for topic '%s': %s. " - "Use manage_topic service to add the topic first.", - topic_name.c_str(), message.c_str()); + if (info.field == TopicParamField::kFrequency) { + it->second->clearExpectedDt(); + RCLCPP_DEBUG( + this->get_logger(), + "Cleared expected frequency for topic '%s' (parameter deleted)", + info.topic_name.c_str()); + } else if (info.field == TopicParamField::kTolerance) { + // Reset tolerance to default if frequency is still set + auto freq_opt = get_numeric_parameter(make_freq_param_name(info.topic_name)); + if (freq_opt.has_value() && freq_opt.value() > 0) { + it->second->setExpectedDt(freq_opt.value(), kDefaultTolerancePercent); + RCLCPP_DEBUG( + this->get_logger(), + "Reset tolerance to default (%.1f%%) for topic '%s' (parameter deleted)", + kDefaultTolerancePercent, info.topic_name.c_str()); + } + } } } @@ -417,6 +435,7 @@ void GreenwaveMonitor::load_topic_parameters_from_overrides() // Build a local map of topic configs from startup parameters std::map startup_configs; + // Construct expected frequency and tolerance pairs from startup parameters for (const auto & name : all_params.names) { auto info = parse_topic_param_name(name); if (info.field == TopicParamField::kNone || info.topic_name.empty()) { @@ -436,15 +455,11 @@ void GreenwaveMonitor::load_topic_parameters_from_overrides() } else { config.tolerance = value; } - - RCLCPP_INFO( - this->get_logger(), - "Initial parameter: %s for topic '%s' = %.2f %s", - get_field_name(info.field), info.topic_name.c_str(), value, get_field_unit(info.field)); } - // Apply all configs that have frequency set + // Iterate over starting config and add topics/set expected frequencies for (const auto & [topic, config] : startup_configs) { + // Topics will only be added if frequency is set if (config.expected_frequency.has_value()) { double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); @@ -457,9 +472,7 @@ void GreenwaveMonitor::load_topic_parameters_from_overrides() message, false); // don't update parameters - if (success) { - RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); - } else { + if (!success) { RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); } } @@ -624,6 +637,14 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag topic, std::make_unique(*this, topic, diagnostics_config)); + // If parameters are set, use them to set the expected frequency and tolerance + auto freq_opt = get_numeric_parameter(make_freq_param_name(topic)); + auto tol_opt = get_numeric_parameter(make_tol_param_name(topic)); + double tolerance = tol_opt.value_or(kDefaultTolerancePercent); + if (freq_opt.has_value() && tolerance >= 0.0 && freq_opt.value() > 0.0) { + message_diagnostics_[topic]->setExpectedDt(freq_opt.value(), tolerance); + } + message = "Successfully added topic"; return true; } From 99f29810d6608911e3b36acbb59cc30ede6a9c96 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 5 Jan 2026 10:19:25 -0800 Subject: [PATCH 13/33] Catch exceptions with declare Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index b9bc598..9f037f0 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -280,8 +280,14 @@ bool GreenwaveMonitor::set_topic_expected_frequency( // Sync parameters with the new values if (update_parameters) { updating_params_internally_ = true; - declare_or_set_parameter(make_freq_param_name(topic_name), expected_hz); - declare_or_set_parameter(make_tol_param_name(topic_name), tolerance_percent); + try { + declare_or_set_parameter(make_freq_param_name(topic_name), expected_hz); + declare_or_set_parameter(make_tol_param_name(topic_name), tolerance_percent); + } catch (const std::exception & e) { + message = "Could not set parameters for topic '" + topic_name + "': " + e.what(); + updating_params_internally_ = false; + return false; + } updating_params_internally_ = false; } From de7b89927261e3d605f423d8f6efd06ddde06210 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 5 Jan 2026 10:44:00 -0800 Subject: [PATCH 14/33] Fix typo Signed-off-by: Blake McHale --- greenwave_monitor/include/greenwave_monitor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 0b698ba..20a3237 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -80,7 +80,7 @@ class GreenwaveMonitor : public rclcpp::Node // Callback for handling deletions of parameters (across all nodes) void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg); - // Read all parameters from node at startup and ensure they are applied. on_paramter_change + // Read all parameters from node at startup and ensure they are applied. on_parameter_change // is not called at startup. void load_topic_parameters_from_overrides(); From 4ef8db82068f5e0f89721f5ce8f19ad4e3e75078 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 5 Jan 2026 11:07:27 -0800 Subject: [PATCH 15/33] Reject non-numeric parameters and allow both int and double Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 9f037f0..48c38b7 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -19,6 +19,7 @@ #include +#include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" @@ -321,10 +322,7 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( auto value_opt = param_to_double(param); if (!value_opt.has_value()) { - RCLCPP_WARN( - this->get_logger(), - "Parameter '%s' is not a numeric type, skipping", - param.get_name().c_str()); + errors.push_back(param.get_name() + ": must be a numeric type (int or double)"); continue; } @@ -388,6 +386,11 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( } } + if (!errors.empty()) { + result.successful = false; + result.reason = "Invalid parameters: " + rcpputils::join(errors, ", "); + } + return result; } @@ -509,7 +512,10 @@ void GreenwaveMonitor::try_undeclare_parameter(const std::string & param_name) void GreenwaveMonitor::declare_or_set_parameter(const std::string & param_name, double value) { if (!this->has_parameter(param_name)) { - this->declare_parameter(param_name, value); + // Allow both integer and double types for numeric parameters + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + this->declare_parameter(param_name, value, descriptor); } else { this->set_parameter(rclcpp::Parameter(param_name, value)); } From 6fa7976fbf9f6f8d9340d01f461073e94f546d74 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 5 Jan 2026 11:42:08 -0800 Subject: [PATCH 16/33] Add tests for deleting and setting invalid parameters Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 59 +++++++- greenwave_monitor/src/greenwave_monitor.cpp | 5 + .../test/parameters/test_param_dynamic.py | 129 ++++++++++++++++-- 3 files changed, 177 insertions(+), 16 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index dc3c274..fd54cc1 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -66,10 +66,21 @@ def make_tol_param(topic: str) -> str: return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' -def set_parameter(test_node: Node, param_name: str, value: float, +def set_parameter(test_node: Node, param_name: str, value, node_name: str = MONITOR_NODE_NAME, timeout_sec: float = 10.0) -> bool: - """Set a parameter on the monitor node using rclpy service client.""" + """Set a parameter on the monitor node using rclpy service client. + + Args: + test_node: The ROS node to use for the service client. + param_name: The name of the parameter to set. + value: The value to set (float, int, or str). + node_name: The name of the target node. + timeout_sec: Timeout for the service call. + + Returns: + True if the parameter was set successfully, False otherwise. + """ full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' service_name = f'{full_node_name}/set_parameters' @@ -80,8 +91,19 @@ def set_parameter(test_node: Node, param_name: str, value: float, param = Parameter() param.name = param_name param.value = ParameterValue() - param.value.type = ParameterType.PARAMETER_DOUBLE - param.value.double_value = float(value) + + 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) request = SetParameters.Request() request.parameters = [param] @@ -125,6 +147,35 @@ def get_parameter(test_node: Node, param_name: str, return False, None +def delete_parameter(test_node: Node, param_name: str, + node_name: str = MONITOR_NODE_NAME, + timeout_sec: float = 10.0) -> bool: + """Delete a parameter from the monitor node using rclpy service client.""" + full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + service_name = f'{full_node_name}/set_parameters' + + client = test_node.create_client(SetParameters, service_name) + if not client.wait_for_service(timeout_sec=5.0): + return False + + param = Parameter() + param.name = param_name + param.value = ParameterValue() + param.value.type = ParameterType.PARAMETER_NOT_SET + + request = SetParameters.Request() + request.parameters = [param] + + future = client.call_async(request) + rclpy.spin_until_future_complete(test_node, future, timeout_sec=timeout_sec) + + test_node.destroy_client(client) + + if future.result() is None: + return False + return all(r.successful for r in future.result().results) + + def create_minimal_publisher( topic: str, frequency_hz: float, message_type: str, id_suffix: str = ''): """Create a minimal publisher node with the given parameters.""" diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 48c38b7..79ac429 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -320,6 +320,11 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( continue; } + // Allow PARAMETER_NOT_SET for parameter deletion + if (param.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + continue; + } + auto value_opt = param_to_double(param); if (!value_opt.has_value()) { errors.push_back(param.get_name() + ": must be a numeric type (int or double)"); diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 6e157c5..63903f6 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -26,6 +26,7 @@ collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, + delete_parameter, get_parameter, make_freq_param, make_tol_param, @@ -38,6 +39,8 @@ TEST_TOPIC = '/dynamic_param_topic' +TEST_TOPIC_SET_PARAMS = '/dynamic_param_topic_set_params' +TEST_TOPIC_DELETE_PARAM = '/dynamic_param_topic_delete_param' TEST_FREQUENCY = 30.0 TEST_TOLERANCE = 20.0 NONEXISTENT_TOPIC = '/topic_that_does_not_exist' @@ -52,10 +55,20 @@ def generate_test_description(): TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' ) + publisher_set_params = create_minimal_publisher( + TEST_TOPIC_SET_PARAMS, TEST_FREQUENCY, 'imu', '_set_params' + ) + + publisher_delete_param = create_minimal_publisher( + TEST_TOPIC_DELETE_PARAM, TEST_FREQUENCY, 'imu', '_delete_param' + ) + return ( launch.LaunchDescription([ ros2_monitor_node, publisher, + publisher_set_params, + publisher_delete_param, launch_testing.actions.ReadyToTest() ]), {} ) @@ -70,19 +83,19 @@ def test_set_parameters(self): """Test setting frequency and tolerance parameters in sequence.""" time.sleep(2.0) - freq_param = make_freq_param(TEST_TOPIC) - tol_param = make_tol_param(TEST_TOPIC) + freq_param = make_freq_param(TEST_TOPIC_SET_PARAMS) + tol_param = make_tol_param(TEST_TOPIC_SET_PARAMS) # 1. Verify topic is not monitored initially initial_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 + self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=1, timeout_sec=2.0 ) self.assertEqual( len(initial_diagnostics), 0, - f'{TEST_TOPIC} should not be monitored initially' + f'{TEST_TOPIC_SET_PARAMS} should not be monitored initially' ) - # 2. Set tolerance before frequency - topic should remain unmonitored + # 2. Set tolerance before frequency - should succeed but not start monitoring success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) self.assertTrue(success, f'Failed to set {tol_param}') @@ -95,14 +108,14 @@ def test_set_parameters(self): time.sleep(1.0) diagnostics_after_tol = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=2.0 + self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=1, timeout_sec=2.0 ) self.assertEqual( len(diagnostics_after_tol), 0, - f'{TEST_TOPIC} should remain unmonitored after setting only tolerance' + f'{TEST_TOPIC_SET_PARAMS} should remain unmonitored after setting only tolerance' ) - # 3. Set frequency - topic should become monitored + # 3. Set frequency - topic should have frequency checking enabled success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') @@ -115,7 +128,7 @@ def test_set_parameters(self): time.sleep(1.0) diagnostics_after_freq = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=10.0 ) self.assertGreaterEqual( len(diagnostics_after_freq), 3, @@ -135,7 +148,7 @@ def test_set_parameters(self): time.sleep(2.0) diagnostics_with_zero_tol = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=2, timeout_sec=5.0 + self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=2, timeout_sec=5.0 ) self.assertGreaterEqual( len(diagnostics_with_zero_tol), 2, @@ -158,7 +171,7 @@ def test_set_parameters(self): # Wait for diagnostics to stabilize after tolerance change time.sleep(3.0) diagnostics_after_reset = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=10.0 ) self.assertGreaterEqual( len(diagnostics_after_reset), 3, @@ -187,7 +200,7 @@ def test_set_parameters(self): time.sleep(2.0) diagnostics_mismatched = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 + self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=10.0 ) self.assertGreaterEqual( len(diagnostics_mismatched), 3, @@ -229,6 +242,98 @@ def test_set_frequency_for_nonexistent_topic(self): f'{NONEXISTENT_TOPIC} should not appear in diagnostics' ) + def test_non_numeric_parameter_rejected(self): + """Test that non-numeric parameter values are rejected.""" + time.sleep(1.0) + + freq_param = make_freq_param(TEST_TOPIC) + success = set_parameter(self.test_node, freq_param, 'not_a_number') + self.assertFalse(success, 'Non-numeric frequency parameter should be rejected') + + tol_param = make_tol_param(TEST_TOPIC) + success = set_parameter(self.test_node, tol_param, 'invalid') + self.assertFalse(success, 'Non-numeric tolerance parameter should be rejected') + + def test_non_positive_frequency_rejected(self): + """Test that non-positive frequency values are rejected.""" + time.sleep(1.0) + + freq_param = make_freq_param(TEST_TOPIC) + + # Test zero frequency + success = set_parameter(self.test_node, freq_param, 0.0) + self.assertFalse(success, 'Zero frequency should be rejected') + + # Test negative frequency + success = set_parameter(self.test_node, freq_param, -10.0) + 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(TEST_TOPIC) + success = set_parameter(self.test_node, tol_param, -5.0) + self.assertFalse(success, 'Negative tolerance should be rejected') + + def test_delete_parameter_clears_error(self): + """Test that deleting a parameter clears the error state.""" + time.sleep(2.0) + + freq_param = make_freq_param(TEST_TOPIC_DELETE_PARAM) + tol_param = make_tol_param(TEST_TOPIC_DELETE_PARAM) + + # Set up monitoring with correct frequency (publisher is at 30 Hz) + success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) + self.assertTrue(success, f'Failed to set {freq_param}') + + success = set_parameter(self.test_node, tol_param, 10.0) + self.assertTrue(success, f'Failed to set {tol_param}') + + time.sleep(2.0) + + # Verify initial diagnostics are OK + diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC_DELETE_PARAM, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual(len(diagnostics), 1, 'Should have diagnostics') + self.assertEqual( + ord(diagnostics[-1].level), 0, + 'Initial diagnostics should be OK' + ) + + # Set mismatched frequency to cause error (expect 1 Hz but publisher is 30 Hz) + success = set_parameter(self.test_node, freq_param, 1.0) + self.assertTrue(success, 'Failed to set mismatched frequency') + + time.sleep(2.0) + + # Verify diagnostics show error + diagnostics_error = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC_DELETE_PARAM, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual(len(diagnostics_error), 1, 'Should have diagnostics') + has_error = any(ord(d.level) != 0 for d in diagnostics_error) + self.assertTrue(has_error, 'Should have error diagnostics with mismatched frequency') + + # Delete the frequency parameter to clear expected frequency + success = delete_parameter(self.test_node, freq_param) + self.assertTrue(success, f'Failed to delete {freq_param}') + + time.sleep(2.0) + + # Verify diagnostics are OK again (no expected frequency = no error) + diagnostics_after_delete = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC_DELETE_PARAM, expected_count=3, timeout_sec=10.0 + ) + self.assertGreaterEqual( + len(diagnostics_after_delete), 1, 'Should have diagnostics after delete' + ) + self.assertEqual( + ord(diagnostics_after_delete[-1].level), 0, + 'Diagnostics should be OK after deleting frequency parameter' + ) + if __name__ == '__main__': unittest.main() From 53b052a949a19eeb26cebd53fcc5fb5e9604e044 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Mon, 5 Jan 2026 11:51:28 -0800 Subject: [PATCH 17/33] Fix lint Signed-off-by: Blake McHale --- greenwave_monitor/greenwave_monitor/test_utils.py | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index fd54cc1..5cf34fe 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -69,18 +69,7 @@ def make_tol_param(topic: str) -> str: def set_parameter(test_node: Node, param_name: str, value, node_name: str = MONITOR_NODE_NAME, timeout_sec: float = 10.0) -> bool: - """Set a parameter on the monitor node using rclpy service client. - - Args: - test_node: The ROS node to use for the service client. - param_name: The name of the parameter to set. - value: The value to set (float, int, or str). - node_name: The name of the target node. - timeout_sec: Timeout for the service call. - - Returns: - True if the parameter was set successfully, False otherwise. - """ + """Set a parameter on the monitor node using rclpy service client.""" full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' service_name = f'{full_node_name}/set_parameters' From 82971d761db3f41250cbb0841acaedda67ec4cb8 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Wed, 7 Jan 2026 00:31:10 -0800 Subject: [PATCH 18/33] Move parameters to greenwave_diagnostics.hpp Signed-off-by: Blake McHale --- docs/images/SERVICES.md | 22 +- greenwave_monitor/CMakeLists.txt | 24 +- .../config/greenwave_monitor.yaml | 9 + greenwave_monitor/examples/example.launch.py | 67 +- .../greenwave_monitor/ncurses_frontend.py | 8 +- .../greenwave_monitor/test_utils.py | 41 +- .../greenwave_monitor/ui_adaptor.py | 299 ++++++-- ...gnostics.hpp => greenwave_diagnostics.hpp} | 415 +++++++++- .../include/greenwave_monitor.hpp | 64 +- .../include/minimal_publisher_node.hpp | 4 +- greenwave_monitor/src/greenwave_monitor.cpp | 711 ++++++------------ .../src/minimal_publisher_node.cpp | 23 +- .../test/parameters/test_param_dynamic.py | 6 +- .../parameters/test_param_enable_existing.py | 147 ++++ .../test/parameters/test_param_yaml.py | 6 +- ...ics.cpp => test_greenwave_diagnostics.cpp} | 69 +- .../test/test_greenwave_monitor.py | 15 +- .../test/test_topic_monitoring_integration.py | 11 +- greenwave_monitor_interfaces/CMakeLists.txt | 1 - .../srv/SetExpectedFrequency.srv | 27 - 20 files changed, 1175 insertions(+), 794 deletions(-) create mode 100644 greenwave_monitor/config/greenwave_monitor.yaml rename greenwave_monitor/include/{message_diagnostics.hpp => greenwave_diagnostics.hpp} (51%) create mode 100644 greenwave_monitor/test/parameters/test_param_enable_existing.py rename greenwave_monitor/test/{test_message_diagnostics.cpp => test_greenwave_diagnostics.cpp} (77%) delete mode 100644 greenwave_monitor_interfaces/srv/SetExpectedFrequency.srv diff --git a/docs/images/SERVICES.md b/docs/images/SERVICES.md index 2f6de1e..7b8b1ac 100644 --- a/docs/images/SERVICES.md +++ b/docs/images/SERVICES.md @@ -1,8 +1,8 @@ -# Services +# Services and 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 provides a `ManageTopic` service to dynamically add or remove topics from monitoring. Expected frequencies are configured via ROS parameters, which enables additional diagnostic values and statuses. -## Manage Topic +## Manage Topic Service The monitor node exposes a `/greenwave_monitor/manage_topic` service that follows the `greenwave_monitor_interfaces/srv/ManageTopic` service definition. @@ -18,20 +18,28 @@ To remove a topic from the monitoring list: ros2 service call /greenwave_monitor/manage_topic greenwave_monitor_interfaces/srv/ManageTopic "{topic_name: '/topic2', add_topic: 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: +- `topics..expected_frequency` - Expected publish rate in Hz +- `topics..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 topics./topic2.expected_frequency 30.0 +ros2 param set /greenwave_monitor topics./topic2.tolerance 10.0 ``` To clear the expected frequency for a topic: ```bash -ros2 service call /greenwave_monitor/set_expected_frequency greenwave_monitor_interfaces/srv/SetExpectedFrequency "{topic_name: '/topic2', clear_expected: true}" +ros2 param delete /greenwave_monitor topics./topic2.expected_frequency +``` + +Parameters can also be set at launch time: +```bash +ros2 run greenwave_monitor greenwave_monitor --ros-args -p topics./topic2.expected_frequency:=30.0 -p topics./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 29c1a56..827cb1f 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -23,9 +23,9 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# Add message_diagnostics.hpp as a header-only library -add_library(message_diagnostics INTERFACE) -target_include_directories(message_diagnostics INTERFACE +# Add greenwave_diagnostics.hpp as a header-only library +add_library(greenwave_diagnostics INTERFACE) +target_include_directories(greenwave_diagnostics INTERFACE $ $) @@ -36,7 +36,7 @@ ament_target_dependencies(greenwave_monitor diagnostic_msgs greenwave_monitor_interfaces ) -target_link_libraries(greenwave_monitor message_diagnostics) +target_link_libraries(greenwave_monitor greenwave_diagnostics) target_include_directories(greenwave_monitor PUBLIC $ @@ -51,7 +51,7 @@ add_executable(minimal_publisher_node src/minimal_publisher_node.cpp src/minimal_publisher_main.cpp) ament_target_dependencies(minimal_publisher_node rclcpp std_msgs sensor_msgs diagnostic_msgs) -target_link_libraries(minimal_publisher_node message_diagnostics) +target_link_libraries(minimal_publisher_node greenwave_diagnostics) target_include_directories(minimal_publisher_node PUBLIC $ $) @@ -60,7 +60,7 @@ install(TARGETS minimal_publisher_node DESTINATION lib/${PROJECT_NAME}) install( - DIRECTORY launch examples + DIRECTORY launch examples config DESTINATION share/${PROJECT_NAME} ) @@ -128,21 +128,21 @@ if(BUILD_TESTING) endforeach() # 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 @@ -155,7 +155,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 92bb6d2..d182337 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -12,52 +12,91 @@ # 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_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': {'expected_frequency': 100.0, 'tolerance': 5.0}, - '/image_topic': {'expected_frequency': 30.0, 'tolerance': 5.0}, - '/string_topic': {'expected_frequency': 1000.0, 'tolerance': 5.0} - }, + '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] ) ]) 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 5cf34fe..ad2f4ca 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -29,7 +29,7 @@ TOL_SUFFIX, TOPIC_PARAM_PREFIX, ) -from greenwave_monitor_interfaces.srv import ManageTopic, SetExpectedFrequency +from greenwave_monitor_interfaces.srv import ManageTopic import launch_ros from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue from rcl_interfaces.srv import GetParameters, SetParameters @@ -166,7 +166,8 @@ def delete_parameter(test_node: Node, param_name: str, 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', @@ -175,7 +176,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' ) @@ -247,33 +249,6 @@ def call_manage_topic_service(node: Node, 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, @@ -404,11 +379,7 @@ def create_service_clients(node: Node, namespace: str = MONITOR_NODE_NAMESPACE, ManageTopic, f'/{namespace}/{node_name}/manage_topic' ) - set_frequency_client = node.create_client( - SetExpectedFrequency, f'/{namespace}/{node_name}/set_expected_frequency' - ) - - return manage_topic_client, set_frequency_client + return manage_topic_client class RosNodeTestCase(unittest.TestCase, ABC): diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 2047f00..e95037c 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -26,32 +26,33 @@ 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: +- ManageTopic service client: start/stop monitoring a topic (`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 from enum import Enum +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 ParameterEvent, ParameterType, ParameterValue -from rcl_interfaces.srv import GetParameters, ListParameters +from greenwave_monitor_interfaces.srv import ManageTopic +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 = 'topics.' +TOPIC_PARAM_PREFIX = 'greenwave_diagnostics.' FREQ_SUFFIX = '.expected_frequency' TOL_SUFFIX = '.tolerance' +ENABLED_SUFFIX = '.enabled' DEFAULT_TOLERANCE_PERCENT = 5.0 @@ -137,7 +138,7 @@ class GreenwaveUiAdaptor: 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 + expected frequencies via ROS parameters. Service names may be discovered dynamically or constructed from an optional namespace and node name. """ @@ -170,7 +171,6 @@ def _setup_ros_components(self): ) manage_service_name = f'{self.monitor_node_name}/manage_topic' - set_freq_service_name = f'{self.monitor_node_name}/set_expected_frequency' self.node.get_logger().info(f'Connecting to monitor service: {manage_service_name}') @@ -179,12 +179,7 @@ def _setup_ros_components(self): manage_service_name ) - self.set_expected_frequency_client = self.node.create_client( - SetExpectedFrequency, - set_freq_service_name - ) - - # Parameter service clients for querying initial state + # Parameter service clients for querying and setting parameters self.list_params_client = self.node.create_client( ListParameters, f'{self.monitor_node_name}/list_parameters' @@ -193,66 +188,121 @@ def _setup_ros_components(self): GetParameters, f'{self.monitor_node_name}/get_parameters' ) + self.set_params_client = self.node.create_client( + SetParameters, + f'{self.monitor_node_name}/set_parameters' + ) + + # Track pending node queries to prevent garbage collection + self._pending_node_queries: Dict[str, dict] = {} - # Query initial parameters after a short delay to let services come up + # Query initial parameters after a short delay to let nodes come up self._initial_params_timer = self.node.create_timer( - 0.1, self._fetch_initial_parameters_callback) + 2.0, 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 + """Timer callback to fetch initial parameters from all nodes.""" + # Cancel timer - we only run once 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) + # Get all nodes in the system + node_names_and_namespaces = self.node.get_node_names_and_namespaces() + + for node_name, node_namespace in node_names_and_namespaces: + # Skip our own node + if node_name == self.node.get_name(): + continue + + if node_namespace == '/': + full_node_name = f'/{node_name}' + else: + full_node_name = f'{node_namespace}/{node_name}' + + self._query_node_parameters(full_node_name) + + def _query_node_parameters(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') + + # Check if service exists (non-blocking) + if not list_client.service_is_ready(): + self.node.destroy_client(list_client) + return + + # Store client to prevent garbage collection + 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 = ['topics'] + list_request.prefixes = ['greenwave_diagnostics'] list_request.depth = 10 - list_future = self.list_params_client.call_async(list_request) - list_future.add_done_callback(self._on_list_parameters_response) + list_future = list_client.call_async(list_request) + list_future.add_done_callback( + lambda f, qid=query_id: self._on_node_list_response(f, qid)) - def _on_list_parameters_response(self, future): - """Handle response from list_parameters service.""" + def _on_node_list_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: + return + if future.result() is None: + self._cleanup_node_query(query_id) 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: + self._cleanup_node_query(query_id) return - # Store for use in get callback - self._pending_param_names = param_names + # Create get_parameters client + 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 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) + get_future = get_client.call_async(get_request) + get_future.add_done_callback( + lambda f, qid=query_id: self._on_node_get_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_parameters_response(self, future): - """Handle response from get_parameters service.""" + def _on_node_get_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: + return + if future.result() is None: + self._cleanup_node_query(query_id) return - param_names = getattr(self, '_pending_param_names', []) + param_names = query['param_names'] values = future.result().values # Parse parameters into expected_frequencies @@ -280,11 +330,22 @@ def _on_get_parameters_response(self, future): for topic_name, config in topic_configs.items(): freq = config.get('freq', 0.0) tol = config.get('tol', DEFAULT_TOLERANCE_PERCENT) - if freq > 0: + 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 _extract_topic_name(self, diagnostic_name: str) -> str: """ @@ -340,7 +401,8 @@ def _on_parameter_event(self, msg: ParameterEvent): current = self.expected_frequencies.get(topic_name, (0.0, 0.0)) if field == TopicParamField.FREQUENCY: - if value > 0: + # Treat NaN or non-positive as "cleared" + if value > 0 and not math.isnan(value): self.expected_frequencies[topic_name] = (value, current[1]) elif topic_name in self.expected_frequencies: del self.expected_frequencies[topic_name] @@ -403,27 +465,107 @@ def toggle_topic_monitoring(self, topic_name: str): action = 'start' if request.add_topic else 'stop' self.node.get_logger().error(f'Failed to {action} monitoring: {e}') + def _find_node_with_topic_param(self, topic_name: str) -> str: + """ + Find a node that has the frequency parameter for this topic. + + Searches all nodes in the system for the parameter. Falls back to the + monitor node if no node is found with the parameter. + """ + freq_param_name = f'{TOPIC_PARAM_PREFIX}{topic_name}{FREQ_SUFFIX}' + + # Get all nodes in the system + node_names_and_namespaces = self.node.get_node_names_and_namespaces() + + for node_name, node_namespace in node_names_and_namespaces: + # Skip our own node + if node_name == self.node.get_name(): + continue + + if node_namespace == '/': + full_node_name = f'/{node_name}' + else: + full_node_name = f'{node_namespace}/{node_name}' + + # Check if this node has the frequency parameter + list_client = self.node.create_client( + ListParameters, f'{full_node_name}/list_parameters') + if not list_client.wait_for_service(timeout_sec=0.5): + self.node.destroy_client(list_client) + continue + + list_request = ListParameters.Request() + list_request.prefixes = [freq_param_name] + list_request.depth = 1 + + future = list_client.call_async(list_request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=1.0) + self.node.destroy_client(list_client) + + if future.result() is not None: + if freq_param_name in future.result().result.names: + return full_node_name + + # Fall back to the monitor node + return self.monitor_node_name + def set_expected_frequency(self, topic_name: str, expected_hz: float = 0.0, 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.' + """Set or clear the expected frequency for a topic via parameters.""" + # Find a node that has the parameter, or fall back to monitor node + target_node = self._find_node_with_topic_param(topic_name) + + # Create a client to the target node's set_parameters service + set_params_client = self.node.create_client( + SetParameters, f'{target_node}/set_parameters') + if not set_params_client.wait_for_service(timeout_sec=1.0): + self.node.destroy_client(set_params_client) + return False, f'Could not connect to {target_node}/set_parameters service.' + + freq_param_name = f'{TOPIC_PARAM_PREFIX}{topic_name}{FREQ_SUFFIX}' + tol_param_name = f'{TOPIC_PARAM_PREFIX}{topic_name}{TOL_SUFFIX}' + + request = SetParameters.Request() + + if clear: + # Clear by setting frequency to NaN and tolerance to default + freq_param = Parameter() + freq_param.name = freq_param_name + freq_param.value = ParameterValue() + freq_param.value.type = ParameterType.PARAMETER_DOUBLE + freq_param.value.double_value = float('nan') + + tol_param = Parameter() + tol_param.name = tol_param_name + tol_param.value = ParameterValue() + tol_param.value.type = ParameterType.PARAMETER_DOUBLE + tol_param.value.double_value = DEFAULT_TOLERANCE_PERCENT + + request.parameters = [freq_param, tol_param] + else: + # Set frequency and tolerance parameters + freq_param = Parameter() + freq_param.name = freq_param_name + freq_param.value = ParameterValue() + freq_param.value.type = ParameterType.PARAMETER_DOUBLE + freq_param.value.double_value = expected_hz - 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 + tol_param = Parameter() + tol_param.name = tol_param_name + tol_param.value = ParameterValue() + tol_param.value.type = ParameterType.PARAMETER_DOUBLE + tol_param.value.double_value = tolerance_percent + + request.parameters = [freq_param, tol_param] - # Use asynchronous service call to prevent deadlock try: - future = self.set_expected_frequency_client.call_async(request) + future = set_params_client.call_async(request) rclpy.spin_until_future_complete(self.node, future, timeout_sec=3.0) + self.node.destroy_client(set_params_client) if future.result() is None: action = 'clear' if clear else 'set' @@ -431,21 +573,27 @@ def set_expected_frequency(self, self.node.get_logger().error(error_msg) return False, error_msg - response = future.result() + results = future.result().results + all_successful = all(r.successful for r in results) - if not response.success: + if not all_successful: action = 'clear' if clear else 'set' - self.node.get_logger().error( - f'Failed to {action} expected frequency: {response.message}') - return False, response.message - 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 + reasons = [r.reason for r in results if not r.successful and r.reason] + error_msg = f'Failed to {action} expected frequency: {"; ".join(reasons)}' + self.node.get_logger().error(error_msg) + return False, error_msg + + with self.data_lock: + if clear: + self.expected_frequencies.pop(topic_name, None) + else: + self.expected_frequencies[topic_name] = (expected_hz, tolerance_percent) + + action = 'Cleared' if clear else 'Set' + return True, f'{action} expected frequency for {topic_name}' + except Exception as e: + self.node.destroy_client(set_params_client) action = 'clear' if clear else 'set' error_msg = f'Failed to {action} expected frequency: {e}' self.node.get_logger().error(error_msg) @@ -460,3 +608,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: + 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 51% rename from greenwave_monitor/include/message_diagnostics.hpp rename to greenwave_monitor/include/greenwave_diagnostics.hpp index 324d37d..88274b4 100644 --- a/greenwave_monitor/include/message_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -18,10 +18,12 @@ #pragma once #include +#include #include #include #include #include +#include #include #include #include @@ -31,9 +33,12 @@ #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 +51,21 @@ 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 const char * kEnableNodeTimeSuffix = ".enable_node_time"; +inline constexpr const char * kEnableMsgTimeSuffix = ".enable_msg_time"; +inline constexpr const char * kEnableIncreasingMsgTimeSuffix = ".enable_increasing_msg_time"; +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 +89,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 +122,89 @@ 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; + enable_node_time_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + + constants::kEnableNodeTimeSuffix; + enable_msg_time_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + + constants::kEnableMsgTimeSuffix; + enable_increasing_msg_time_param_name_ = std::string(constants::kTopicParamPrefix) + + topic_name_ + + constants::kEnableIncreasingMsgTimeSuffix; + + // 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(enable_node_time_param_name_, + diagnostics_config_.enable_node_time_diagnostics); + node_.declare_parameter(enable_msg_time_param_name_, + diagnostics_config_.enable_msg_time_diagnostics); + node_.declare_parameter(enable_increasing_msg_time_param_name_, + diagnostics_config_.enable_increasing_msg_time_diagnostics); + 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(enable_node_time_param_name_, + diagnostics_config_.enable_node_time_diagnostics); + setParameterOrDefault(enable_msg_time_param_name_, + diagnostics_config_.enable_msg_time_diagnostics); + setParameterOrDefault(enable_increasing_msg_time_param_name_, + diagnostics_config_.enable_increasing_msg_time_diagnostics); + setParameterOrDefault(enabled_param_name_, constants::kDefaultEnabled); + setParameterOrDefault(freq_param_name_, default_freq); + setParameterOrDefault(tol_param_name_, constants::kDefaultTolerancePercent); + } + } + + ~GreenwaveDiagnostics() + { + // Unregister parameter callback to avoid dangling references + if (param_callback_handle_) { + node_.remove_on_set_parameters_callback(param_callback_handle_.get()); + } } // 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 +214,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 +229,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 +252,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 +267,15 @@ 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 +335,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 +369,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 +385,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; + + // Automatically enable node and msg time diagnostics when expected frequency is set + node_.set_parameter(rclcpp::Parameter(enable_node_time_param_name_, true)); + node_.set_parameter(rclcpp::Parameter(enable_msg_time_param_name_, 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 + node_.set_parameter(rclcpp::Parameter(enable_node_time_param_name_, false)); + node_.set_parameter(rclcpp::Parameter(enable_msg_time_param_name_, false)); } private: @@ -352,7 +452,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 +464,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 +500,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 +523,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 +553,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 +566,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 +586,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 +602,244 @@ 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::optional new_enable_node_time; + std::optional new_enable_msg_time; + std::optional new_enable_increasing_msg_time; + 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_ && + param.get_name() != enable_node_time_param_name_ && + param.get_name() != enable_msg_time_param_name_ && + param.get_name() != enable_increasing_msg_time_param_name_) + { + continue; + } + + // Allow PARAMETER_NOT_SET for parameter deletion + if (param.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + 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)"); + 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; + } + // Handle boolean types together + } else if (param.get_name() == enabled_param_name_) { + new_enabled = param.as_bool(); + } else if (param.get_name() == enable_node_time_param_name_) { + new_enable_node_time = param.as_bool(); + } else if (param.get_name() == enable_msg_time_param_name_) { + new_enable_msg_time = param.as_bool(); + } else if (param.get_name() == enable_increasing_msg_time_param_name_) { + new_enable_increasing_msg_time = param.as_bool(); + } + } + + // Exit early if any parameters are invalid. No half changes. + if (!error_reasons.empty()) { + result.successful = false; + result.reason = "Invalid parameters: " + 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); + } + } + + // Process deleted parameters + for (const auto & param : msg->deleted_parameters) { + if (param.name == freq_param_name_) { + clearExpectedDt(); + RCLCPP_DEBUG( + node_.get_logger(), + "Cleared expected frequency for topic '%s' (parameter deleted)", + topic_name_.c_str()); + } else if (param.name == tol_param_name_) { + // Reset tolerance to default if frequency is still set + auto freq_opt = getNumericParameter(freq_param_name_); + if (freq_opt.has_value() && freq_opt.value() > 0) { + setExpectedDt(freq_opt.value(), constants::kDefaultTolerancePercent); + RCLCPP_DEBUG( + node_.get_logger(), + "Reset tolerance to default (%.1f%%) for topic '%s' (parameter deleted)", + constants::kDefaultTolerancePercent, topic_name_.c_str()); + } + } + } + } + + 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; + } else if (param.name == enable_node_time_param_name_) { + diagnostics_config_.enable_node_time_diagnostics = param.value.bool_value; + } else if (param.name == enable_msg_time_param_name_) { + diagnostics_config_.enable_msg_time_diagnostics = param.value.bool_value; + } else if (param.name == enable_increasing_msg_time_param_name_) { + diagnostics_config_.enable_increasing_msg_time_diagnostics = param.value.bool_value; + } + } + + 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()); + } + return std::nullopt; + } + + void tryUndeclareParameter(const std::string & param_name) + { + try { + if (node_.has_parameter(param_name)) { + node_.undeclare_parameter(param_name); + } + } catch (const std::exception & e) { + RCLCPP_WARN( + node_.get_logger(), "Could not undeclare %s: %s", + param_name.c_str(), e.what()); + } + } + + 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(), + "Iniital 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()); + } + } + + void undeclareParameters() + { + tryUndeclareParameter(freq_param_name_); + tryUndeclareParameter(tol_param_name_); + } + + // Parameter names for this topic + std::string freq_param_name_; + std::string tol_param_name_; + std::string enabled_param_name_; + std::string enable_node_time_param_name_; + std::string enable_msg_time_param_name_; + std::string enable_increasing_msg_time_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 20a3237..57a37f7 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -20,21 +20,17 @@ #include #include #include -#include +#include #include -#include #include #include "rclcpp/rclcpp.hpp" -#include "rcl_interfaces/msg/parameter_event.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" #include "diagnostic_msgs/msg/key_value.hpp" -#include "message_diagnostics.hpp" +#include "greenwave_diagnostics.hpp" #include "greenwave_monitor_interfaces/srv/manage_topic.hpp" -#include "greenwave_monitor_interfaces/srv/set_expected_frequency.hpp" class GreenwaveMonitor : public rclcpp::Node { @@ -42,15 +38,6 @@ class GreenwaveMonitor : public rclcpp::Node explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); private: - struct TopicConfig - { - std::optional expected_frequency; - std::optional tolerance; - }; - - std::optional find_topic_type_with_retry( - const std::string & topic, const int max_retries, const int retry_period_s); - void topic_callback( const std::shared_ptr msg, const std::string & topic, const std::string & type); @@ -61,60 +48,25 @@ class GreenwaveMonitor : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - void handle_set_expected_frequency( - const std::shared_ptr request, - std::shared_ptr response); - - bool set_topic_expected_frequency( - const std::string & topic_name, - double expected_hz, - double tolerance_percent, - bool add_topic_if_missing, - std::string & message, - bool update_parameters = true); - - // Callback for this nodes parameter changes + additions - rcl_interfaces::msg::SetParametersResult on_parameter_change( - const std::vector & parameters); - - // Callback for handling deletions of parameters (across all nodes) - void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg); - - // Read all parameters from node at startup and ensure they are applied. on_parameter_change - // is not called at startup. - void load_topic_parameters_from_overrides(); - - std::optional get_numeric_parameter(const std::string & param_name); - - void try_undeclare_parameter(const std::string & param_name); - - void declare_or_set_parameter(const std::string & param_name, double value); - - void undeclare_topic_parameters(const std::string & topic_name); - - bool add_topic(const std::string & topic, std::string & message); + bool 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::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - rclcpp::Subscription::SharedPtr param_event_subscription_; - - // Flag to skip parameter callback when updating params internally (avoids redundant work) - bool updating_params_internally_ = false; }; diff --git a/greenwave_monitor/include/minimal_publisher_node.hpp b/greenwave_monitor/include/minimal_publisher_node.hpp index fb3a38c..f6e2a43 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; @@ -41,7 +41,7 @@ class MinimalPublisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; rclcpp::PublisherBase::SharedPtr publisher_; rclcpp::SubscriptionBase::SharedPtr subscription_; - std::unique_ptr message_diagnostics_; + std::unique_ptr greenwave_diagnostics_; size_t count_; std::string message_type_; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 79ac429..22215a0 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -16,84 +16,11 @@ // SPDX-License-Identifier: Apache-2.0 #include "greenwave_monitor.hpp" - -#include - #include "rcl_interfaces/msg/parameter_descriptor.hpp" -#include "rcl_interfaces/srv/list_parameters.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" using namespace std::chrono_literals; -namespace -{ -constexpr const char * kTopicParamPrefix = "topics."; -constexpr const char * kFreqSuffix = ".expected_frequency"; -constexpr const char * kTolSuffix = ".tolerance"; -constexpr double kDefaultTolerancePercent = 5.0; - -std::string make_freq_param_name(const std::string & topic_name) -{ - return std::string(kTopicParamPrefix) + topic_name + kFreqSuffix; -} - -std::string make_tol_param_name(const std::string & topic_name) -{ - return std::string(kTopicParamPrefix) + topic_name + kTolSuffix; -} - -enum class TopicParamField { kNone, kFrequency, kTolerance }; - -struct TopicParamInfo -{ - std::string topic_name; - TopicParamField field = TopicParamField::kNone; -}; - -// Parse a parameter name like "topics./my_topic.expected_frequency" into topic name and field type -TopicParamInfo parse_topic_param_name(const std::string & param_name) -{ - TopicParamInfo info; - - if (param_name.rfind(kTopicParamPrefix, 0) != 0) { - return info; - } - - std::string topic_and_field = param_name.substr(strlen(kTopicParamPrefix)); - - const size_t freq_suffix_len = strlen(kFreqSuffix); - const size_t tol_suffix_len = strlen(kTolSuffix); - const size_t len = topic_and_field.length(); - - bool is_freq = len > freq_suffix_len && - topic_and_field.rfind(kFreqSuffix) == len - freq_suffix_len; - bool is_tol = len > tol_suffix_len && - topic_and_field.rfind(kTolSuffix) == len - tol_suffix_len; - - if (is_freq) { - info.topic_name = topic_and_field.substr(0, len - freq_suffix_len); - info.field = TopicParamField::kFrequency; - } else if (is_tol) { - info.topic_name = topic_and_field.substr(0, len - tol_suffix_len); - info.field = TopicParamField::kTolerance; - } - - return info; -} - -// Convert a parameter to double if it's a numeric type -std::optional param_to_double(const rclcpp::Parameter & param) -{ - if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { - return param.as_double(); - } else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { - return static_cast(param.as_int()); - } - return std::nullopt; -} - -} // namespace - GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) : Node("greenwave_monitor", rclcpp::NodeOptions(options) @@ -104,15 +31,12 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) // Get the topics parameter (declare only if not already declared from overrides) if (!this->has_parameter("topics")) { - this->declare_parameter>("topics", {""}); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + this->declare_parameter>("topics", {""}, descriptor); } - 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(); + auto topics = this->get_parameter("topics").as_string_array(); for (const auto & topic : topics) { if (!topic.empty()) { std::string message; @@ -120,17 +44,14 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) } } - // Register parameter callback for dynamic topic configuration - param_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&GreenwaveMonitor::on_parameter_change, this, std::placeholders::_1)); - - // Subscribe to parameter events to handle parameter deletions - param_event_subscription_ = this->create_subscription( - "/parameter_events", 10, - std::bind(&GreenwaveMonitor::on_parameter_event, this, std::placeholders::_1)); - - // Process any topic parameters passed at startup - load_topic_parameters_from_overrides(); + // Also check for topics defined via greenwave_diagnostics..* parameters + auto topics_from_params = get_topics_from_parameters(); + for (const auto & topic : topics_from_params) { + if (greenwave_diagnostics_.find(topic) == greenwave_diagnostics_.end()) { + std::string message; + add_topic(topic, message); + } + } timer_ = this->create_wall_timer( 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); @@ -142,29 +63,6 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) 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)); -} - -std::optional GreenwaveMonitor::find_topic_type_with_retry( - const std::string & topic, const int max_retries, const int retry_period_s) -{ - 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]; - } - } - return std::nullopt; } void GreenwaveMonitor::topic_callback( @@ -172,16 +70,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", @@ -198,340 +96,13 @@ void GreenwaveMonitor::handle_manage_topic( std::shared_ptr response) { if (request->add_topic) { - response->success = add_topic(request->topic_name, response->message); + // No retries for service calls - caller can retry if needed + response->success = add_topic(request->topic_name, response->message, 0); } else { response->success = remove_topic(request->topic_name, response->message); } } -void GreenwaveMonitor::handle_set_expected_frequency( - const std::shared_ptr request, - std::shared_ptr response) -{ - auto it = message_diagnostics_.find(request->topic_name); - - if (it == message_diagnostics_.end()) { - if (!request->add_topic_if_missing) { - response->success = false; - response->message = "Failed to find topic"; - return; - } - - if (!add_topic(request->topic_name, response->message)) { - response->success = false; - return; - } - it = message_diagnostics_.find(request->topic_name); - } - - message_diagnostics::MessageDiagnostics & msg_diagnostics_obj = *(it->second); - - if (request->clear_expected) { - msg_diagnostics_obj.clearExpectedDt(); - undeclare_topic_parameters(request->topic_name); - - response->success = true; - response->message = "Successfully cleared expected frequency for topic '" + - request->topic_name + "'"; - return; - } - - response->success = set_topic_expected_frequency( - request->topic_name, - request->expected_hz, - request->tolerance_percent, - false, // topic already exists at this point - response->message); -} - -bool GreenwaveMonitor::set_topic_expected_frequency( - const std::string & topic_name, - double expected_hz, - double tolerance_percent, - bool add_topic_if_missing, - std::string & message, - bool update_parameters) -{ - if (expected_hz <= 0.0) { - message = "Invalid expected frequency, must be set to a positive value"; - return false; - } - if (tolerance_percent < 0.0) { - message = "Invalid tolerance, must be a non-negative percentage"; - return false; - } - - auto it = message_diagnostics_.find(topic_name); - - if (it == message_diagnostics_.end()) { - if (!add_topic_if_missing) { - message = "Failed to find topic '" + topic_name + "'"; - return false; - } - - if (!add_topic(topic_name, message)) { - return false; - } - it = message_diagnostics_.find(topic_name); - } - - message_diagnostics::MessageDiagnostics & msg_diagnostics_obj = *(it->second); - msg_diagnostics_obj.setExpectedDt(expected_hz, tolerance_percent); - - // Sync parameters with the new values - if (update_parameters) { - updating_params_internally_ = true; - try { - declare_or_set_parameter(make_freq_param_name(topic_name), expected_hz); - declare_or_set_parameter(make_tol_param_name(topic_name), tolerance_percent); - } catch (const std::exception & e) { - message = "Could not set parameters for topic '" + topic_name + "': " + e.what(); - updating_params_internally_ = false; - return false; - } - updating_params_internally_ = false; - } - - message = "Successfully set expected frequency for topic '" + - topic_name + "' to " + std::to_string(expected_hz) + - " hz with tolerance " + std::to_string(tolerance_percent) + "%"; - return true; -} - -rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - // Skip if updating from within the node (avoids redundant work and deadlock) - if (updating_params_internally_) { - return result; - } - - // Collect validation errors and valid configs - std::vector errors; - std::map incoming_configs; - - // Construct expected frequency and tolerance pairs from parameter changes - for (const auto & param : parameters) { - auto info = parse_topic_param_name(param.get_name()); - if (info.field == TopicParamField::kNone || info.topic_name.empty()) { - continue; - } - - // Allow PARAMETER_NOT_SET for parameter deletion - if (param.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { - continue; - } - - auto value_opt = param_to_double(param); - if (!value_opt.has_value()) { - errors.push_back(param.get_name() + ": must be a numeric type (int or double)"); - continue; - } - - double value = value_opt.value(); - TopicConfig & config = incoming_configs[info.topic_name]; - - if (info.field == TopicParamField::kFrequency) { - if (value <= 0.0) { - errors.push_back( - param.get_name() + ": Invalid frequency, must be positive"); - continue; - } - config.expected_frequency = value; - } else { - if (value < 0.0) { - errors.push_back( - param.get_name() + ": Invalid tolerance, must be non-negative"); - continue; - } - config.tolerance = value; - } - } - - // Iterate over incoming configs and set expected frequencies/tolerances - for (const auto & [topic_name, incoming] : incoming_configs) { - // Get expected frequency: prefer incoming, fall back to existing parameter - double expected_freq = 0.0; - if (incoming.expected_frequency.has_value()) { - expected_freq = incoming.expected_frequency.value(); - } else { - auto freq_opt = get_numeric_parameter(make_freq_param_name(topic_name)); - if (freq_opt.has_value()) { - expected_freq = freq_opt.value(); - } else { - // Tolerance set without frequency - nothing to apply yet - continue; - } - } - - // Get tolerance: prefer incoming, then existing parameter, then default - double tolerance = incoming.tolerance.value_or( - get_numeric_parameter(make_tol_param_name(topic_name)).value_or(kDefaultTolerancePercent) - ); - - std::string message; - bool success = set_topic_expected_frequency( - topic_name, - expected_freq, - tolerance, - true, - message, - false); // don't update parameters - called from parameter change - - // Log warning if the topic is not up yet or an error occurs while trying to monitor it - // Still errors if parameter is invalid value since that is redundantly checked earlier - if (!success) { - RCLCPP_WARN( - this->get_logger(), - "Could not apply monitoring config for topic '%s': %s", - topic_name.c_str(), message.c_str()); - } - } - - if (!errors.empty()) { - result.successful = false; - result.reason = "Invalid parameters: " + rcpputils::join(errors, ", "); - } - - return result; -} - -void GreenwaveMonitor::on_parameter_event( - const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) -{ - // Only process events from this node - if (msg->node != this->get_fully_qualified_name()) { - return; - } - - for (const auto & param : msg->deleted_parameters) { - auto info = parse_topic_param_name(param.name); - if (info.field == TopicParamField::kNone || info.topic_name.empty()) { - continue; - } - - auto it = message_diagnostics_.find(info.topic_name); - if (it == message_diagnostics_.end()) { - continue; - } - - if (info.field == TopicParamField::kFrequency) { - it->second->clearExpectedDt(); - RCLCPP_DEBUG( - this->get_logger(), - "Cleared expected frequency for topic '%s' (parameter deleted)", - info.topic_name.c_str()); - } else if (info.field == TopicParamField::kTolerance) { - // Reset tolerance to default if frequency is still set - auto freq_opt = get_numeric_parameter(make_freq_param_name(info.topic_name)); - if (freq_opt.has_value() && freq_opt.value() > 0) { - it->second->setExpectedDt(freq_opt.value(), kDefaultTolerancePercent); - RCLCPP_DEBUG( - this->get_logger(), - "Reset tolerance to default (%.1f%%) for topic '%s' (parameter deleted)", - kDefaultTolerancePercent, info.topic_name.c_str()); - } - } - } -} - -void GreenwaveMonitor::load_topic_parameters_from_overrides() -{ - // Parameters are automatically declared from overrides due to NodeOptions setting. - // List all parameters and filter by prefix manually (list_parameters prefix matching - // can be unreliable with deeply nested parameter names). - auto all_params = this->list_parameters( - {}, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); - - // Build a local map of topic configs from startup parameters - std::map startup_configs; - - // Construct expected frequency and tolerance pairs from startup parameters - for (const auto & name : all_params.names) { - auto info = parse_topic_param_name(name); - if (info.field == TopicParamField::kNone || info.topic_name.empty()) { - continue; - } - - auto value_opt = get_numeric_parameter(name); - if (!value_opt.has_value()) { - continue; - } - - double value = value_opt.value(); - TopicConfig & config = startup_configs[info.topic_name]; - - if (info.field == TopicParamField::kFrequency) { - config.expected_frequency = value; - } else { - config.tolerance = value; - } - } - - // Iterate over starting config and add topics/set expected frequencies - for (const auto & [topic, config] : startup_configs) { - // Topics will only be added if frequency is set - if (config.expected_frequency.has_value()) { - double tolerance = config.tolerance.value_or(kDefaultTolerancePercent); - - std::string message; - bool success = set_topic_expected_frequency( - topic, - config.expected_frequency.value(), - tolerance, - true, // add topic if missing - safe at startup - message, - false); // don't update parameters - - if (!success) { - RCLCPP_WARN(this->get_logger(), "%s", message.c_str()); - } - } - } -} - -std::optional GreenwaveMonitor::get_numeric_parameter(const std::string & param_name) -{ - if (!this->has_parameter(param_name)) { - return std::nullopt; - } - return param_to_double(this->get_parameter(param_name)); -} - -void GreenwaveMonitor::try_undeclare_parameter(const std::string & param_name) -{ - try { - if (this->has_parameter(param_name)) { - this->undeclare_parameter(param_name); - } - } catch (const std::exception & e) { - RCLCPP_WARN( - this->get_logger(), "Could not undeclare %s: %s", - param_name.c_str(), e.what()); - } -} - -void GreenwaveMonitor::declare_or_set_parameter(const std::string & param_name, double value) -{ - if (!this->has_parameter(param_name)) { - // Allow both integer and double types for numeric parameters - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.dynamic_typing = true; - this->declare_parameter(param_name, value, descriptor); - } else { - this->set_parameter(rclcpp::Parameter(param_name, value)); - } -} - -void GreenwaveMonitor::undeclare_topic_parameters(const std::string & topic_name) -{ - try_undeclare_parameter(make_freq_param_name(topic_name)); - try_undeclare_parameter(make_tol_param_name(topic_name)); -} - bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) { // We use a cache to avoid repeated lookups for the same message type. @@ -617,26 +188,122 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & type_name) return has_header; } -bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & message) +bool GreenwaveMonitor::add_topic( + const std::string & topic, std::string & message, + int max_retries, double retry_period_s) { // Check if topic already exists - if (message_diagnostics_.find(topic) != message_diagnostics_.end()) { + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { message = "Topic already being monitored"; return false; } RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); - const int max_retries = 5; - const int retry_period_s = 1; - auto maybe_type = find_topic_type_with_retry(topic, max_retries, retry_period_s); - 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"; + // Get publishers for this topic with retry logic + std::vector publishers; + for (int attempt = 0; attempt <= max_retries; ++attempt) { + publishers = this->get_publishers_info_by_topic(topic); + 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()) { + RCLCPP_ERROR(this->get_logger(), "Failed to find publishers for topic '%s'", topic.c_str()); + message = "Failed to find publishers for topic"; return false; } - const std::string type = maybe_type.value(); + // Get the message type from the first publisher + const std::string type = publishers[0].topic_type(); + + // Check if any publisher node already has the enabled parameter for this topic + std::string enabled_param_name = + std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + + topic + greenwave_diagnostics::constants::kEnabledSuffix; + + // Try to find an existing node with the enabled parameter + // We use list_parameters via a temporary node to avoid executor deadlock + try { + // Get our own node's full name to skip self-reference + std::string our_ns = this->get_namespace(); + std::string our_name = this->get_name(); + std::string our_full_name = (our_ns == "/") ? + ("/" + our_name) : (our_ns + "/" + our_name); + + // Create a single temporary node for parameter operations + static std::atomic 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_param_" + std::to_string(temp_node_counter++), + "/_greenwave_internal", + temp_options); + + for (const auto & pub_info : publishers) { + std::string node_name = pub_info.node_name(); + std::string node_namespace = pub_info.node_namespace(); + std::string full_node_name = (node_namespace == "/") ? + ("/" + node_name) : (node_namespace + "/" + node_name); + + // Skip if this is our own node (avoid self-reference) + if (full_node_name == our_full_name) { + continue; + } + + auto param_client = std::make_shared( + temp_node, full_node_name); + if (!param_client->wait_for_service(std::chrono::milliseconds(500))) { + continue; + } + + if (param_client->has_parameter(enabled_param_name)) { + // Check if already enabled - can't add twice + auto current_params = param_client->get_parameters({enabled_param_name}); + if (!current_params.empty() && + current_params[0].get_type() == rclcpp::ParameterType::PARAMETER_BOOL && + current_params[0].as_bool()) + { + message = "Topic already being monitored on node: " + full_node_name; + return false; + } + + auto results = param_client->set_parameters({ + rclcpp::Parameter(enabled_param_name, true) + }); + if (!results.empty() && results[0].successful) { + RCLCPP_INFO( + this->get_logger(), + "Enabled monitoring via parameter on node '%s' for topic '%s'", + full_node_name.c_str(), topic.c_str()); + message = "Enabled monitoring on existing node: " + full_node_name; + return true; + } + } + } + // Explicitly clear temp_node before proceeding to avoid any lingering async operations + temp_node.reset(); + } catch (const std::exception & e) { + RCLCPP_WARN( + this->get_logger(), + "Exception while checking for existing monitoring on topic '%s': %s", + topic.c_str(), e.what()); + } + + // Small delay to ensure any async cleanup from temp_node is complete + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + // No existing node with the parameter found, create local GreenwaveDiagnostics auto sub = this->create_generic_subscription( topic, type, @@ -646,21 +313,14 @@ 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)); - - // If parameters are set, use them to set the expected frequency and tolerance - auto freq_opt = get_numeric_parameter(make_freq_param_name(topic)); - auto tol_opt = get_numeric_parameter(make_tol_param_name(topic)); - double tolerance = tol_opt.value_or(kDefaultTolerancePercent); - if (freq_opt.has_value() && tolerance >= 0.0 && freq_opt.value() > 0.0) { - message_diagnostics_[topic]->setExpectedDt(freq_opt.value(), tolerance); - } + std::make_unique(*this, topic, + diagnostics_config)); message = "Successfully added topic"; return true; @@ -668,9 +328,81 @@ bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & messag bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & message) { - auto diag_it = message_diagnostics_.find(topic); - if (diag_it == message_diagnostics_.end()) { - message = "Topic not found"; + auto diag_it = greenwave_diagnostics_.find(topic); + if (diag_it == greenwave_diagnostics_.end()) { + // Topic not monitored locally, try to find a publisher node with the enabled parameter + std::string enabled_param_name = + std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + + topic + greenwave_diagnostics::constants::kEnabledSuffix; + + auto publishers = this->get_publishers_info_by_topic(topic); + + try { + // Get our own node's full name to skip self-reference + std::string our_ns = this->get_namespace(); + std::string our_name = this->get_name(); + std::string our_full_name = (our_ns == "/") ? + ("/" + our_name) : (our_ns + "/" + our_name); + + // Create a single temporary node for parameter operations + static std::atomic 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_param_" + std::to_string(temp_node_counter++), + "/_greenwave_internal", + temp_options); + + for (const auto & pub_info : publishers) { + std::string node_name = pub_info.node_name(); + std::string node_namespace = pub_info.node_namespace(); + std::string full_node_name = (node_namespace == "/") ? + ("/" + node_name) : (node_namespace + "/" + node_name); + + // Skip if this is our own node + if (full_node_name == our_full_name) { + continue; + } + + auto param_client = std::make_shared( + temp_node, full_node_name); + if (!param_client->wait_for_service(std::chrono::milliseconds(500))) { + continue; + } + + if (param_client->has_parameter(enabled_param_name)) { + // Check if already disabled - can't remove twice + auto current_params = param_client->get_parameters({enabled_param_name}); + if (!current_params.empty() && + current_params[0].get_type() == rclcpp::ParameterType::PARAMETER_BOOL && + !current_params[0].as_bool()) + { + message = "Topic already disabled on node: " + full_node_name; + return false; + } + + auto results = param_client->set_parameters({ + rclcpp::Parameter(enabled_param_name, false) + }); + if (!results.empty() && results[0].successful) { + RCLCPP_INFO( + this->get_logger(), + "Disabled monitoring via parameter on node '%s' for topic '%s'", + full_node_name.c_str(), topic.c_str()); + message = "Disabled monitoring on existing node: " + full_node_name; + return true; + } + } + } + } catch (const std::exception & e) { + RCLCPP_WARN( + this->get_logger(), + "Exception while checking for existing monitoring on topic '%s': %s", + topic.c_str(), e.what()); + } + + message = "No parameter " + enabled_param_name + " found on global parameter server"; return false; } @@ -685,15 +417,48 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes subscriptions_.erase(sub_it); } - message_diagnostics_.erase(diag_it); - - // Clear any associated parameters - undeclare_topic_parameters(topic); + // 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; } +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); + } + } + + return 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/minimal_publisher_node.cpp b/greenwave_monitor/src/minimal_publisher_node.cpp index e7de807..8648604 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,9 @@ void MinimalPublisher::timer_callback() message); } - const auto msg_timestamp = this->now(); - message_diagnostics_->updateDiagnostics(msg_timestamp.nanoseconds()); - // message_diagnostics_->publishDiagnostics(); + if (greenwave_diagnostics_) { + const auto msg_timestamp = this->now(); + greenwave_diagnostics_->updateDiagnostics(msg_timestamp.nanoseconds()); + greenwave_diagnostics_->publishDiagnostics(); + } } diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 63903f6..5b1c081 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -56,11 +56,13 @@ def generate_test_description(): ) publisher_set_params = create_minimal_publisher( - TEST_TOPIC_SET_PARAMS, TEST_FREQUENCY, 'imu', '_set_params' + TEST_TOPIC_SET_PARAMS, TEST_FREQUENCY, 'imu', '_set_params', + enable_diagnostics=False ) publisher_delete_param = create_minimal_publisher( - TEST_TOPIC_DELETE_PARAM, TEST_FREQUENCY, 'imu', '_delete_param' + TEST_TOPIC_DELETE_PARAM, TEST_FREQUENCY, 'imu', '_delete_param', + enable_diagnostics=False ) return ( diff --git a/greenwave_monitor/test/parameters/test_param_enable_existing.py b/greenwave_monitor/test/parameters/test_param_enable_existing.py new file mode 100644 index 0000000..3d5fdcb --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_enable_existing.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: add_topic enables existing node's parameter instead of creating local diagnostics.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + call_manage_topic_service, + create_minimal_publisher, + create_monitor_node, + create_service_clients, + RosNodeTestCase, + wait_for_service_connection, +) +from greenwave_monitor.ui_adaptor import ENABLED_SUFFIX, TOPIC_PARAM_PREFIX +import launch +import launch_testing +import pytest +from rcl_interfaces.srv import GetParameters +import rclpy + + +TEST_TOPIC = '/enable_existing_test_topic' +TEST_FREQUENCY = 50.0 +PUBLISHER_NODE_NAME = 'minimal_publisher_node_enable_test' + + +def make_enabled_param(topic: str) -> str: + """Build enabled parameter name for a topic.""" + return f'{TOPIC_PARAM_PREFIX}{topic}{ENABLED_SUFFIX}' + + +@pytest.mark.launch_test +def generate_test_description(): + """Launch monitor and publisher nodes for testing.""" + ros2_monitor_node = create_monitor_node(topics=['']) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_enable_test' + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestEnableExistingNodeParameter(RosNodeTestCase): + """Test that add_topic enables parameter on existing nodes.""" + + TEST_NODE_NAME = 'enable_existing_test_node' + + def test_add_topic_enables_existing_node_parameter(self): + """Test that adding a topic with an existing publisher sets the enabled parameter.""" + time.sleep(3.0) + + publisher_full_name = f'/{PUBLISHER_NODE_NAME}' + + # Create a client to get parameters from the publisher node + 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 on publisher node' + ) + + enabled_param_name = make_enabled_param(TEST_TOPIC) + + # Verify the publisher node has the 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(), 'Failed to get parameters from publisher') + self.assertTrue( + len(future.result().values) > 0, + f'Publisher node should have parameter {enabled_param_name}' + ) + + # Create service client for manage_topic + manage_topic_client = create_service_clients(self.test_node) + self.assertTrue( + wait_for_service_connection( + self.test_node, manage_topic_client, + timeout_sec=5.0, service_name='manage_topic' + ), + 'ManageTopic service not available' + ) + + # Call add_topic for the test topic + response = call_manage_topic_service( + self.test_node, manage_topic_client, add=True, topic=TEST_TOPIC + ) + self.assertIsNotNone(response, 'ManageTopic service call failed') + self.assertTrue(response.success, f'Failed to add topic: {response.message}') + + # Verify the response message indicates it enabled monitoring on existing node + self.assertIn( + 'Enabled monitoring on existing node', + response.message, + f'Expected message about enabling existing node, got: {response.message}' + ) + + # Verify the enabled parameter is now true on the publisher node + 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(), 'Failed to get parameters after add_topic') + self.assertTrue( + len(future.result().values) > 0, + 'Parameter should still exist after add_topic' + ) + self.assertTrue( + future.result().values[0].bool_value, + 'Enabled parameter should be true after add_topic' + ) + + # Cleanup + self.test_node.destroy_client(get_params_client) + self.test_node.destroy_client(manage_topic_client) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py index 3a7c049..90895cc 100644 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ b/greenwave_monitor/test/parameters/test_param_yaml.py @@ -54,10 +54,10 @@ def generate_test_description(): f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n' f' ros__parameters:\n' f' # Flat dotted key format (requires quotes)\n' - f' "topics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n' - f' "topics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n' + f' "greenwave_diagnostics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n' + f' "greenwave_diagnostics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n' f' # Nested dictionary format\n' - f' topics:\n' + f' greenwave_diagnostics:\n' f' {NESTED_TOPIC}:\n' f' expected_frequency: {NESTED_FREQUENCY}\n' f' tolerance: {TEST_TOLERANCE}\n' diff --git a/greenwave_monitor/test/test_message_diagnostics.cpp b/greenwave_monitor/test/test_greenwave_diagnostics.cpp similarity index 77% rename from greenwave_monitor/test/test_message_diagnostics.cpp rename to greenwave_monitor/test/test_greenwave_diagnostics.cpp index df3e0dc..6b158ae 100644 --- a/greenwave_monitor/test/test_message_diagnostics.cpp +++ b/greenwave_monitor/test/test_greenwave_diagnostics.cpp @@ -16,7 +16,7 @@ // SPDX-License-Identifier: Apache-2.0 /** -Unit tests for functionality in message_diagnostics.hpp, +Unit tests for functionality in greenwave_diagnostics.hpp, such as frame rate and latency calculation accuracy. **/ @@ -28,7 +28,7 @@ such as frame rate and latency calculation accuracy. #include #include -#include "message_diagnostics.hpp" +#include "greenwave_diagnostics.hpp" namespace test_constants { @@ -36,7 +36,7 @@ inline constexpr uint64_t kMillisecondsToSeconds = 1000ULL; inline constexpr uint64_t kStartTimestampNs = 10000000ULL; } // namespace test_constants -class MessageDiagnosticsTest : public ::testing::Test +class GreenwaveDiagnosticsTest : public ::testing::Test { protected: static void SetUpTestSuite() @@ -62,25 +62,25 @@ class MessageDiagnosticsTest : public ::testing::Test std::shared_ptr node_; }; -TEST_F(MessageDiagnosticsTest, FrameRateMsgTest) +TEST_F(GreenwaveDiagnosticsTest, FrameRateMsgTest) { - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); uint64_t timestamp = test_constants::kStartTimestampNs; // in nanoseconds for (int i = 0; i < 1000; i++) { - message_diagnostics.updateDiagnostics(timestamp); + greenwave_diagnostics.updateDiagnostics(timestamp); timestamp += 10000000; // 10 ms in nanoseconds } - EXPECT_EQ(message_diagnostics.getFrameRateMsg(), 100); // 100 Hz + EXPECT_EQ(greenwave_diagnostics.getFrameRateMsg(), 100); // 100 Hz } -TEST_F(MessageDiagnosticsTest, FrameRateNodeTest) +TEST_F(GreenwaveDiagnosticsTest, FrameRateNodeTest) { - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); // dummy timestamp, not used for node time calculation constexpr auto timestamp = test_constants::kStartTimestampNs; @@ -90,7 +90,7 @@ TEST_F(MessageDiagnosticsTest, FrameRateNodeTest) constexpr int interarrival_time_ms = 10; // 100 hz for (int i = 0; i < num_messages; i++) { - message_diagnostics.updateDiagnostics(timestamp); + greenwave_diagnostics.updateDiagnostics(timestamp); std::this_thread::sleep_for(std::chrono::milliseconds(interarrival_time_ms)); } @@ -100,14 +100,14 @@ TEST_F(MessageDiagnosticsTest, FrameRateNodeTest) const double expected_frame_rate = static_cast(num_messages) / total_duration.count(); // allow 2.0 Hz error - EXPECT_NEAR(message_diagnostics.getFrameRateNode(), expected_frame_rate, 2.0); + EXPECT_NEAR(greenwave_diagnostics.getFrameRateNode(), expected_frame_rate, 2.0); } -TEST_F(MessageDiagnosticsTest, MessageLatencyTest) +TEST_F(GreenwaveDiagnosticsTest, MessageLatencyTest) { - // Initialize MessageDiagnostics - message_diagnostics::MessageDiagnostics message_diagnostics( - *node_, "test_topic", message_diagnostics::MessageDiagnosticsConfig()); + // Initialize GreenwaveDiagnostics + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); const rclcpp::Time current_time = node_->get_clock()->now(); // Make message timestamp a certain amount of time earlier than current time @@ -116,28 +116,29 @@ TEST_F(MessageDiagnosticsTest, MessageLatencyTest) current_time - rclcpp::Duration::from_seconds( expected_latency_ms / static_cast(test_constants::kMillisecondsToSeconds)); - message_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); + greenwave_diagnostics.updateDiagnostics(msg_timestamp.nanoseconds()); - EXPECT_NEAR(message_diagnostics.getLatency(), expected_latency_ms, 1.0); // allow 1 ms tolerance + // allow 1 ms tolerance + EXPECT_NEAR(greenwave_diagnostics.getLatency(), expected_latency_ms, 1.0); } -TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) +TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) { constexpr int input_frequency = 50; // 50 Hz // 20 ms in nanoseconds const int64_t interarrival_time_ns = static_cast( - ::message_diagnostics::constants::kSecondsToNanoseconds / input_frequency); + ::greenwave_diagnostics::constants::kSecondsToNanoseconds / input_frequency); - // Initialize MessageDiagnostics with diagnostics enabled - message_diagnostics::MessageDiagnosticsConfig config; + // Initialize GreenwaveDiagnostics with diagnostics enabled + greenwave_diagnostics::GreenwaveDiagnosticsConfig config; config.enable_msg_time_diagnostics = true; config.enable_node_time_diagnostics = true; config.enable_increasing_msg_time_diagnostics = true; // in us config.expected_dt_us = interarrival_time_ns / - ::message_diagnostics::constants::kMicrosecondsToNanoseconds; + ::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds; - message_diagnostics::MessageDiagnostics message_diagnostics(*node_, "test_topic", config); + greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics(*node_, "test_topic", config); // Create a subscriber to receive diagnostic messages std::vector received_diagnostics; @@ -150,8 +151,8 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) // 50 ms delay constexpr int64_t delay_time_ns = 50 * - static_cast(::message_diagnostics::constants::kMillisecondsToMicroseconds) * - static_cast(::message_diagnostics::constants::kMicrosecondsToNanoseconds); + static_cast(::greenwave_diagnostics::constants::kMillisecondsToMicroseconds) * + static_cast(::greenwave_diagnostics::constants::kMicrosecondsToNanoseconds); // Starting message timestamp in nanoseconds auto msg_timestamp = test_constants::kStartTimestampNs; @@ -167,8 +168,8 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) sent_count++; - message_diagnostics.updateDiagnostics(msg_timestamp); - message_diagnostics.publishDiagnostics(); + greenwave_diagnostics.updateDiagnostics(msg_timestamp); + greenwave_diagnostics.publishDiagnostics(); // Add a non-increasing timestamp at count 5 if (sent_count == 5) { @@ -196,7 +197,7 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) const auto sum_interarrival_time_msg_sec = static_cast( msg_timestamp - test_constants::kStartTimestampNs) / - static_cast(::message_diagnostics::constants::kSecondsToNanoseconds); + static_cast(::greenwave_diagnostics::constants::kSecondsToNanoseconds); const double expected_frame_rate_msg = static_cast(interarrival_time_count) / sum_interarrival_time_msg_sec; @@ -234,11 +235,11 @@ TEST_F(MessageDiagnosticsTest, DiagnosticPublishSubscribeTest) // Sometimes diagnostics may arrive out of order, so we use getter methods instead of values from // the last diagnostic message to prevent flakiness - EXPECT_NEAR(message_diagnostics.getFrameRateNode(), expected_frame_rate_node, 1.0); + EXPECT_NEAR(greenwave_diagnostics.getFrameRateNode(), expected_frame_rate_node, 1.0); // Allow small floating point differences for frame rate msg constexpr double frame_rate_msg_tolerance = 0.001; EXPECT_NEAR( - message_diagnostics.getFrameRateMsg(), expected_frame_rate_msg, frame_rate_msg_tolerance); + greenwave_diagnostics.getFrameRateMsg(), expected_frame_rate_msg, frame_rate_msg_tolerance); // Sometimes diagnostics may arrive out of order, so we need to check all received diagnostics // to see if the expected msg frame rate is somewhere in there diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 2c49e2d..2ecbb72 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -59,9 +59,12 @@ def generate_test_description(message_type, expected_frequency, tolerance_hz): publishers = [ # Main test topic publisher with parametrized frequency create_minimal_publisher('/test_topic', expected_frequency, message_type), - # Additional publishers for topic management tests - create_minimal_publisher('/test_topic1', expected_frequency, message_type, '1'), - create_minimal_publisher('/test_topic2', expected_frequency, message_type, '2') + # Additional publishers for topic management tests (start with diagnostics disabled + # so add_topic can enable them) + 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 = { @@ -119,7 +122,7 @@ 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( + manage_client = create_service_clients( self.test_node, MONITOR_NODE_NAMESPACE, MONITOR_NODE_NAME ) service_available = wait_for_service_connection( @@ -188,7 +191,7 @@ def test_manage_one_topic(self, expected_frequency, message_type, tolerance_hz): add=False, topic=TEST_TOPIC, service_client=service_client) self.assertTrue(response.success) - # 2. Removing the same topic again should fail because it no longer exists. + # 2. Removing the same topic again should fail because it's already disabled. response = self.call_manage_topic( add=False, topic=TEST_TOPIC, service_client=service_client) self.assertFalse(response.success) @@ -201,7 +204,7 @@ def test_manage_one_topic(self, expected_frequency, message_type, tolerance_hz): # Verify diagnostics after adding the topic back self.verify_diagnostics(TEST_TOPIC, expected_frequency, message_type, tolerance_hz) - # 4. Adding the same topic again should fail because it's already monitored. + # 4. Adding the same topic again should fail because it's already enabled. response = self.call_manage_topic( add=True, topic=TEST_TOPIC, service_client=service_client) self.assertFalse(response.success) diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index 3aca226..61e14ce 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -140,10 +140,9 @@ def tearDown(self): self.test_node.destroy_subscription( self.diagnostics_monitor.param_events_subscription) self.test_node.destroy_client(self.diagnostics_monitor.manage_topic_client) - self.test_node.destroy_client( - self.diagnostics_monitor.set_expected_frequency_client) self.test_node.destroy_client(self.diagnostics_monitor.list_params_client) self.test_node.destroy_client(self.diagnostics_monitor.get_params_client) + self.test_node.destroy_client(self.diagnostics_monitor.set_params_client) except Exception: pass # Ignore cleanup errors @@ -155,17 +154,17 @@ def test_service_discovery_default_namespace( # The monitor should discover the services automatically self.assertIsNotNone(self.diagnostics_monitor.manage_topic_client) - self.assertIsNotNone(self.diagnostics_monitor.set_expected_frequency_client) + self.assertIsNotNone(self.diagnostics_monitor.set_params_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 + set_params_available = ( + self.diagnostics_monitor.set_params_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') + self.assertTrue(set_params_available, 'SetParameters service should be available') def test_diagnostic_data_conversion(self, expected_frequency, message_type, tolerance_hz): """Test conversion from DiagnosticStatus to UiDiagnosticData.""" diff --git a/greenwave_monitor_interfaces/CMakeLists.txt b/greenwave_monitor_interfaces/CMakeLists.txt index a3c2b46..b6714fd 100644 --- a/greenwave_monitor_interfaces/CMakeLists.txt +++ b/greenwave_monitor_interfaces/CMakeLists.txt @@ -27,7 +27,6 @@ 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/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 From cc67a224ff3eabe5665488e755b632ba68a80b4d Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Wed, 7 Jan 2026 15:31:57 -0800 Subject: [PATCH 19/33] Add parameter tests to test_greenwave_diagnostics Signed-off-by: Blake McHale --- .../greenwave_monitor/ui_adaptor.py | 17 +- .../include/greenwave_diagnostics.hpp | 111 +-- .../include/greenwave_monitor.hpp | 3 + greenwave_monitor/src/greenwave_monitor.cpp | 221 +++--- .../test/parameters/test_param_dynamic.py | 56 +- .../test/test_greenwave_diagnostics.cpp | 654 +++++++++++++++++- 6 files changed, 765 insertions(+), 297 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index e95037c..074b8d2 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -87,6 +87,10 @@ def param_value_to_float(value: ParameterValue) -> float | None: return value.double_value elif value.type == ParameterType.PARAMETER_INTEGER: return float(value.integer_value) + elif value.type == ParameterType.PARAMETER_STRING: + str_value = value.string_value + if str_value == 'nan' or str_value == 'NaN' or str_value == 'NAN': + return float('nan') return None @@ -383,10 +387,6 @@ def _on_diagnostics(self, msg: DiagnosticArray): 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) @@ -402,10 +402,8 @@ def _on_parameter_event(self, msg: ParameterEvent): if field == TopicParamField.FREQUENCY: # Treat NaN or non-positive as "cleared" - if value > 0 and not math.isnan(value): + if value > 0 or math.isnan(value): 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 @@ -457,9 +455,8 @@ def toggle_topic_monitoring(self, topic_name: str): return if not request.add_topic and topic_name in self.ui_diagnostics: + # Remove the topic from the UI diagnostics, but keep its settings del self.ui_diagnostics[topic_name] - if topic_name in self.expected_frequencies: - del self.expected_frequencies[topic_name] except Exception as e: action = 'start' if request.add_topic else 'stop' @@ -612,7 +609,7 @@ def get_expected_frequency(self, topic_name: str) -> tuple[float, float]: 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: + if freq <= 0.0 or math.isnan(freq): return '-' if tol > 0.0: return f'{freq:.1f}Hz±{tol:.0f}%' diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index 88274b4..b7fa3d1 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -56,9 +56,6 @@ 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 const char * kEnableNodeTimeSuffix = ".enable_node_time"; -inline constexpr const char * kEnableMsgTimeSuffix = ".enable_msg_time"; -inline constexpr const char * kEnableIncreasingMsgTimeSuffix = ".enable_increasing_msg_time"; inline constexpr double kDefaultTolerancePercent = 5.0; inline constexpr double kDefaultFrequencyHz = std::numeric_limits::quiet_NaN(); inline constexpr bool kDefaultEnabled = true; @@ -130,13 +127,6 @@ class GreenwaveDiagnostics constants::kTolSuffix; enabled_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + constants::kEnabledSuffix; - enable_node_time_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + - constants::kEnableNodeTimeSuffix; - enable_msg_time_param_name_ = std::string(constants::kTopicParamPrefix) + topic_name_ + - constants::kEnableMsgTimeSuffix; - enable_increasing_msg_time_param_name_ = std::string(constants::kTopicParamPrefix) + - topic_name_ + - constants::kEnableIncreasingMsgTimeSuffix; // Register parameter callback for this topic's parameters param_callback_handle_ = node_.add_on_set_parameters_callback( @@ -160,12 +150,6 @@ class GreenwaveDiagnostics if (!auto_declare) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.dynamic_typing = true; - node_.declare_parameter(enable_node_time_param_name_, - diagnostics_config_.enable_node_time_diagnostics); - node_.declare_parameter(enable_msg_time_param_name_, - diagnostics_config_.enable_msg_time_diagnostics); - node_.declare_parameter(enable_increasing_msg_time_param_name_, - diagnostics_config_.enable_increasing_msg_time_diagnostics); 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); @@ -173,12 +157,6 @@ class GreenwaveDiagnostics // 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(enable_node_time_param_name_, - diagnostics_config_.enable_node_time_diagnostics); - setParameterOrDefault(enable_msg_time_param_name_, - diagnostics_config_.enable_msg_time_diagnostics); - setParameterOrDefault(enable_increasing_msg_time_param_name_, - diagnostics_config_.enable_increasing_msg_time_diagnostics); setParameterOrDefault(enabled_param_name_, constants::kDefaultEnabled); setParameterOrDefault(freq_param_name_, default_freq); setParameterOrDefault(tol_param_name_, constants::kDefaultTolerancePercent); @@ -393,9 +371,9 @@ class GreenwaveDiagnostics (tolerance_percent / 100.0)); diagnostics_config_.jitter_tolerance_us = tolerance_us; - // Automatically enable node and msg time diagnostics when expected frequency is set - node_.set_parameter(rclcpp::Parameter(enable_node_time_param_name_, true)); - node_.set_parameter(rclcpp::Parameter(enable_msg_time_param_name_, true)); + // 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() @@ -405,8 +383,8 @@ class GreenwaveDiagnostics diagnostics_config_.jitter_tolerance_us = 0; // Disable node and msg time diagnostics when expected frequency is cleared - node_.set_parameter(rclcpp::Parameter(enable_node_time_param_name_, false)); - node_.set_parameter(rclcpp::Parameter(enable_msg_time_param_name_, false)); + diagnostics_config_.enable_node_time_diagnostics = false; + diagnostics_config_.enable_msg_time_diagnostics = false; } private: @@ -612,9 +590,6 @@ class GreenwaveDiagnostics std::optional new_freq; std::optional new_tol; std::optional new_enabled; - std::optional new_enable_node_time; - std::optional new_enable_msg_time; - std::optional new_enable_increasing_msg_time; std::vector error_reasons; ////////////////////////////////////////////////////////////////////////////// @@ -623,16 +598,15 @@ class GreenwaveDiagnostics 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_ && - param.get_name() != enable_node_time_param_name_ && - param.get_name() != enable_msg_time_param_name_ && - param.get_name() != enable_increasing_msg_time_param_name_) + param.get_name() != enabled_param_name_) { continue; } - // Allow PARAMETER_NOT_SET for parameter deletion + // 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; } @@ -641,7 +615,8 @@ class GreenwaveDiagnostics 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)"); + error_reasons.push_back(param.get_name() + + ": must be a numeric type (int or double or NaN)"); continue; } @@ -659,22 +634,16 @@ class GreenwaveDiagnostics } new_tol = value; } - // Handle boolean types together + // Handle boolean types } else if (param.get_name() == enabled_param_name_) { new_enabled = param.as_bool(); - } else if (param.get_name() == enable_node_time_param_name_) { - new_enable_node_time = param.as_bool(); - } else if (param.get_name() == enable_msg_time_param_name_) { - new_enable_msg_time = param.as_bool(); - } else if (param.get_name() == enable_increasing_msg_time_param_name_) { - new_enable_increasing_msg_time = param.as_bool(); } } // Exit early if any parameters are invalid. No half changes. if (!error_reasons.empty()) { result.successful = false; - result.reason = "Invalid parameters: " + rcpputils::join(error_reasons, "; "); + result.reason = rcpputils::join(error_reasons, "; "); } // Execution of changes happens in onParameterEvent after parameters are committed @@ -718,27 +687,6 @@ class GreenwaveDiagnostics setExpectedDt(freq, tol); } } - - // Process deleted parameters - for (const auto & param : msg->deleted_parameters) { - if (param.name == freq_param_name_) { - clearExpectedDt(); - RCLCPP_DEBUG( - node_.get_logger(), - "Cleared expected frequency for topic '%s' (parameter deleted)", - topic_name_.c_str()); - } else if (param.name == tol_param_name_) { - // Reset tolerance to default if frequency is still set - auto freq_opt = getNumericParameter(freq_param_name_); - if (freq_opt.has_value() && freq_opt.value() > 0) { - setExpectedDt(freq_opt.value(), constants::kDefaultTolerancePercent); - RCLCPP_DEBUG( - node_.get_logger(), - "Reset tolerance to default (%.1f%%) for topic '%s' (parameter deleted)", - constants::kDefaultTolerancePercent, topic_name_.c_str()); - } - } - } } void applyParameterChange(const rcl_interfaces::msg::Parameter & param) @@ -754,12 +702,6 @@ class GreenwaveDiagnostics prev_timestamp_msg_us_ = std::numeric_limits::min(); } enabled_ = new_enabled; - } else if (param.name == enable_node_time_param_name_) { - diagnostics_config_.enable_node_time_diagnostics = param.value.bool_value; - } else if (param.name == enable_msg_time_param_name_) { - diagnostics_config_.enable_msg_time_diagnostics = param.value.bool_value; - } else if (param.name == enable_increasing_msg_time_param_name_) { - diagnostics_config_.enable_increasing_msg_time_diagnostics = param.value.bool_value; } } @@ -777,21 +719,13 @@ class GreenwaveDiagnostics return param.as_double(); } else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { return static_cast(param.as_int()); - } - return std::nullopt; - } - - void tryUndeclareParameter(const std::string & param_name) - { - try { - if (node_.has_parameter(param_name)) { - node_.undeclare_parameter(param_name); + } 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(); } - } catch (const std::exception & e) { - RCLCPP_WARN( - node_.get_logger(), "Could not undeclare %s: %s", - param_name.c_str(), e.what()); } + return std::nullopt; } template @@ -820,19 +754,10 @@ class GreenwaveDiagnostics } } - void undeclareParameters() - { - tryUndeclareParameter(freq_param_name_); - tryUndeclareParameter(tol_param_name_); - } - // Parameter names for this topic std::string freq_param_name_; std::string tol_param_name_; std::string enabled_param_name_; - std::string enable_node_time_param_name_; - std::string enable_msg_time_param_name_; - std::string enable_increasing_msg_time_param_name_; // Parameter callback handle rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 57a37f7..4c30cc2 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -54,6 +54,9 @@ class GreenwaveMonitor : public rclcpp::Node bool remove_topic(const std::string & topic, std::string & message); + bool try_set_external_enabled_param( + const std::string & topic, bool enabled, std::string & message); + bool has_header_from_type(const std::string & type_name); std::set get_topics_from_parameters(); diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 22215a0..e770948 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -226,73 +226,15 @@ bool GreenwaveMonitor::add_topic( // Get the message type from the first publisher const std::string type = publishers[0].topic_type(); - // Check if any publisher node already has the enabled parameter for this topic - std::string enabled_param_name = - std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + - topic + greenwave_diagnostics::constants::kEnabledSuffix; - - // Try to find an existing node with the enabled parameter - // We use list_parameters via a temporary node to avoid executor deadlock + // Try to enable monitoring on an existing node with the parameter try { - // Get our own node's full name to skip self-reference - std::string our_ns = this->get_namespace(); - std::string our_name = this->get_name(); - std::string our_full_name = (our_ns == "/") ? - ("/" + our_name) : (our_ns + "/" + our_name); - - // Create a single temporary node for parameter operations - static std::atomic 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_param_" + std::to_string(temp_node_counter++), - "/_greenwave_internal", - temp_options); - - for (const auto & pub_info : publishers) { - std::string node_name = pub_info.node_name(); - std::string node_namespace = pub_info.node_namespace(); - std::string full_node_name = (node_namespace == "/") ? - ("/" + node_name) : (node_namespace + "/" + node_name); - - // Skip if this is our own node (avoid self-reference) - if (full_node_name == our_full_name) { - continue; - } - - auto param_client = std::make_shared( - temp_node, full_node_name); - if (!param_client->wait_for_service(std::chrono::milliseconds(500))) { - continue; - } - - if (param_client->has_parameter(enabled_param_name)) { - // Check if already enabled - can't add twice - auto current_params = param_client->get_parameters({enabled_param_name}); - if (!current_params.empty() && - current_params[0].get_type() == rclcpp::ParameterType::PARAMETER_BOOL && - current_params[0].as_bool()) - { - message = "Topic already being monitored on node: " + full_node_name; - return false; - } - - auto results = param_client->set_parameters({ - rclcpp::Parameter(enabled_param_name, true) - }); - if (!results.empty() && results[0].successful) { - RCLCPP_INFO( - this->get_logger(), - "Enabled monitoring via parameter on node '%s' for topic '%s'", - full_node_name.c_str(), topic.c_str()); - message = "Enabled monitoring on existing node: " + full_node_name; - return true; - } - } + if (try_set_external_enabled_param(topic, true, message)) { + return true; + } + // If already monitored externally, return false + if (message.find("already being monitored") != std::string::npos) { + return false; } - // Explicitly clear temp_node before proceeding to avoid any lingering async operations - temp_node.reset(); } catch (const std::exception & e) { RCLCPP_WARN( this->get_logger(), @@ -300,9 +242,6 @@ bool GreenwaveMonitor::add_topic( topic.c_str(), e.what()); } - // Small delay to ensure any async cleanup from temp_node is complete - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - // No existing node with the parameter found, create local GreenwaveDiagnostics auto sub = this->create_generic_subscription( topic, @@ -331,79 +270,16 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes auto diag_it = greenwave_diagnostics_.find(topic); if (diag_it == greenwave_diagnostics_.end()) { // Topic not monitored locally, try to find a publisher node with the enabled parameter - std::string enabled_param_name = - std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + - topic + greenwave_diagnostics::constants::kEnabledSuffix; - - auto publishers = this->get_publishers_info_by_topic(topic); - try { - // Get our own node's full name to skip self-reference - std::string our_ns = this->get_namespace(); - std::string our_name = this->get_name(); - std::string our_full_name = (our_ns == "/") ? - ("/" + our_name) : (our_ns + "/" + our_name); - - // Create a single temporary node for parameter operations - static std::atomic 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_param_" + std::to_string(temp_node_counter++), - "/_greenwave_internal", - temp_options); - - for (const auto & pub_info : publishers) { - std::string node_name = pub_info.node_name(); - std::string node_namespace = pub_info.node_namespace(); - std::string full_node_name = (node_namespace == "/") ? - ("/" + node_name) : (node_namespace + "/" + node_name); - - // Skip if this is our own node - if (full_node_name == our_full_name) { - continue; - } - - auto param_client = std::make_shared( - temp_node, full_node_name); - if (!param_client->wait_for_service(std::chrono::milliseconds(500))) { - continue; - } - - if (param_client->has_parameter(enabled_param_name)) { - // Check if already disabled - can't remove twice - auto current_params = param_client->get_parameters({enabled_param_name}); - if (!current_params.empty() && - current_params[0].get_type() == rclcpp::ParameterType::PARAMETER_BOOL && - !current_params[0].as_bool()) - { - message = "Topic already disabled on node: " + full_node_name; - return false; - } - - auto results = param_client->set_parameters({ - rclcpp::Parameter(enabled_param_name, false) - }); - if (!results.empty() && results[0].successful) { - RCLCPP_INFO( - this->get_logger(), - "Disabled monitoring via parameter on node '%s' for topic '%s'", - full_node_name.c_str(), topic.c_str()); - message = "Disabled monitoring on existing node: " + full_node_name; - return true; - } - } - } + return try_set_external_enabled_param(topic, false, message); } catch (const std::exception & e) { RCLCPP_WARN( this->get_logger(), "Exception while checking for existing monitoring on topic '%s': %s", topic.c_str(), e.what()); + message = "Exception while checking external nodes"; + return false; } - - message = "No parameter " + enabled_param_name + " found on global parameter server"; - return false; } // Find and remove the subscription @@ -425,6 +301,83 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes return true; } +bool GreenwaveMonitor::try_set_external_enabled_param( + const std::string & topic, bool enabled, std::string & message) +{ + std::string enabled_param_name = + std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + + topic + greenwave_diagnostics::constants::kEnabledSuffix; + + auto publishers = this->get_publishers_info_by_topic(topic); + if (publishers.empty()) { + message = "No publishers found for topic"; + return false; + } + + std::string our_ns = this->get_namespace(); + std::string our_name = this->get_name(); + std::string our_full_name = (our_ns == "/") ? + ("/" + our_name) : (our_ns + "/" + our_name); + + static std::atomic 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_param_" + std::to_string(temp_node_counter++), + "/_greenwave_internal", + temp_options); + + for (const auto & pub_info : publishers) { + std::string node_name = pub_info.node_name(); + std::string node_namespace = pub_info.node_namespace(); + std::string full_node_name = (node_namespace == "/") ? + ("/" + node_name) : (node_namespace + "/" + node_name); + + if (full_node_name == our_full_name) { + continue; + } + + auto param_client = std::make_shared( + temp_node, full_node_name); + if (!param_client->wait_for_service(std::chrono::milliseconds(500))) { + continue; + } + + if (!param_client->has_parameter(enabled_param_name)) { + continue; + } + + auto current_params = param_client->get_parameters({enabled_param_name}); + if (!current_params.empty() && + current_params[0].get_type() == rclcpp::ParameterType::PARAMETER_BOOL && + current_params[0].as_bool() == enabled) + { + message = enabled ? + "Topic already being monitored on node: " + full_node_name : + "Topic already disabled on node: " + full_node_name; + return false; + } + + auto results = param_client->set_parameters({ + rclcpp::Parameter(enabled_param_name, enabled) + }); + if (!results.empty() && results[0].successful) { + RCLCPP_INFO( + this->get_logger(), + "%s monitoring via parameter on node '%s' for topic '%s'", + enabled ? "Enabled" : "Disabled", + full_node_name.c_str(), topic.c_str()); + message = (enabled ? std::string("Enabled") : std::string("Disabled")) + + " monitoring on existing node: " + full_node_name; + return true; + } + } + + message = "No external node with parameter " + enabled_param_name + " found"; + return false; +} + std::set GreenwaveMonitor::get_topics_from_parameters() { std::set topics; diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 5b1c081..9e00746 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -278,63 +278,15 @@ def test_negative_tolerance_rejected(self): success = set_parameter(self.test_node, tol_param, -5.0) self.assertFalse(success, 'Negative tolerance should be rejected') - def test_delete_parameter_clears_error(self): - """Test that deleting a parameter clears the error state.""" + def test_delete_parameter_rejected(self): + """Test that deleting a parameter is rejected.""" time.sleep(2.0) freq_param = make_freq_param(TEST_TOPIC_DELETE_PARAM) - tol_param = make_tol_param(TEST_TOPIC_DELETE_PARAM) - # Set up monitoring with correct frequency (publisher is at 30 Hz) - success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) - self.assertTrue(success, f'Failed to set {freq_param}') - - success = set_parameter(self.test_node, tol_param, 10.0) - self.assertTrue(success, f'Failed to set {tol_param}') - - time.sleep(2.0) - - # Verify initial diagnostics are OK - diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_DELETE_PARAM, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual(len(diagnostics), 1, 'Should have diagnostics') - self.assertEqual( - ord(diagnostics[-1].level), 0, - 'Initial diagnostics should be OK' - ) - - # Set mismatched frequency to cause error (expect 1 Hz but publisher is 30 Hz) - success = set_parameter(self.test_node, freq_param, 1.0) - self.assertTrue(success, 'Failed to set mismatched frequency') - - time.sleep(2.0) - - # Verify diagnostics show error - diagnostics_error = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_DELETE_PARAM, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual(len(diagnostics_error), 1, 'Should have diagnostics') - has_error = any(ord(d.level) != 0 for d in diagnostics_error) - self.assertTrue(has_error, 'Should have error diagnostics with mismatched frequency') - - # Delete the frequency parameter to clear expected frequency + # Attempt to delete the frequency parameter - should be rejected success = delete_parameter(self.test_node, freq_param) - self.assertTrue(success, f'Failed to delete {freq_param}') - - time.sleep(2.0) - - # Verify diagnostics are OK again (no expected frequency = no error) - diagnostics_after_delete = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_DELETE_PARAM, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual( - len(diagnostics_after_delete), 1, 'Should have diagnostics after delete' - ) - self.assertEqual( - ord(diagnostics_after_delete[-1].level), 0, - 'Diagnostics should be OK after deleting frequency parameter' - ) + self.assertFalse(success, 'Parameter deletion should be rejected') if __name__ == '__main__': diff --git a/greenwave_monitor/test/test_greenwave_diagnostics.cpp b/greenwave_monitor/test/test_greenwave_diagnostics.cpp index 6b158ae..9951aad 100644 --- a/greenwave_monitor/test/test_greenwave_diagnostics.cpp +++ b/greenwave_monitor/test/test_greenwave_diagnostics.cpp @@ -27,6 +27,7 @@ such as frame rate and latency calculation accuracy. #include #include #include +#include #include "greenwave_diagnostics.hpp" @@ -34,6 +35,16 @@ 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 @@ -56,24 +67,144 @@ class GreenwaveDiagnosticsTest : public ::testing::Test 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) { - // Initialize GreenwaveDiagnostics - greenwave_diagnostics::GreenwaveDiagnostics greenwave_diagnostics( + greenwave_diagnostics::GreenwaveDiagnostics diag( *node_, "test_topic", greenwave_diagnostics::GreenwaveDiagnosticsConfig()); - uint64_t timestamp = test_constants::kStartTimestampNs; // in nanoseconds - for (int i = 0; i < 1000; i++) { - greenwave_diagnostics.updateDiagnostics(timestamp); - timestamp += 10000000; // 10 ms in nanoseconds - } - EXPECT_EQ(greenwave_diagnostics.getFrameRateMsg(), 100); // 100 Hz + 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) @@ -276,3 +407,510 @@ TEST_F(GreenwaveDiagnosticsTest, DiagnosticPublishSubscribeTest) 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"; + } +} From 154d35e3ced4e0ea9a8acaac9477b052d6f6ff62 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Wed, 7 Jan 2026 22:13:42 -0800 Subject: [PATCH 20/33] More tests. One consolidated test no deletions. Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 32 +- greenwave_monitor/src/greenwave_monitor.cpp | 20 +- .../src/minimal_publisher_node.cpp | 8 +- .../test/parameters/test_param_dynamic.py | 165 ++---- .../parameters/test_param_enable_existing.py | 22 +- .../parameters/test_param_enabled_false.py | 95 ++++ .../test/parameters/test_param_new_topic.py | 33 +- .../test/parameters/test_param_tol_only.py | 21 +- .../test/parameters/test_parameters.py | 521 ++++++++++++++++++ 9 files changed, 765 insertions(+), 152 deletions(-) create mode 100644 greenwave_monitor/test/parameters/test_param_enabled_false.py create mode 100644 greenwave_monitor/test/parameters/test_parameters.py diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index ad2f4ca..b7c7e8a 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -25,6 +25,7 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from greenwave_monitor.ui_adaptor import ( + ENABLED_SUFFIX, FREQ_SUFFIX, TOL_SUFFIX, TOPIC_PARAM_PREFIX, @@ -66,11 +67,20 @@ def make_tol_param(topic: str) -> str: return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' +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 the monitor node using rclpy service client.""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + """Set a parameter on a node using rclpy service client.""" + if node_namespace: + full_node_name = f'/{node_namespace}/{node_name}' + else: + full_node_name = f'/{node_name}' service_name = f'{full_node_name}/set_parameters' client = test_node.create_client(SetParameters, service_name) @@ -108,9 +118,13 @@ def set_parameter(test_node: Node, param_name: str, value, def get_parameter(test_node: Node, param_name: str, - node_name: str = MONITOR_NODE_NAME) -> Tuple[bool, Optional[float]]: - """Get a parameter from the monitor node using rclpy service client.""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + node_name: str = MONITOR_NODE_NAME, + node_namespace: str = MONITOR_NODE_NAMESPACE) -> Tuple[bool, Optional[float]]: + """Get a parameter from a node using rclpy service client.""" + if node_namespace: + full_node_name = f'/{node_namespace}/{node_name}' + else: + full_node_name = f'/{node_name}' service_name = f'{full_node_name}/get_parameters' client = test_node.create_client(GetParameters, service_name) @@ -138,9 +152,13 @@ def get_parameter(test_node: Node, param_name: str, 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 the monitor node using rclpy service client.""" - full_node_name = f'/{MONITOR_NODE_NAMESPACE}/{node_name}' + """Delete a parameter from a node using rclpy service client.""" + if node_namespace: + full_node_name = f'/{node_namespace}/{node_name}' + else: + full_node_name = f'/{node_name}' service_name = f'{full_node_name}/set_parameters' client = test_node.create_client(SetParameters, service_name) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e770948..b4a8aac 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -409,7 +409,25 @@ std::set GreenwaveMonitor::get_topics_from_parameters() } } - return topics; + // 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 diff --git a/greenwave_monitor/src/minimal_publisher_node.cpp b/greenwave_monitor/src/minimal_publisher_node.cpp index 8648604..bb3db0b 100644 --- a/greenwave_monitor/src/minimal_publisher_node.cpp +++ b/greenwave_monitor/src/minimal_publisher_node.cpp @@ -143,8 +143,12 @@ void MinimalPublisher::timer_callback() } if (greenwave_diagnostics_) { - const auto msg_timestamp = this->now(); - greenwave_diagnostics_->updateDiagnostics(msg_timestamp.nanoseconds()); + // 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/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py index 9e00746..0c77f0e 100644 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ b/greenwave_monitor/test/parameters/test_param_dynamic.py @@ -44,6 +44,9 @@ TEST_FREQUENCY = 30.0 TEST_TOLERANCE = 20.0 NONEXISTENT_TOPIC = '/topic_that_does_not_exist' +# Publisher node names (those with GreenwaveDiagnostics) +PUBLISHER_NODE_NAME = 'minimal_publisher_node_dynamic' +PUBLISHER_SET_PARAMS_NODE = 'minimal_publisher_node_set_params' @pytest.mark.launch_test @@ -57,7 +60,7 @@ def generate_test_description(): publisher_set_params = create_minimal_publisher( TEST_TOPIC_SET_PARAMS, TEST_FREQUENCY, 'imu', '_set_params', - enable_diagnostics=False + enable_diagnostics=True ) publisher_delete_param = create_minimal_publisher( @@ -82,124 +85,50 @@ class TestDynamicParameterChanges(RosNodeTestCase): TEST_NODE_NAME = 'dynamic_param_test_node' def test_set_parameters(self): - """Test setting frequency and tolerance parameters in sequence.""" + """Test setting frequency and tolerance parameters dynamically.""" time.sleep(2.0) freq_param = make_freq_param(TEST_TOPIC_SET_PARAMS) tol_param = make_tol_param(TEST_TOPIC_SET_PARAMS) - # 1. Verify topic is not monitored initially + # 1. Verify diagnostics are being published (publisher has diagnostics enabled) initial_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=1, timeout_sec=2.0 - ) - self.assertEqual( - len(initial_diagnostics), 0, - f'{TEST_TOPIC_SET_PARAMS} should not be monitored initially' + self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=5.0 ) - - # 2. Set tolerance before frequency - should succeed but not start monitoring - success = set_parameter(self.test_node, tol_param, TEST_TOLERANCE) - self.assertTrue(success, f'Failed to set {tol_param}') - - success, actual_tol = get_parameter(self.test_node, tol_param) - self.assertTrue(success, f'Failed to get {tol_param}') - self.assertAlmostEqual( - actual_tol, TEST_TOLERANCE, places=1, - msg=f'Tolerance mismatch: expected {TEST_TOLERANCE}, got {actual_tol}' - ) - - time.sleep(1.0) - diagnostics_after_tol = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=1, timeout_sec=2.0 - ) - self.assertEqual( - len(diagnostics_after_tol), 0, - f'{TEST_TOPIC_SET_PARAMS} should remain unmonitored after setting only tolerance' + self.assertGreaterEqual( + len(initial_diagnostics), 3, + f'{TEST_TOPIC_SET_PARAMS} should have diagnostics from publisher' ) - # 3. Set frequency - topic should have frequency checking enabled - success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) + # 2. Set frequency and tolerance on the publisher node + success = set_parameter( + self.test_node, freq_param, TEST_FREQUENCY, + node_name=PUBLISHER_SET_PARAMS_NODE, node_namespace='') self.assertTrue(success, f'Failed to set {freq_param}') - success, actual_freq = get_parameter(self.test_node, freq_param) + success = set_parameter( + self.test_node, tol_param, TEST_TOLERANCE, + node_name=PUBLISHER_SET_PARAMS_NODE, 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, node_namespace='') self.assertTrue(success, f'Failed to get {freq_param}') self.assertAlmostEqual( actual_freq, TEST_FREQUENCY, places=1, msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' ) - time.sleep(1.0) - diagnostics_after_freq = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual( - len(diagnostics_after_freq), 3, - 'Expected diagnostics after setting frequency param' - ) - - # 4. Set tolerance to 0.0 - should cause diagnostics to show error - success = set_parameter(self.test_node, tol_param, 0.0) - self.assertTrue(success, f'Failed to set {tol_param} to 0.0') - - success, actual_tol = get_parameter(self.test_node, tol_param) - self.assertTrue(success, f'Failed to get {tol_param}') - self.assertAlmostEqual( - actual_tol, 0.0, places=1, - msg=f'Tolerance mismatch: expected 0.0, got {actual_tol}' - ) - - time.sleep(2.0) - diagnostics_with_zero_tol = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=2, timeout_sec=5.0 - ) - self.assertGreaterEqual( - len(diagnostics_with_zero_tol), 2, - 'Topic should still be monitored with zero tolerance' - ) - - # Check that at least one diagnostic has ERROR level (frequency outside 0% tolerance) - has_error = any( - ord(d.level) != 0 for d in diagnostics_with_zero_tol - ) - self.assertTrue( - has_error, - 'Expected ERROR diagnostics with 0% tolerance' - ) - - # Reset tolerance to 10% - should no longer error - success = set_parameter(self.test_node, tol_param, 10.0) - self.assertTrue(success, f'Failed to reset {tol_param}') - - # Wait for diagnostics to stabilize after tolerance change - time.sleep(3.0) - diagnostics_after_reset = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual( - len(diagnostics_after_reset), 3, - 'Expected diagnostics after resetting tolerance' - ) - - # Verify most recent diagnostic is OK after resetting tolerance - last_diagnostic = diagnostics_after_reset[-1] - self.assertEqual( - ord(last_diagnostic.level), 0, - 'Expected OK diagnostic after resetting tolerance to 10%' - ) - - # 5. Update expected frequency to mismatched value - should cause error - # Publisher is still at 30 Hz, tolerance is 10%, but we set expected to 1 Hz + # 3. Update expected frequency to mismatched value - should cause error + # Publisher is still at 30 Hz, tolerance is 20%, but we set expected to 1 Hz mismatched_frequency = 1.0 - success = set_parameter(self.test_node, freq_param, mismatched_frequency) + success = set_parameter( + self.test_node, freq_param, mismatched_frequency, + node_name=PUBLISHER_SET_PARAMS_NODE, node_namespace='') self.assertTrue(success, f'Failed to update {freq_param}') - success, actual_freq = get_parameter(self.test_node, freq_param) - self.assertTrue(success, f'Failed to get updated {freq_param}') - self.assertAlmostEqual( - actual_freq, mismatched_frequency, places=1, - msg=f'Frequency mismatch: expected {mismatched_frequency}, got {actual_freq}' - ) - time.sleep(2.0) diagnostics_mismatched = collect_diagnostics_for_topic( self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=10.0 @@ -209,13 +138,13 @@ def test_set_parameters(self): 'Should still receive diagnostics after frequency update' ) - # Verify diagnostics show error due to frequency mismatch - has_error = any( + # Verify diagnostics show non-OK status due to frequency mismatch + has_non_ok = any( ord(d.level) != 0 for d in diagnostics_mismatched ) self.assertTrue( - has_error, - 'Expected ERROR diagnostics when actual frequency (30 Hz) ' + has_non_ok, + 'Expected non-OK diagnostics when actual frequency (30 Hz) ' 'does not match expected (1 Hz)' ) @@ -248,44 +177,60 @@ def test_non_numeric_parameter_rejected(self): """Test that non-numeric parameter values are rejected.""" time.sleep(1.0) + # Target the publisher node which has GreenwaveDiagnostics freq_param = make_freq_param(TEST_TOPIC) - success = set_parameter(self.test_node, freq_param, 'not_a_number') + success = set_parameter( + self.test_node, freq_param, 'not_a_number', + node_name=PUBLISHER_NODE_NAME, node_namespace='') self.assertFalse(success, 'Non-numeric frequency parameter should be rejected') tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter(self.test_node, tol_param, 'invalid') + success = set_parameter( + self.test_node, tol_param, 'invalid', + node_name=PUBLISHER_NODE_NAME, node_namespace='') self.assertFalse(success, 'Non-numeric tolerance parameter should be rejected') def test_non_positive_frequency_rejected(self): """Test that non-positive frequency values are rejected.""" time.sleep(1.0) + # Target the publisher node which has GreenwaveDiagnostics freq_param = make_freq_param(TEST_TOPIC) # Test zero frequency - success = set_parameter(self.test_node, freq_param, 0.0) + success = set_parameter( + self.test_node, freq_param, 0.0, + node_name=PUBLISHER_NODE_NAME, node_namespace='') self.assertFalse(success, 'Zero frequency should be rejected') # Test negative frequency - success = set_parameter(self.test_node, freq_param, -10.0) + success = set_parameter( + self.test_node, freq_param, -10.0, + node_name=PUBLISHER_NODE_NAME, 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) + # Target the publisher node which has GreenwaveDiagnostics tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter(self.test_node, tol_param, -5.0) + success = set_parameter( + self.test_node, tol_param, -5.0, + node_name=PUBLISHER_NODE_NAME, node_namespace='') self.assertFalse(success, 'Negative tolerance should be rejected') def test_delete_parameter_rejected(self): """Test that deleting a parameter is rejected.""" time.sleep(2.0) - freq_param = make_freq_param(TEST_TOPIC_DELETE_PARAM) + # Target the publisher node which has GreenwaveDiagnostics + freq_param = make_freq_param(TEST_TOPIC) # Attempt to delete the frequency parameter - should be rejected - success = delete_parameter(self.test_node, freq_param) + success = delete_parameter( + self.test_node, freq_param, + node_name=PUBLISHER_NODE_NAME, node_namespace='') self.assertFalse(success, 'Parameter deletion should be rejected') diff --git a/greenwave_monitor/test/parameters/test_param_enable_existing.py b/greenwave_monitor/test/parameters/test_param_enable_existing.py index 3d5fdcb..af6f91d 100644 --- a/greenwave_monitor/test/parameters/test_param_enable_existing.py +++ b/greenwave_monitor/test/parameters/test_param_enable_existing.py @@ -109,11 +109,29 @@ def test_add_topic_enables_existing_node_parameter(self): 'ManageTopic service not available' ) - # Call add_topic for the test topic + # First, call remove_topic to disable monitoring on the publisher + response = call_manage_topic_service( + self.test_node, manage_topic_client, add=False, topic=TEST_TOPIC + ) + self.assertIsNotNone(response, 'ManageTopic remove service call failed') + self.assertTrue(response.success, f'Failed to remove topic: {response.message}') + + # Verify the enabled parameter is now false + 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(), 'Failed to get parameters after remove') + self.assertFalse( + future.result().values[0].bool_value, + 'Enabled parameter should be false after remove_topic' + ) + + # Now call add_topic to re-enable monitoring response = call_manage_topic_service( self.test_node, manage_topic_client, add=True, topic=TEST_TOPIC ) - self.assertIsNotNone(response, 'ManageTopic service call failed') + self.assertIsNotNone(response, 'ManageTopic add service call failed') self.assertTrue(response.success, f'Failed to add topic: {response.message}') # Verify the response message indicates it enabled monitoring on existing node diff --git a/greenwave_monitor/test/parameters/test_param_enabled_false.py b/greenwave_monitor/test/parameters/test_param_enabled_false.py new file mode 100644 index 0000000..fc3fc69 --- /dev/null +++ b/greenwave_monitor/test/parameters/test_param_enabled_false.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Test: enabled=false should NOT start monitoring at startup.""" + +import time +import unittest + +from greenwave_monitor.test_utils import ( + collect_diagnostics_for_topic, + create_minimal_publisher, + make_enabled_param, + make_freq_param, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE, + RosNodeTestCase, +) +import launch +import launch_ros.actions +import launch_testing +import pytest + + +TEST_TOPIC = '/enabled_false_topic' +TEST_FREQUENCY = 50.0 + + +@pytest.mark.launch_test +def generate_test_description(): + """Test with enabled=false - should NOT monitor even with other params set.""" + params = { + make_freq_param(TEST_TOPIC): TEST_FREQUENCY, + make_enabled_param(TEST_TOPIC): False + } + + ros2_monitor_node = launch_ros.actions.Node( + package='greenwave_monitor', + executable='greenwave_monitor', + name=MONITOR_NODE_NAME, + namespace=MONITOR_NODE_NAMESPACE, + parameters=[params], + output='screen' + ) + + publisher = create_minimal_publisher( + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_enabled_false', + enable_diagnostics=False + ) + + return ( + launch.LaunchDescription([ + ros2_monitor_node, + publisher, + launch_testing.actions.ReadyToTest() + ]), {} + ) + + +class TestEnabledFalseParameter(RosNodeTestCase): + """Test that enabled=false prevents monitoring at startup.""" + + TEST_NODE_NAME = 'enabled_false_test_node' + + def test_enabled_false_does_not_monitor(self): + """Test that enabled=false prevents topic monitoring even with frequency set.""" + time.sleep(2.0) + + received_diagnostics = collect_diagnostics_for_topic( + self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=3.0 + ) + + self.assertEqual( + len(received_diagnostics), 0, + f'Should not monitor topic with enabled=false, got {len(received_diagnostics)}' + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py index 7a0048b..55c2e8c 100644 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ b/greenwave_monitor/test/parameters/test_param_new_topic.py @@ -23,9 +23,9 @@ import unittest from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, create_minimal_publisher, create_monitor_node, + get_parameter, make_freq_param, RosNodeTestCase, set_parameter, @@ -45,7 +45,8 @@ def generate_test_description(): ros2_monitor_node = create_monitor_node() publisher = create_minimal_publisher( - NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic' + NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic', + enable_diagnostics=False ) return ( @@ -63,30 +64,22 @@ class TestAddNewTopicViaParam(RosNodeTestCase): TEST_NODE_NAME = 'new_topic_test_node' def test_add_new_topic_via_frequency_param(self): - """Test that setting frequency param for new topic starts monitoring.""" + """Test that frequency param can be set for a topic (parameter is stored).""" + # NOTE: Dynamic topic addition via parameter events is not implemented. + # This test verifies that the parameter can be set, but monitoring only + # starts at node startup when parameters are already configured. time.sleep(2.0) - initial_diagnostics = collect_diagnostics_for_topic( - self.test_node, NEW_TOPIC, expected_count=1, timeout_sec=2.0 - ) - self.assertEqual( - len(initial_diagnostics), 0, - 'Topic should not be monitored initially' - ) - freq_param = make_freq_param(NEW_TOPIC) success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) self.assertTrue(success, f'Failed to set {freq_param}') - time.sleep(2.0) - - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, NEW_TOPIC, expected_count=3, timeout_sec=10.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - 'Should monitor new topic after setting frequency param' + # Verify the parameter was stored (monitoring won't start dynamically) + success, value = get_parameter(self.test_node, freq_param) + self.assertTrue(success, f'Failed to get {freq_param}') + self.assertAlmostEqual( + value, TEST_FREQUENCY, places=1, + msg=f'Parameter value mismatch: expected {TEST_FREQUENCY}, got {value}' ) diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py index a90a634..7ba24b8 100644 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ b/greenwave_monitor/test/parameters/test_param_tol_only.py @@ -17,7 +17,7 @@ # # SPDX-License-Identifier: Apache-2.0 -"""Test: only tolerance specified - should NOT start monitoring.""" +"""Test: only tolerance specified - should still start monitoring.""" import time import unittest @@ -42,7 +42,7 @@ @pytest.mark.launch_test def generate_test_description(): - """Test with only tolerance specified (should not monitor).""" + """Test with only tolerance specified (should still monitor).""" params = { make_tol_param(TEST_TOPIC): 15.0 } @@ -57,7 +57,8 @@ def generate_test_description(): ) publisher = create_minimal_publisher( - TEST_TOPIC, TEST_FREQUENCY, 'imu', '_tol_only' + TEST_TOPIC, TEST_FREQUENCY, 'imu', '_tol_only', + enable_diagnostics=False ) return ( @@ -70,21 +71,21 @@ def generate_test_description(): class TestToleranceOnlyParameter(RosNodeTestCase): - """Test that only specifying tolerance does NOT start monitoring.""" + """Test that specifying only tolerance still starts monitoring.""" TEST_NODE_NAME = 'tol_only_test_node' - def test_tolerance_only_does_not_monitor(self): - """Test that specifying only tolerance does not start monitoring.""" + def test_tolerance_only_starts_monitoring(self): + """Test that specifying only tolerance starts monitoring (any param triggers add_topic).""" time.sleep(2.0) received_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=3.0 + self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=5.0 ) - self.assertEqual( - len(received_diagnostics), 0, - f'Should not monitor topic with only tolerance set, got {len(received_diagnostics)}' + self.assertGreaterEqual( + len(received_diagnostics), 3, + f'Should monitor topic when tolerance is set, got {len(received_diagnostics)}' ) diff --git a/greenwave_monitor/test/parameters/test_parameters.py b/greenwave_monitor/test/parameters/test_parameters.py new file mode 100644 index 0000000..3c38efc --- /dev/null +++ b/greenwave_monitor/test/parameters/test_parameters.py @@ -0,0 +1,521 @@ +#!/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 ( + call_manage_topic_service, + collect_diagnostics_for_topic, + create_minimal_publisher, + create_service_clients, + delete_parameter, + find_best_diagnostic, + get_parameter, + make_enabled_param, + make_freq_param, + make_tol_param, + MONITOR_NODE_NAME, + MONITOR_NODE_NAMESPACE, + RosNodeTestCase, + set_parameter, + wait_for_service_connection, +) +from greenwave_monitor.ui_adaptor import ENABLED_SUFFIX, 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 on existing nodes.""" + + TEST_NODE_NAME = 'enable_existing_test_node' + + def test_add_topic_enables_existing_node_parameter(self): + """Test that add_topic enables parameter on existing nodes.""" + 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) + + # Create manage_topic client + manage_topic_client = create_service_clients(self.test_node) + self.assertTrue( + wait_for_service_connection( + self.test_node, manage_topic_client, + timeout_sec=5.0, service_name='manage_topic')) + + # Remove topic to disable monitoring + response = call_manage_topic_service( + self.test_node, manage_topic_client, add=False, topic=ENABLE_EXISTING_TOPIC) + self.assertIsNotNone(response) + self.assertTrue(response.success) + + # Verify enabled=false + 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.assertFalse(future.result().values[0].bool_value) + + # Re-enable via add_topic + response = call_manage_topic_service( + self.test_node, manage_topic_client, add=True, topic=ENABLE_EXISTING_TOPIC) + self.assertIsNotNone(response) + self.assertTrue(response.success) + self.assertIn('Enabled monitoring on existing node', response.message) + + # Verify enabled=true + 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) + self.assertTrue(future.result().values[0].bool_value) + + self.test_node.destroy_client(get_params_client) + self.test_node.destroy_client(manage_topic_client) + + +if __name__ == '__main__': + unittest.main() + From b9292b34a4b3c113435e0d4bce4cf84d646f0a16 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Wed, 7 Jan 2026 22:17:23 -0800 Subject: [PATCH 21/33] Remove old test files Signed-off-by: Blake McHale --- greenwave_monitor/CMakeLists.txt | 15 +- .../test/parameters/test_param_dynamic.py | 238 ------------------ .../parameters/test_param_enable_existing.py | 165 ------------ .../parameters/test_param_enabled_false.py | 95 ------- .../test/parameters/test_param_freq_only.py | 95 ------- .../parameters/test_param_multiple_topics.py | 185 -------------- .../test/parameters/test_param_new_topic.py | 87 ------- .../test/parameters/test_param_tol_only.py | 93 ------- .../test/parameters/test_param_yaml.py | 145 ----------- .../test/parameters/test_topic_parameters.py | 118 --------- .../test/{parameters => }/test_parameters.py | 0 11 files changed, 5 insertions(+), 1231 deletions(-) delete mode 100644 greenwave_monitor/test/parameters/test_param_dynamic.py delete mode 100644 greenwave_monitor/test/parameters/test_param_enable_existing.py delete mode 100644 greenwave_monitor/test/parameters/test_param_enabled_false.py delete mode 100644 greenwave_monitor/test/parameters/test_param_freq_only.py delete mode 100644 greenwave_monitor/test/parameters/test_param_multiple_topics.py delete mode 100644 greenwave_monitor/test/parameters/test_param_new_topic.py delete mode 100644 greenwave_monitor/test/parameters/test_param_tol_only.py delete mode 100644 greenwave_monitor/test/parameters/test_param_yaml.py delete mode 100644 greenwave_monitor/test/parameters/test_topic_parameters.py rename greenwave_monitor/test/{parameters => }/test_parameters.py (100%) diff --git a/greenwave_monitor/CMakeLists.txt b/greenwave_monitor/CMakeLists.txt index 827cb1f..ac4668f 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -116,16 +116,11 @@ 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 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_greenwave_diagnostics test/test_greenwave_diagnostics.cpp diff --git a/greenwave_monitor/test/parameters/test_param_dynamic.py b/greenwave_monitor/test/parameters/test_param_dynamic.py deleted file mode 100644 index 0c77f0e..0000000 --- a/greenwave_monitor/test/parameters/test_param_dynamic.py +++ /dev/null @@ -1,238 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: dynamic parameter changes via ros2 param set.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, - create_minimal_publisher, - create_monitor_node, - delete_parameter, - get_parameter, - make_freq_param, - make_tol_param, - RosNodeTestCase, - set_parameter, -) -import launch -import launch_testing -import pytest - - -TEST_TOPIC = '/dynamic_param_topic' -TEST_TOPIC_SET_PARAMS = '/dynamic_param_topic_set_params' -TEST_TOPIC_DELETE_PARAM = '/dynamic_param_topic_delete_param' -TEST_FREQUENCY = 30.0 -TEST_TOLERANCE = 20.0 -NONEXISTENT_TOPIC = '/topic_that_does_not_exist' -# Publisher node names (those with GreenwaveDiagnostics) -PUBLISHER_NODE_NAME = 'minimal_publisher_node_dynamic' -PUBLISHER_SET_PARAMS_NODE = 'minimal_publisher_node_set_params' - - -@pytest.mark.launch_test -def generate_test_description(): - """Test dynamic parameter changes via ros2 param set.""" - ros2_monitor_node = create_monitor_node() - - publisher = create_minimal_publisher( - TEST_TOPIC, TEST_FREQUENCY, 'imu', '_dynamic' - ) - - publisher_set_params = create_minimal_publisher( - TEST_TOPIC_SET_PARAMS, TEST_FREQUENCY, 'imu', '_set_params', - enable_diagnostics=True - ) - - publisher_delete_param = create_minimal_publisher( - TEST_TOPIC_DELETE_PARAM, TEST_FREQUENCY, 'imu', '_delete_param', - enable_diagnostics=False - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - publisher_set_params, - publisher_delete_param, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestDynamicParameterChanges(RosNodeTestCase): - """Test changing parameters dynamically via ros2 param set.""" - - TEST_NODE_NAME = 'dynamic_param_test_node' - - def test_set_parameters(self): - """Test setting frequency and tolerance parameters dynamically.""" - time.sleep(2.0) - - freq_param = make_freq_param(TEST_TOPIC_SET_PARAMS) - tol_param = make_tol_param(TEST_TOPIC_SET_PARAMS) - - # 1. Verify diagnostics are being published (publisher has diagnostics enabled) - initial_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=5.0 - ) - self.assertGreaterEqual( - len(initial_diagnostics), 3, - f'{TEST_TOPIC_SET_PARAMS} should have diagnostics from publisher' - ) - - # 2. Set frequency and tolerance on the publisher node - success = set_parameter( - self.test_node, freq_param, TEST_FREQUENCY, - node_name=PUBLISHER_SET_PARAMS_NODE, node_namespace='') - self.assertTrue(success, f'Failed to set {freq_param}') - - success = set_parameter( - self.test_node, tol_param, TEST_TOLERANCE, - node_name=PUBLISHER_SET_PARAMS_NODE, 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, node_namespace='') - self.assertTrue(success, f'Failed to get {freq_param}') - self.assertAlmostEqual( - actual_freq, TEST_FREQUENCY, places=1, - msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' - ) - - # 3. Update expected frequency to mismatched value - should cause error - # Publisher is still at 30 Hz, tolerance is 20%, but we set expected to 1 Hz - mismatched_frequency = 1.0 - success = set_parameter( - self.test_node, freq_param, mismatched_frequency, - node_name=PUBLISHER_SET_PARAMS_NODE, node_namespace='') - self.assertTrue(success, f'Failed to update {freq_param}') - - time.sleep(2.0) - diagnostics_mismatched = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC_SET_PARAMS, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual( - len(diagnostics_mismatched), 3, - 'Should still receive diagnostics after frequency update' - ) - - # Verify diagnostics show non-OK status due to frequency mismatch - has_non_ok = any( - ord(d.level) != 0 for d in diagnostics_mismatched - ) - self.assertTrue( - has_non_ok, - 'Expected non-OK diagnostics when actual frequency (30 Hz) ' - 'does not match expected (1 Hz)' - ) - - def test_set_frequency_for_nonexistent_topic(self): - """Test setting expected frequency for a topic that does not exist.""" - time.sleep(1.0) - - freq_param = make_freq_param(NONEXISTENT_TOPIC) - success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) - self.assertTrue(success, f'Failed to set {freq_param}') - - # Verify parameter was set - success, actual_freq = get_parameter(self.test_node, freq_param) - self.assertTrue(success, f'Failed to get {freq_param}') - self.assertAlmostEqual( - actual_freq, TEST_FREQUENCY, places=1, - msg=f'Frequency mismatch: expected {TEST_FREQUENCY}, got {actual_freq}' - ) - - # Topic should not appear in diagnostics since it doesn't exist - diagnostics = collect_diagnostics_for_topic( - self.test_node, NONEXISTENT_TOPIC, expected_count=1, timeout_sec=3.0 - ) - self.assertEqual( - len(diagnostics), 0, - f'{NONEXISTENT_TOPIC} should not appear in diagnostics' - ) - - def test_non_numeric_parameter_rejected(self): - """Test that non-numeric parameter values are rejected.""" - time.sleep(1.0) - - # Target the publisher node which has GreenwaveDiagnostics - freq_param = make_freq_param(TEST_TOPIC) - success = set_parameter( - self.test_node, freq_param, 'not_a_number', - node_name=PUBLISHER_NODE_NAME, node_namespace='') - self.assertFalse(success, 'Non-numeric frequency parameter should be rejected') - - tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter( - self.test_node, tol_param, 'invalid', - node_name=PUBLISHER_NODE_NAME, node_namespace='') - self.assertFalse(success, 'Non-numeric tolerance parameter should be rejected') - - def test_non_positive_frequency_rejected(self): - """Test that non-positive frequency values are rejected.""" - time.sleep(1.0) - - # Target the publisher node which has GreenwaveDiagnostics - freq_param = make_freq_param(TEST_TOPIC) - - # Test zero frequency - success = set_parameter( - self.test_node, freq_param, 0.0, - node_name=PUBLISHER_NODE_NAME, node_namespace='') - self.assertFalse(success, 'Zero frequency should be rejected') - - # Test negative frequency - success = set_parameter( - self.test_node, freq_param, -10.0, - node_name=PUBLISHER_NODE_NAME, 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) - - # Target the publisher node which has GreenwaveDiagnostics - tol_param = make_tol_param(TEST_TOPIC) - success = set_parameter( - self.test_node, tol_param, -5.0, - node_name=PUBLISHER_NODE_NAME, node_namespace='') - self.assertFalse(success, 'Negative tolerance should be rejected') - - def test_delete_parameter_rejected(self): - """Test that deleting a parameter is rejected.""" - time.sleep(2.0) - - # Target the publisher node which has GreenwaveDiagnostics - freq_param = make_freq_param(TEST_TOPIC) - - # Attempt to delete the frequency parameter - should be rejected - success = delete_parameter( - self.test_node, freq_param, - node_name=PUBLISHER_NODE_NAME, node_namespace='') - self.assertFalse(success, 'Parameter deletion should be rejected') - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_enable_existing.py b/greenwave_monitor/test/parameters/test_param_enable_existing.py deleted file mode 100644 index af6f91d..0000000 --- a/greenwave_monitor/test/parameters/test_param_enable_existing.py +++ /dev/null @@ -1,165 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: add_topic enables existing node's parameter instead of creating local diagnostics.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - call_manage_topic_service, - create_minimal_publisher, - create_monitor_node, - create_service_clients, - RosNodeTestCase, - wait_for_service_connection, -) -from greenwave_monitor.ui_adaptor import ENABLED_SUFFIX, TOPIC_PARAM_PREFIX -import launch -import launch_testing -import pytest -from rcl_interfaces.srv import GetParameters -import rclpy - - -TEST_TOPIC = '/enable_existing_test_topic' -TEST_FREQUENCY = 50.0 -PUBLISHER_NODE_NAME = 'minimal_publisher_node_enable_test' - - -def make_enabled_param(topic: str) -> str: - """Build enabled parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{ENABLED_SUFFIX}' - - -@pytest.mark.launch_test -def generate_test_description(): - """Launch monitor and publisher nodes for testing.""" - ros2_monitor_node = create_monitor_node(topics=['']) - - publisher = create_minimal_publisher( - TEST_TOPIC, TEST_FREQUENCY, 'imu', '_enable_test' - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestEnableExistingNodeParameter(RosNodeTestCase): - """Test that add_topic enables parameter on existing nodes.""" - - TEST_NODE_NAME = 'enable_existing_test_node' - - def test_add_topic_enables_existing_node_parameter(self): - """Test that adding a topic with an existing publisher sets the enabled parameter.""" - time.sleep(3.0) - - publisher_full_name = f'/{PUBLISHER_NODE_NAME}' - - # Create a client to get parameters from the publisher node - 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 on publisher node' - ) - - enabled_param_name = make_enabled_param(TEST_TOPIC) - - # Verify the publisher node has the 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(), 'Failed to get parameters from publisher') - self.assertTrue( - len(future.result().values) > 0, - f'Publisher node should have parameter {enabled_param_name}' - ) - - # Create service client for manage_topic - manage_topic_client = create_service_clients(self.test_node) - self.assertTrue( - wait_for_service_connection( - self.test_node, manage_topic_client, - timeout_sec=5.0, service_name='manage_topic' - ), - 'ManageTopic service not available' - ) - - # First, call remove_topic to disable monitoring on the publisher - response = call_manage_topic_service( - self.test_node, manage_topic_client, add=False, topic=TEST_TOPIC - ) - self.assertIsNotNone(response, 'ManageTopic remove service call failed') - self.assertTrue(response.success, f'Failed to remove topic: {response.message}') - - # Verify the enabled parameter is now false - 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(), 'Failed to get parameters after remove') - self.assertFalse( - future.result().values[0].bool_value, - 'Enabled parameter should be false after remove_topic' - ) - - # Now call add_topic to re-enable monitoring - response = call_manage_topic_service( - self.test_node, manage_topic_client, add=True, topic=TEST_TOPIC - ) - self.assertIsNotNone(response, 'ManageTopic add service call failed') - self.assertTrue(response.success, f'Failed to add topic: {response.message}') - - # Verify the response message indicates it enabled monitoring on existing node - self.assertIn( - 'Enabled monitoring on existing node', - response.message, - f'Expected message about enabling existing node, got: {response.message}' - ) - - # Verify the enabled parameter is now true on the publisher node - 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(), 'Failed to get parameters after add_topic') - self.assertTrue( - len(future.result().values) > 0, - 'Parameter should still exist after add_topic' - ) - self.assertTrue( - future.result().values[0].bool_value, - 'Enabled parameter should be true after add_topic' - ) - - # Cleanup - self.test_node.destroy_client(get_params_client) - self.test_node.destroy_client(manage_topic_client) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_enabled_false.py b/greenwave_monitor/test/parameters/test_param_enabled_false.py deleted file mode 100644 index fc3fc69..0000000 --- a/greenwave_monitor/test/parameters/test_param_enabled_false.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: enabled=false should NOT start monitoring at startup.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, - create_minimal_publisher, - make_enabled_param, - make_freq_param, - MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE, - RosNodeTestCase, -) -import launch -import launch_ros.actions -import launch_testing -import pytest - - -TEST_TOPIC = '/enabled_false_topic' -TEST_FREQUENCY = 50.0 - - -@pytest.mark.launch_test -def generate_test_description(): - """Test with enabled=false - should NOT monitor even with other params set.""" - params = { - make_freq_param(TEST_TOPIC): TEST_FREQUENCY, - make_enabled_param(TEST_TOPIC): False - } - - ros2_monitor_node = launch_ros.actions.Node( - package='greenwave_monitor', - executable='greenwave_monitor', - name=MONITOR_NODE_NAME, - namespace=MONITOR_NODE_NAMESPACE, - parameters=[params], - output='screen' - ) - - publisher = create_minimal_publisher( - TEST_TOPIC, TEST_FREQUENCY, 'imu', '_enabled_false', - enable_diagnostics=False - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestEnabledFalseParameter(RosNodeTestCase): - """Test that enabled=false prevents monitoring at startup.""" - - TEST_NODE_NAME = 'enabled_false_test_node' - - def test_enabled_false_does_not_monitor(self): - """Test that enabled=false prevents topic monitoring even with frequency set.""" - time.sleep(2.0) - - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=1, timeout_sec=3.0 - ) - - self.assertEqual( - len(received_diagnostics), 0, - f'Should not monitor topic with enabled=false, got {len(received_diagnostics)}' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_freq_only.py b/greenwave_monitor/test/parameters/test_param_freq_only.py deleted file mode 100644 index 578f427..0000000 --- a/greenwave_monitor/test/parameters/test_param_freq_only.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: only expected_frequency specified, tolerance defaults to 5%.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, - create_minimal_publisher, - create_monitor_node, - find_best_diagnostic, - RosNodeTestCase, -) -import launch -import launch_testing -import pytest - - -TEST_TOPIC = '/freq_only_topic' -TEST_FREQUENCY = 50.0 - - -@pytest.mark.launch_test -def generate_test_description(): - """Test with only expected_frequency specified.""" - topic_configs = { - TEST_TOPIC: { - 'expected_frequency': TEST_FREQUENCY - # No tolerance - should default to 5% - } - } - - ros2_monitor_node = create_monitor_node( - topic_configs=topic_configs - ) - - publisher = create_minimal_publisher( - TEST_TOPIC, TEST_FREQUENCY, 'imu', '_freq_only' - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestFrequencyOnlyParameter(RosNodeTestCase): - """Test that only specifying frequency works (tolerance defaults).""" - - TEST_NODE_NAME = 'freq_only_test_node' - - def test_frequency_only_uses_default_tolerance(self): - """Test that specifying only frequency uses default tolerance.""" - time.sleep(2.0) - - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - f'Expected at least 3 diagnostics, got {len(received_diagnostics)}' - ) - best_status, _ = find_best_diagnostic( - received_diagnostics, TEST_FREQUENCY, 'imu' - ) - self.assertIsNotNone( - best_status, - 'Should have valid frame rate with default tolerance' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_multiple_topics.py b/greenwave_monitor/test/parameters/test_param_multiple_topics.py deleted file mode 100644 index b351b33..0000000 --- a/greenwave_monitor/test/parameters/test_param_multiple_topics.py +++ /dev/null @@ -1,185 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: multiple topics configured via parameters at startup.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, - create_minimal_publisher, - create_monitor_node, - find_best_diagnostic, - RosNodeTestCase, -) -import launch -import launch_testing -import pytest - - -# Topics with expected frequencies configured -TOPIC_1 = '/multi_topic_1' -TOPIC_2 = '/multi_topic_2' -TOPIC_3 = '/multi_topic_3' -FREQUENCY_1 = 10.0 -FREQUENCY_2 = 25.0 -FREQUENCY_3 = 50.0 -TOLERANCE = 20.0 - -# Topics specified as list only (no expected frequency) -TOPIC_LIST_1 = '/multi_topic_list_1' -TOPIC_LIST_2 = '/multi_topic_list_2' -LIST_PUBLISHER_FREQ = 30.0 - - -@pytest.mark.launch_test -def generate_test_description(): - """Test multiple topics configured via parameters.""" - # Topics with frequency/tolerance configs - topic_configs = { - TOPIC_1: { - 'expected_frequency': FREQUENCY_1, - 'tolerance': TOLERANCE - }, - TOPIC_2: { - 'expected_frequency': FREQUENCY_2, - 'tolerance': TOLERANCE - }, - TOPIC_3: { - 'expected_frequency': FREQUENCY_3, - 'tolerance': TOLERANCE - } - } - - # Also include topics specified as simple list (no frequencies) - topics_list = [TOPIC_LIST_1, TOPIC_LIST_2] - - ros2_monitor_node = create_monitor_node( - topics=topics_list, - topic_configs=topic_configs - ) - - publisher_1 = create_minimal_publisher(TOPIC_1, FREQUENCY_1, 'imu', '_multi_1') - publisher_2 = create_minimal_publisher(TOPIC_2, FREQUENCY_2, 'imu', '_multi_2') - publisher_3 = create_minimal_publisher(TOPIC_3, FREQUENCY_3, 'imu', '_multi_3') - # Publishers for topics without expected frequencies - publisher_list_1 = create_minimal_publisher( - TOPIC_LIST_1, LIST_PUBLISHER_FREQ, 'imu', '_list_1') - publisher_list_2 = create_minimal_publisher( - TOPIC_LIST_2, LIST_PUBLISHER_FREQ, 'imu', '_list_2') - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher_1, - publisher_2, - publisher_3, - publisher_list_1, - publisher_list_2, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestMultipleTopicsViaParameters(RosNodeTestCase): - """Test that multiple topics can be configured via parameters.""" - - TEST_NODE_NAME = 'multiple_topics_test_node' - - def test_all_topics_monitored(self): - """Test that all configured topics are monitored.""" - time.sleep(2.0) - - topics_to_check = [ - (TOPIC_1, FREQUENCY_1), - (TOPIC_2, FREQUENCY_2), - (TOPIC_3, FREQUENCY_3), - ] - - for topic, expected_freq in topics_to_check: - with self.subTest(topic=topic): - diagnostics = collect_diagnostics_for_topic( - self.test_node, topic, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual( - len(diagnostics), 3, - f'Expected at least 3 diagnostics for {topic}' - ) - - best_status, best_values = find_best_diagnostic( - diagnostics, expected_freq, 'imu' - ) - self.assertIsNotNone( - best_status, - f'Should have valid diagnostics for {topic}' - ) - - frame_rate = best_values[0] - tolerance_hz = expected_freq * TOLERANCE / 100.0 - self.assertAlmostEqual( - frame_rate, expected_freq, delta=tolerance_hz, - msg=f'{topic}: frame rate {frame_rate} not within ' - f'{tolerance_hz} of expected {expected_freq}' - ) - - def test_topics_list_monitored_without_expected_frequency(self): - """Test topics in list are monitored but show no expected frequency.""" - time.sleep(2.0) - - for topic in [TOPIC_LIST_1, TOPIC_LIST_2]: - with self.subTest(topic=topic): - diagnostics = collect_diagnostics_for_topic( - self.test_node, topic, expected_count=3, timeout_sec=10.0 - ) - self.assertGreaterEqual( - len(diagnostics), 3, - f'Expected at least 3 diagnostics for {topic}' - ) - - # Verify expected_frequency is 0.0 or not present (not configured) - last_diag = diagnostics[-1] - expected_freq_value = None - frame_rate_value = None - for kv in last_diag.values: - if kv.key == 'expected_frequency': - expected_freq_value = float(kv.value) - elif kv.key == 'frame_rate_node': - frame_rate_value = float(kv.value) - - # When not configured, expected_frequency is either not present or 0.0 - self.assertTrue( - expected_freq_value is None or expected_freq_value == 0.0, - f'{topic}: expected_frequency should be None or 0.0, ' - f'got {expected_freq_value}' - ) - - # Verify frame rate is being reported (topic is monitored) - self.assertIsNotNone( - frame_rate_value, - f'{topic}: should have frame_rate_node in diagnostics' - ) - self.assertGreater( - frame_rate_value, 0.0, - f'{topic}: frame_rate_node should be > 0' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_new_topic.py b/greenwave_monitor/test/parameters/test_param_new_topic.py deleted file mode 100644 index 55c2e8c..0000000 --- a/greenwave_monitor/test/parameters/test_param_new_topic.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: add new topic to monitoring via ros2 param set.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - create_minimal_publisher, - create_monitor_node, - get_parameter, - make_freq_param, - RosNodeTestCase, - set_parameter, -) -import launch -import launch_testing -import pytest - - -NEW_TOPIC = '/new_dynamic_topic' -TEST_FREQUENCY = 50.0 - - -@pytest.mark.launch_test -def generate_test_description(): - """Test adding a new topic via ros2 param set.""" - ros2_monitor_node = create_monitor_node() - - publisher = create_minimal_publisher( - NEW_TOPIC, TEST_FREQUENCY, 'imu', '_new_dynamic', - enable_diagnostics=False - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestAddNewTopicViaParam(RosNodeTestCase): - """Test adding a new topic to monitoring via ros2 param set.""" - - TEST_NODE_NAME = 'new_topic_test_node' - - def test_add_new_topic_via_frequency_param(self): - """Test that frequency param can be set for a topic (parameter is stored).""" - # NOTE: Dynamic topic addition via parameter events is not implemented. - # This test verifies that the parameter can be set, but monitoring only - # starts at node startup when parameters are already configured. - time.sleep(2.0) - - freq_param = make_freq_param(NEW_TOPIC) - success = set_parameter(self.test_node, freq_param, TEST_FREQUENCY) - self.assertTrue(success, f'Failed to set {freq_param}') - - # Verify the parameter was stored (monitoring won't start dynamically) - success, value = get_parameter(self.test_node, freq_param) - self.assertTrue(success, f'Failed to get {freq_param}') - self.assertAlmostEqual( - value, TEST_FREQUENCY, places=1, - msg=f'Parameter value mismatch: expected {TEST_FREQUENCY}, got {value}' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_tol_only.py b/greenwave_monitor/test/parameters/test_param_tol_only.py deleted file mode 100644 index 7ba24b8..0000000 --- a/greenwave_monitor/test/parameters/test_param_tol_only.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: only tolerance specified - should still start monitoring.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, - create_minimal_publisher, - make_tol_param, - MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE, - RosNodeTestCase, -) -import launch -import launch_ros.actions -import launch_testing -import pytest - - -TEST_TOPIC = '/tol_only_topic' -TEST_FREQUENCY = 50.0 - - -@pytest.mark.launch_test -def generate_test_description(): - """Test with only tolerance specified (should still monitor).""" - params = { - make_tol_param(TEST_TOPIC): 15.0 - } - - ros2_monitor_node = launch_ros.actions.Node( - package='greenwave_monitor', - executable='greenwave_monitor', - name=MONITOR_NODE_NAME, - namespace=MONITOR_NODE_NAMESPACE, - parameters=[params], - output='screen' - ) - - publisher = create_minimal_publisher( - TEST_TOPIC, TEST_FREQUENCY, 'imu', '_tol_only', - enable_diagnostics=False - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestToleranceOnlyParameter(RosNodeTestCase): - """Test that specifying only tolerance still starts monitoring.""" - - TEST_NODE_NAME = 'tol_only_test_node' - - def test_tolerance_only_starts_monitoring(self): - """Test that specifying only tolerance starts monitoring (any param triggers add_topic).""" - time.sleep(2.0) - - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=5.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - f'Should monitor topic when tolerance is set, got {len(received_diagnostics)}' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_param_yaml.py b/greenwave_monitor/test/parameters/test_param_yaml.py deleted file mode 100644 index 90895cc..0000000 --- a/greenwave_monitor/test/parameters/test_param_yaml.py +++ /dev/null @@ -1,145 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Test: load topic configuration from YAML parameter file.""" - -import os -import tempfile -import time -import unittest - -from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, - create_minimal_publisher, - find_best_diagnostic, - MONITOR_NODE_NAME, - MONITOR_NODE_NAMESPACE, - RosNodeTestCase, -) -import launch -import launch_ros.actions -import launch_testing -import pytest - - -YAML_TOPIC = '/yaml_config_topic' -NESTED_TOPIC = '/nested_yaml_topic' -TEST_FREQUENCY = 50.0 -NESTED_FREQUENCY = 25.0 -TEST_TOLERANCE = 10.0 - - -@pytest.mark.launch_test -def generate_test_description(): - """Test loading parameters from a YAML file.""" - # Write YAML manually - demonstrates both flat dotted keys and nested dict formats - # Use full namespace path for node parameters - yaml_content = ( - f'/{MONITOR_NODE_NAMESPACE}/{MONITOR_NODE_NAME}:\n' - f' ros__parameters:\n' - f' # Flat dotted key format (requires quotes)\n' - f' "greenwave_diagnostics.{YAML_TOPIC}.expected_frequency": {TEST_FREQUENCY}\n' - f' "greenwave_diagnostics.{YAML_TOPIC}.tolerance": {TEST_TOLERANCE}\n' - f' # Nested dictionary format\n' - f' greenwave_diagnostics:\n' - f' {NESTED_TOPIC}:\n' - f' expected_frequency: {NESTED_FREQUENCY}\n' - f' tolerance: {TEST_TOLERANCE}\n' - ) - - yaml_dir = tempfile.mkdtemp() - yaml_path = os.path.join(yaml_dir, 'test_params.yaml') - with open(yaml_path, 'w') as f: - f.write(yaml_content) - - ros2_monitor_node = launch_ros.actions.Node( - package='greenwave_monitor', - executable='greenwave_monitor', - name=MONITOR_NODE_NAME, - namespace=MONITOR_NODE_NAMESPACE, - parameters=[yaml_path], - output='screen' - ) - - publisher = create_minimal_publisher( - YAML_TOPIC, TEST_FREQUENCY, 'imu', '_yaml' - ) - - nested_publisher = create_minimal_publisher( - NESTED_TOPIC, NESTED_FREQUENCY, 'imu', '_nested_yaml' - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - nested_publisher, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -class TestYamlParameterFile(RosNodeTestCase): - """Test loading topic configuration from YAML parameter file.""" - - TEST_NODE_NAME = 'yaml_test_node' - - def test_topic_configured_via_yaml(self): - """Test that topic is monitored when configured via YAML file.""" - time.sleep(2.0) - - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, YAML_TOPIC, expected_count=3, timeout_sec=10.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - 'Expected diagnostics from YAML-configured topic' - ) - best_status, _ = find_best_diagnostic( - received_diagnostics, TEST_FREQUENCY, 'imu' - ) - self.assertIsNotNone( - best_status, - 'Should have valid frame rate from YAML config' - ) - - def test_nested_dict_topic_configured_via_yaml(self): - """Test that topic configured via nested YAML dict is monitored.""" - time.sleep(2.0) - - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, NESTED_TOPIC, expected_count=3, timeout_sec=10.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - 'Expected diagnostics from nested YAML-configured topic' - ) - best_status, _ = find_best_diagnostic( - received_diagnostics, NESTED_FREQUENCY, 'imu' - ) - self.assertIsNotNone( - best_status, - 'Should have valid frame rate from nested YAML config' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_topic_parameters.py b/greenwave_monitor/test/parameters/test_topic_parameters.py deleted file mode 100644 index 4901635..0000000 --- a/greenwave_monitor/test/parameters/test_topic_parameters.py +++ /dev/null @@ -1,118 +0,0 @@ -#!/usr/bin/env python3 - -# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -# Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Tests for parameter-based topic configuration - both frequency and tolerance.""" - -import time -import unittest - -from greenwave_monitor.test_utils import ( - collect_diagnostics_for_topic, - create_minimal_publisher, - create_monitor_node, - find_best_diagnostic, - MONITOR_NODE_NAME, - RosNodeTestCase, -) -import launch -import launch_testing -from launch_testing import post_shutdown_test -from launch_testing.asserts import assertExitCodes -import pytest - - -TEST_TOPIC = '/param_test_topic' -TEST_FREQUENCY = 50.0 -TEST_TOLERANCE = 10.0 - - -@pytest.mark.launch_test -def generate_test_description(): - """Generate launch description with both frequency and tolerance set.""" - topic_configs = { - TEST_TOPIC: { - 'expected_frequency': TEST_FREQUENCY, - 'tolerance': TEST_TOLERANCE - } - } - - ros2_monitor_node = create_monitor_node( - topic_configs=topic_configs - ) - - publisher = create_minimal_publisher( - TEST_TOPIC, TEST_FREQUENCY, 'imu', '_param_test' - ) - - return ( - launch.LaunchDescription([ - ros2_monitor_node, - publisher, - launch_testing.actions.ReadyToTest() - ]), {} - ) - - -@post_shutdown_test() -class TestTopicParametersPostShutdown(RosNodeTestCase): - """Post-shutdown tests.""" - - TEST_NODE_NAME = 'shutdown_test_node' - - def test_node_shutdown(self, proc_info): - """Test that the node shuts down correctly.""" - available_nodes = self.test_node.get_node_names() - self.assertNotIn(MONITOR_NODE_NAME, available_nodes) - assertExitCodes(proc_info, allowable_exit_codes=[0]) - - -class TestTopicParameters(RosNodeTestCase): - """Tests for parameter-based topic configuration.""" - - TEST_NODE_NAME = 'topic_params_test_node' - - def test_topic_configured_via_parameters(self): - """Test that topic is monitored when configured via parameters.""" - time.sleep(2.0) - - received_diagnostics = collect_diagnostics_for_topic( - self.test_node, TEST_TOPIC, expected_count=3, timeout_sec=10.0 - ) - - self.assertGreaterEqual( - len(received_diagnostics), 3, - f'Expected at least 3 diagnostics for {TEST_TOPIC}, got {len(received_diagnostics)}' - ) - best_status, best_values = find_best_diagnostic( - received_diagnostics, TEST_FREQUENCY, 'imu' - ) - self.assertIsNotNone( - best_status, - 'Should have received diagnostics with valid frame_rate_node' - ) - frame_rate_node = best_values[0] - tolerance = TEST_FREQUENCY * TEST_TOLERANCE / 100.0 - self.assertAlmostEqual( - frame_rate_node, TEST_FREQUENCY, delta=tolerance, - msg=f'Frame rate {frame_rate_node} not within {tolerance} of {TEST_FREQUENCY}' - ) - - -if __name__ == '__main__': - unittest.main() diff --git a/greenwave_monitor/test/parameters/test_parameters.py b/greenwave_monitor/test/test_parameters.py similarity index 100% rename from greenwave_monitor/test/parameters/test_parameters.py rename to greenwave_monitor/test/test_parameters.py From e0327dd51141d9bb1fd3987b7cf7ebf38771b805 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 8 Jan 2026 00:23:10 -0800 Subject: [PATCH 22/33] Refactor UI adaptor Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 130 +----- .../greenwave_monitor/ui_adaptor.py | 402 +++++++++--------- .../test/test_greenwave_monitor.py | 4 +- greenwave_monitor/test/test_parameters.py | 14 +- 4 files changed, 220 insertions(+), 330 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index b7c7e8a..0d209ef 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -25,15 +25,17 @@ from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus 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, ) from greenwave_monitor_interfaces.srv import ManageTopic import launch_ros -from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue -from rcl_interfaces.srv import GetParameters, SetParameters +from rcl_interfaces.msg import ParameterType, ParameterValue import rclpy from rclpy.node import Node @@ -57,16 +59,6 @@ MONITOR_NODE_NAMESPACE = 'test_namespace' -def make_freq_param(topic: str) -> str: - """Build frequency parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{FREQ_SUFFIX}' - - -def make_tol_param(topic: str) -> str: - """Build tolerance parameter name for a topic.""" - return f'{TOPIC_PARAM_PREFIX}{topic}{TOL_SUFFIX}' - - def make_enabled_param(topic: str) -> str: """Build enabled parameter name for a topic.""" return f'{TOPIC_PARAM_PREFIX}{topic}{ENABLED_SUFFIX}' @@ -77,77 +69,19 @@ def set_parameter(test_node: Node, param_name: str, value, node_namespace: str = MONITOR_NODE_NAMESPACE, timeout_sec: float = 10.0) -> bool: """Set a parameter on a node using rclpy service client.""" - if node_namespace: - full_node_name = f'/{node_namespace}/{node_name}' - else: - full_node_name = f'/{node_name}' - service_name = f'{full_node_name}/set_parameters' - - client = test_node.create_client(SetParameters, service_name) - if not client.wait_for_service(timeout_sec=5.0): - return False - - param = Parameter() - param.name = param_name - param.value = ParameterValue() - - 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) - - request = SetParameters.Request() - request.parameters = [param] - - future = client.call_async(request) - rclpy.spin_until_future_complete(test_node, future, timeout_sec=timeout_sec) - - test_node.destroy_client(client) - - if future.result() is None: - return False - return all(r.successful for r in future.result().results) + 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, Optional[float]]: """Get a parameter from a node using rclpy service client.""" - if node_namespace: - full_node_name = f'/{node_namespace}/{node_name}' - else: - full_node_name = f'/{node_name}' - service_name = f'{full_node_name}/get_parameters' - - client = test_node.create_client(GetParameters, service_name) - if not client.wait_for_service(timeout_sec=5.0): - return False, None - - request = GetParameters.Request() - request.names = [param_name] - - future = client.call_async(request) - rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0) - - test_node.destroy_client(client) - - if future.result() is None or not future.result().values: - return False, None - - param_value = future.result().values[0] - if param_value.type == ParameterType.PARAMETER_DOUBLE: - return True, param_value.double_value - elif param_value.type == ParameterType.PARAMETER_INTEGER: - return True, float(param_value.integer_value) - return False, None + 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, @@ -155,32 +89,10 @@ def delete_parameter(test_node: Node, param_name: str, node_namespace: str = MONITOR_NODE_NAMESPACE, timeout_sec: float = 10.0) -> bool: """Delete a parameter from a node using rclpy service client.""" - if node_namespace: - full_node_name = f'/{node_namespace}/{node_name}' - else: - full_node_name = f'/{node_name}' - service_name = f'{full_node_name}/set_parameters' - - client = test_node.create_client(SetParameters, service_name) - if not client.wait_for_service(timeout_sec=5.0): - return False - - param = Parameter() - param.name = param_name - param.value = ParameterValue() - param.value.type = ParameterType.PARAMETER_NOT_SET - - request = SetParameters.Request() - request.parameters = [param] - - future = client.call_async(request) - rclpy.spin_until_future_complete(test_node, future, timeout_sec=timeout_sec) - - test_node.destroy_client(client) - - if future.result() is None: - return False - return all(r.successful for r in future.result().results) + 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( @@ -390,14 +302,10 @@ 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' - ) - - return manage_topic_client +def create_manage_topic_client(node: Node, namespace: str = MONITOR_NODE_NAMESPACE, + node_name: str = MONITOR_NODE_NAME): + """Create the manage_topic service client for the monitor node.""" + return node.create_client(ManageTopic, f'/{namespace}/{node_name}/manage_topic') class RosNodeTestCase(unittest.TestCase, ABC): diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 074b8d2..8412f66 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -35,7 +35,6 @@ """ from dataclasses import dataclass -from enum import Enum import math import threading import time @@ -56,29 +55,136 @@ DEFAULT_TOLERANCE_PERCENT = 5.0 -class TopicParamField(Enum): - """Type of topic parameter field.""" +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.""" + if not node_namespace or node_namespace == '/': + return f'/{node_name}' + return f'{node_namespace}/{node_name}' if is_client else f'/{node_namespace}/{node_name}' + + +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. + + Args: + node: The ROS node to use for service calls + target_node: Full name of target node (e.g., '/my_node' or '/ns/my_node') + params: Dict mapping parameter names to values + timeout_sec: Service call timeout + + Returns: + Tuple of (all_successful, list of failure reasons) + """ + 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. + + Args: + node: The ROS node to use for service calls + target_node: Full name of target node (e.g., '/my_node' or '/ns/my_node') + param_names: List of parameter names to retrieve + 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} - NONE = 0 - FREQUENCY = 1 - TOLERANCE = 2 + 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) -def parse_topic_param_name(param_name: str) -> tuple[str, TopicParamField]: - """Parse parameter name to extract topic name and field type.""" + 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_float(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 ('freq', 'tol', or '').""" if not param_name.startswith(TOPIC_PARAM_PREFIX): - return ('', TopicParamField.NONE) + return ('', '') 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 (topic_and_field[:-len(FREQ_SUFFIX)], 'freq') + if topic_and_field.endswith(TOL_SUFFIX): + return (topic_and_field[:-len(TOL_SUFFIX)], 'tol') - return ('', TopicParamField.NONE) + return ('', '') def param_value_to_float(value: ParameterValue) -> float | None: @@ -94,6 +200,14 @@ def param_value_to_float(value: ParameterValue) -> float | None: return None +_STATUS_LEVEL_MAP = { + DiagnosticStatus.OK: 'OK', + DiagnosticStatus.WARN: 'WARN', + DiagnosticStatus.ERROR: 'ERROR', + DiagnosticStatus.STALE: 'STALE', +} + + @dataclass class UiDiagnosticData: """ @@ -102,7 +216,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 = '-' @@ -115,16 +228,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': @@ -183,20 +287,6 @@ def _setup_ros_components(self): manage_service_name ) - # Parameter service clients for querying and setting parameters - 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' - ) - self.set_params_client = self.node.create_client( - SetParameters, - f'{self.monitor_node_name}/set_parameters' - ) - # Track pending node queries to prevent garbage collection self._pending_node_queries: Dict[str, dict] = {} @@ -206,37 +296,26 @@ def _setup_ros_components(self): def _fetch_initial_parameters_callback(self): """Timer callback to fetch initial parameters from all nodes.""" - # Cancel timer - we only run once if self._initial_params_timer is not None: self._initial_params_timer.cancel() self._initial_params_timer = None - # Get all nodes in the system - node_names_and_namespaces = self.node.get_node_names_and_namespaces() - - for node_name, node_namespace in node_names_and_namespaces: - # Skip our own node + for node_name, node_namespace in self.node.get_node_names_and_namespaces(): if node_name == self.node.get_name(): continue - if node_namespace == '/': - full_node_name = f'/{node_name}' - else: - full_node_name = f'{node_namespace}/{node_name}' - - self._query_node_parameters(full_node_name) + 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(self, full_node_name: str): + 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') - # Check if service exists (non-blocking) if not list_client.service_is_ready(): self.node.destroy_client(list_client) return - # Store client to prevent garbage collection query_id = full_node_name self._pending_node_queries[query_id] = { 'node_name': full_node_name, @@ -251,27 +330,22 @@ def _query_node_parameters(self, full_node_name: str): list_future = list_client.call_async(list_request) list_future.add_done_callback( - lambda f, qid=query_id: self._on_node_list_response(f, qid)) + lambda f, qid=query_id: self._on_list_params_response(f, qid)) - def _on_node_list_response(self, future, query_id: str): + 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: - return - - if future.result() is None: + if not query or future.result() is None: self._cleanup_node_query(query_id) return - all_param_names = future.result().result.names - param_names = [n for n in all_param_names if n.startswith(TOPIC_PARAM_PREFIX)] - + 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 - # Create get_parameters client full_node_name = query['node_name'] get_client = self.node.create_client( GetParameters, f'{full_node_name}/get_parameters') @@ -289,47 +363,32 @@ def _on_node_list_response(self, future, query_id: str): get_future = get_client.call_async(get_request) get_future.add_done_callback( - lambda f, qid=query_id: self._on_node_get_response(f, qid)) + 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_node_get_response(self, future, query_id: str): + 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: - return - - if future.result() is None: + if not query or future.result() is None: self._cleanup_node_query(query_id) return - param_names = query['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): + for name, value in zip(query['param_names'], future.result().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: + if not topic_name or not field: continue - if topic_name not in topic_configs: topic_configs[topic_name] = {} + topic_configs[topic_name][field] = numeric_value - 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) @@ -351,6 +410,15 @@ def _cleanup_node_query(self, query_id: str): 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: """ Extract topic name from diagnostic status name. @@ -394,30 +462,30 @@ def _on_parameter_event(self, msg: ParameterEvent): continue topic_name, field = parse_topic_param_name(param.name) - if not topic_name or field == TopicParamField.NONE: + if not topic_name or field == '': continue with self.data_lock: current = self.expected_frequencies.get(topic_name, (0.0, 0.0)) - if field == TopicParamField.FREQUENCY: + 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 == TopicParamField.TOLERANCE: + 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 == TopicParamField.NONE: + if not topic_name or field == '': continue with self.data_lock: - if field == TopicParamField.FREQUENCY: + if field == 'freq': self.expected_frequencies.pop(topic_name, None) - elif field == TopicParamField.TOLERANCE: + elif field == 'tol': current = self.expected_frequencies.get(topic_name) if current and current[0] > 0: self.expected_frequencies[topic_name] = ( @@ -430,37 +498,22 @@ def toggle_topic_monitoring(self, topic_name: str): 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 not response.success: - action = 'start' if request.add_topic else 'stop' - self.node.get_logger().error( - f'Failed to {action} monitoring: {response.message}') - return + action = 'start' if request.add_topic else 'stop' + response = self._call_service(self.manage_topic_client, request) - if not request.add_topic and topic_name in self.ui_diagnostics: - # Remove the topic from the UI diagnostics, but keep its settings - del self.ui_diagnostics[topic_name] + if response is None: + self.node.get_logger().error(f'Failed to {action} monitoring: Service call timed out') + 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 response.success: + self.node.get_logger().error(f'Failed to {action} monitoring: {response.message}') + return + if not request.add_topic and topic_name in self.ui_diagnostics: + del self.ui_diagnostics[topic_name] def _find_node_with_topic_param(self, topic_name: str) -> str: """ @@ -469,22 +522,14 @@ def _find_node_with_topic_param(self, topic_name: str) -> str: Searches all nodes in the system for the parameter. Falls back to the monitor node if no node is found with the parameter. """ - freq_param_name = f'{TOPIC_PARAM_PREFIX}{topic_name}{FREQ_SUFFIX}' + freq_param_name = make_freq_param(topic_name) - # Get all nodes in the system - node_names_and_namespaces = self.node.get_node_names_and_namespaces() - - for node_name, node_namespace in node_names_and_namespaces: - # Skip our own node + for node_name, node_namespace in self.node.get_node_names_and_namespaces(): if node_name == self.node.get_name(): continue - if node_namespace == '/': - full_node_name = f'/{node_name}' - else: - full_node_name = f'{node_namespace}/{node_name}' + full_node_name = build_full_node_name(node_name, node_namespace, is_client=True) - # Check if this node has the frequency parameter list_client = self.node.create_client( ListParameters, f'{full_node_name}/list_parameters') if not list_client.wait_for_service(timeout_sec=0.5): @@ -495,15 +540,12 @@ def _find_node_with_topic_param(self, topic_name: str) -> str: list_request.prefixes = [freq_param_name] list_request.depth = 1 - future = list_client.call_async(list_request) - rclpy.spin_until_future_complete(self.node, future, timeout_sec=1.0) + result = self._call_service(list_client, list_request, timeout_sec=1.0) self.node.destroy_client(list_client) - if future.result() is not None: - if freq_param_name in future.result().result.names: - return full_node_name + if result is not None and freq_param_name in result.result.names: + return full_node_name - # Fall back to the monitor node return self.monitor_node_name def set_expected_frequency(self, @@ -513,88 +555,26 @@ def set_expected_frequency(self, clear: bool = False ) -> tuple[bool, str]: """Set or clear the expected frequency for a topic via parameters.""" - # Find a node that has the parameter, or fall back to monitor node target_node = self._find_node_with_topic_param(topic_name) + action = 'clear' if clear else 'set' - # Create a client to the target node's set_parameters service - set_params_client = self.node.create_client( - SetParameters, f'{target_node}/set_parameters') - if not set_params_client.wait_for_service(timeout_sec=1.0): - self.node.destroy_client(set_params_client) - return False, f'Could not connect to {target_node}/set_parameters service.' - - freq_param_name = f'{TOPIC_PARAM_PREFIX}{topic_name}{FREQ_SUFFIX}' - tol_param_name = f'{TOPIC_PARAM_PREFIX}{topic_name}{TOL_SUFFIX}' - - request = SetParameters.Request() - - if clear: - # Clear by setting frequency to NaN and tolerance to default - freq_param = Parameter() - freq_param.name = freq_param_name - freq_param.value = ParameterValue() - freq_param.value.type = ParameterType.PARAMETER_DOUBLE - freq_param.value.double_value = float('nan') - - tol_param = Parameter() - tol_param.name = tol_param_name - tol_param.value = ParameterValue() - tol_param.value.type = ParameterType.PARAMETER_DOUBLE - tol_param.value.double_value = DEFAULT_TOLERANCE_PERCENT - - request.parameters = [freq_param, tol_param] - else: - # Set frequency and tolerance parameters - freq_param = Parameter() - freq_param.name = freq_param_name - freq_param.value = ParameterValue() - freq_param.value.type = ParameterType.PARAMETER_DOUBLE - freq_param.value.double_value = expected_hz - - tol_param = Parameter() - tol_param.name = tol_param_name - tol_param.value = ParameterValue() - tol_param.value.type = ParameterType.PARAMETER_DOUBLE - tol_param.value.double_value = tolerance_percent - - request.parameters = [freq_param, tol_param] + 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, + } - try: - future = set_params_client.call_async(request) - rclpy.spin_until_future_complete(self.node, future, timeout_sec=3.0) - self.node.destroy_client(set_params_client) - - 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 - - results = future.result().results - all_successful = all(r.successful for r in results) - - if not all_successful: - action = 'clear' if clear else 'set' - reasons = [r.reason for r in results if not r.successful and r.reason] - error_msg = f'Failed to {action} expected frequency: {"; ".join(reasons)}' - self.node.get_logger().error(error_msg) - return False, error_msg + success, failures = set_ros_parameters(self.node, target_node, params) - with self.data_lock: - if clear: - self.expected_frequencies.pop(topic_name, None) - else: - self.expected_frequencies[topic_name] = (expected_hz, tolerance_percent) + if not success: + return False, f'Failed to {action} expected frequency: {"; ".join(failures)}' - action = 'Cleared' if clear else 'Set' - return True, f'{action} expected frequency for {topic_name}' + with self.data_lock: + if clear: + self.expected_frequencies.pop(topic_name, None) + else: + self.expected_frequencies[topic_name] = (expected_hz, tolerance_percent) - except Exception as e: - self.node.destroy_client(set_params_client) - 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 + 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.""" diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 2ecbb72..4221985 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -24,9 +24,9 @@ from greenwave_monitor.test_utils import ( call_manage_topic_service, collect_diagnostics_for_topic, + create_manage_topic_client, create_minimal_publisher, create_monitor_node, - create_service_clients, find_best_diagnostic, MANAGE_TOPIC_TEST_CONFIG, MONITOR_NODE_NAME, @@ -122,7 +122,7 @@ 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 = create_service_clients( + manage_client = create_manage_topic_client( self.test_node, MONITOR_NODE_NAMESPACE, MONITOR_NODE_NAME ) service_available = wait_for_service_connection( diff --git a/greenwave_monitor/test/test_parameters.py b/greenwave_monitor/test/test_parameters.py index 3c38efc..a8c90ce 100644 --- a/greenwave_monitor/test/test_parameters.py +++ b/greenwave_monitor/test/test_parameters.py @@ -27,21 +27,24 @@ from greenwave_monitor.test_utils import ( call_manage_topic_service, collect_diagnostics_for_topic, + create_manage_topic_client, create_minimal_publisher, - create_service_clients, delete_parameter, find_best_diagnostic, get_parameter, make_enabled_param, - make_freq_param, - make_tol_param, MONITOR_NODE_NAME, MONITOR_NODE_NAMESPACE, RosNodeTestCase, set_parameter, wait_for_service_connection, ) -from greenwave_monitor.ui_adaptor import ENABLED_SUFFIX, TOPIC_PARAM_PREFIX +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 @@ -476,7 +479,7 @@ def test_add_topic_enables_existing_node_parameter(self): self.assertTrue(len(future.result().values) > 0) # Create manage_topic client - manage_topic_client = create_service_clients(self.test_node) + manage_topic_client = create_manage_topic_client(self.test_node) self.assertTrue( wait_for_service_connection( self.test_node, manage_topic_client, @@ -518,4 +521,3 @@ def test_add_topic_enables_existing_node_parameter(self): if __name__ == '__main__': unittest.main() - From e4d02afad7bb048b0d1b81490918f76b37c2e013 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 8 Jan 2026 16:12:32 -0800 Subject: [PATCH 23/33] Use parameters instead of ManageTopic Signed-off-by: Blake McHale --- docs/images/SERVICES.md | 40 +- greenwave_monitor/CMakeLists.txt | 1 - .../greenwave_monitor/test_utils.py | 32 +- .../greenwave_monitor/ui_adaptor.py | 189 ++++----- .../include/greenwave_monitor.hpp | 38 +- greenwave_monitor/package.xml | 1 - greenwave_monitor/src/greenwave_monitor.cpp | 374 ++++++++++++------ .../test/test_greenwave_monitor.py | 117 +++--- greenwave_monitor/test/test_parameters.py | 59 +-- .../test/test_topic_monitoring_integration.py | 70 ++-- greenwave_monitor_interfaces/CMakeLists.txt | 32 -- greenwave_monitor_interfaces/package.xml | 19 - .../srv/ManageTopic.srv | 24 -- scripts/build_debian_packages.sh | 18 +- 14 files changed, 495 insertions(+), 519 deletions(-) delete mode 100644 greenwave_monitor_interfaces/CMakeLists.txt delete mode 100644 greenwave_monitor_interfaces/package.xml delete mode 100644 greenwave_monitor_interfaces/srv/ManageTopic.srv diff --git a/docs/images/SERVICES.md b/docs/images/SERVICES.md index 7b8b1ac..a439a17 100644 --- a/docs/images/SERVICES.md +++ b/docs/images/SERVICES.md @@ -1,45 +1,51 @@ -# Services and Parameters +# Parameters -The Greenwave Monitor provides a `ManageTopic` service to dynamically add or remove topics from monitoring. Expected frequencies are configured via ROS parameters, 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 Service +## 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 ``` ## Expected Frequency Parameters Expected frequencies are configured via ROS parameters with the following naming convention: -- `topics..expected_frequency` - Expected publish rate in Hz -- `topics..tolerance` - Tolerance percentage (default: 5.0%) +- `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 param set /greenwave_monitor topics./topic2.expected_frequency 30.0 -ros2 param set /greenwave_monitor topics./topic2.tolerance 10.0 +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: -```bash -ros2 param delete /greenwave_monitor topics./topic2.expected_frequency +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 ``` -Parameters can also be set at launch time: +Or via command line: ```bash -ros2 run greenwave_monitor greenwave_monitor --ros-args -p topics./topic2.expected_frequency:=30.0 -p topics./topic2.tolerance:=10.0 +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 ac4668f..7c87791 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -34,7 +34,6 @@ ament_target_dependencies(greenwave_monitor rclcpp std_msgs diagnostic_msgs - greenwave_monitor_interfaces ) target_link_libraries(greenwave_monitor greenwave_diagnostics) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 0d209ef..28fdbc6 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -20,7 +20,7 @@ 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 @@ -33,7 +33,6 @@ TOL_SUFFIX, TOPIC_PARAM_PREFIX, ) -from greenwave_monitor_interfaces.srv import ManageTopic import launch_ros from rcl_interfaces.msg import ParameterType, ParameterValue import rclpy @@ -76,7 +75,7 @@ def set_parameter(test_node: Node, param_name: str, value, def get_parameter(test_node: Node, param_name: str, node_name: str = MONITOR_NODE_NAME, - node_namespace: str = MONITOR_NODE_NAMESPACE) -> Tuple[bool, Optional[float]]: + 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]) @@ -158,27 +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 collect_diagnostics_for_topic(node: Node, topic_name: str, expected_count: int = 5, @@ -302,12 +280,6 @@ def verify_diagnostic_values(status: DiagnosticStatus, return errors -def create_manage_topic_client(node: Node, namespace: str = MONITOR_NODE_NAMESPACE, - node_name: str = MONITOR_NODE_NAME): - """Create the manage_topic service client for the monitor node.""" - return 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. diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 8412f66..79817a8 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -27,7 +27,8 @@ timestamp of the last update for each topic. In addition to passively subscribing, `GreenwaveUiAdaptor` exposes: -- ManageTopic service client: start/stop monitoring a topic (`toggle_topic_monitoring`). +- 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 @@ -41,7 +42,6 @@ from typing import Dict from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from greenwave_monitor_interfaces.srv import ManageTopic from rcl_interfaces.msg import Parameter, ParameterEvent, ParameterType, ParameterValue from rcl_interfaces.srv import GetParameters, ListParameters, SetParameters import rclpy @@ -57,9 +57,16 @@ 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.""" - if not node_namespace or node_namespace == '/': - return f'/{node_name}' - return f'{node_namespace}/{node_name}' if is_client else f'/{node_namespace}/{node_name}' + 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: @@ -100,15 +107,14 @@ def set_ros_parameters(node: Node, target_node: str, params: dict, """ Set one or more parameters on a target node. - Args: - node: The ROS node to use for service calls - target_node: Full name of target node (e.g., '/my_node' or '/ns/my_node') - params: Dict mapping parameter names to values - timeout_sec: Service call timeout - - Returns: - Tuple of (all_successful, list of failure reasons) + :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) @@ -138,14 +144,11 @@ def get_ros_parameters(node: Node, target_node: str, param_names: list, """ Get parameters from a target node. - Args: - node: The ROS node to use for service calls - target_node: Full name of target node (e.g., '/my_node' or '/ns/my_node') - param_names: List of parameter names to retrieve - timeout_sec: Service call timeout - - Returns: - Dict mapping parameter names to their values (float or None if not found/invalid) + :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)): @@ -165,7 +168,7 @@ def get_ros_parameters(node: Node, target_node: str, param_names: list, result = {} for name, value in zip(param_names, future.result().values): - result[name] = param_value_to_float(value) + result[name] = param_value_to_python(value) return result except Exception: node.destroy_client(client) @@ -173,7 +176,7 @@ def get_ros_parameters(node: Node, target_node: str, param_names: list, def parse_topic_param_name(param_name: str) -> tuple[str, str]: - """Parse parameter name to extract topic name and field type ('freq', 'tol', or '').""" + """Parse parameter name to extract topic name and field type.""" if not param_name.startswith(TOPIC_PARAM_PREFIX): return ('', '') @@ -183,20 +186,22 @@ def parse_topic_param_name(param_name: str) -> tuple[str, str]: 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_float(value: ParameterValue) -> float | None: - """Convert a ParameterValue to float if it's a numeric type.""" - if value.type == ParameterType.PARAMETER_DOUBLE: +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 float(value.integer_value) + return value.integer_value elif value.type == ParameterType.PARAMETER_STRING: - str_value = value.string_value - if str_value == 'nan' or str_value == 'NaN' or str_value == 'NAN': - return float('nan') + return value.string_value return None @@ -245,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 ROS parameters. 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() @@ -278,15 +285,6 @@ def _setup_ros_components(self): 10 ) - manage_service_name = f'{self.monitor_node_name}/manage_topic' - - self.node.get_logger().info(f'Connecting to monitor service: {manage_service_name}') - - self.manage_topic_client = self.node.create_client( - ManageTopic, - manage_service_name - ) - # Track pending node queries to prevent garbage collection self._pending_node_queries: Dict[str, dict] = {} @@ -377,19 +375,31 @@ def _on_get_params_response(self, future, query_id: str): 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): - numeric_value = param_value_to_float(value) - if numeric_value is None: + 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] = numeric_value + 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) @@ -454,25 +464,32 @@ def _on_diagnostics(self, msg: DiagnosticArray): self.ui_diagnostics[topic_name] = ui_data def _on_parameter_event(self, msg: ParameterEvent): - """Process parameter change events from the monitor node.""" - # Process changed and new parameters + """Process parameter change events from any node.""" 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 == '': continue 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 + + 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) @@ -483,7 +500,10 @@ def _on_parameter_event(self, msg: ParameterEvent): continue with self.data_lock: - if field == 'freq': + 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) + elif field == 'freq': self.expected_frequencies.pop(topic_name, None) elif field == 'tol': current = self.expected_frequencies.get(topic_name) @@ -492,62 +512,26 @@ def _on_parameter_event(self, msg: ParameterEvent): current[0], DEFAULT_TOLERANCE_PERCENT) 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 - - request = ManageTopic.Request() - request.topic_name = topic_name + """Toggle monitoring for a topic via enabled parameter.""" with self.data_lock: - request.add_topic = topic_name not in self.ui_diagnostics + 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' - action = 'start' if request.add_topic else 'stop' - response = self._call_service(self.manage_topic_client, request) + enabled_param = f'{TOPIC_PARAM_PREFIX}{topic_name}{ENABLED_SUFFIX}' + success, failures = set_ros_parameters( + self.node, target_node, {enabled_param: new_enabled}) - if response is None: - self.node.get_logger().error(f'Failed to {action} monitoring: Service call timed out') + if not success: + self.node.get_logger().error( + f'Failed to {action} monitoring: {"; ".join(failures)}') return with self.data_lock: - if not response.success: - self.node.get_logger().error(f'Failed to {action} monitoring: {response.message}') - return - if not request.add_topic and topic_name in self.ui_diagnostics: + if not new_enabled and topic_name in self.ui_diagnostics: del self.ui_diagnostics[topic_name] - def _find_node_with_topic_param(self, topic_name: str) -> str: - """ - Find a node that has the frequency parameter for this topic. - - Searches all nodes in the system for the parameter. Falls back to the - monitor node if no node is found with the parameter. - """ - freq_param_name = make_freq_param(topic_name) - - 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) - - list_client = self.node.create_client( - ListParameters, f'{full_node_name}/list_parameters') - if not list_client.wait_for_service(timeout_sec=0.5): - self.node.destroy_client(list_client) - continue - - list_request = ListParameters.Request() - list_request.prefixes = [freq_param_name] - list_request.depth = 1 - - result = self._call_service(list_client, list_request, timeout_sec=1.0) - self.node.destroy_client(list_client) - - if result is not None and freq_param_name in result.result.names: - return full_node_name - - return self.monitor_node_name - def set_expected_frequency(self, topic_name: str, expected_hz: float = 0.0, @@ -555,7 +539,8 @@ def set_expected_frequency(self, clear: bool = False ) -> tuple[bool, str]: """Set or clear the expected frequency for a topic via parameters.""" - target_node = self._find_node_with_topic_param(topic_name) + with self.data_lock: + target_node = self.topic_to_node.get(topic_name, self.monitor_node_name) action = 'clear' if clear else 'set' params = { diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 4c30cc2..1f59170 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "rclcpp/rclcpp.hpp" @@ -29,8 +30,16 @@ #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" #include "greenwave_diagnostics.hpp" -#include "greenwave_monitor_interfaces/srv/manage_topic.hpp" + +struct TopicValidationResult +{ + bool valid = false; + std::string topic; + std::string message_type; + std::string error_message; +}; class GreenwaveMonitor : public rclcpp::Node { @@ -44,9 +53,21 @@ class GreenwaveMonitor : public rclcpp::Node 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 external_on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr msg); + + 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 fetch_external_topic_map(); bool add_topic( const std::string & topic, std::string & message, @@ -54,9 +75,6 @@ class GreenwaveMonitor : public rclcpp::Node bool remove_topic(const std::string & topic, std::string & message); - bool try_set_external_enabled_param( - const std::string & topic, bool enabled, std::string & message); - bool has_header_from_type(const std::string & type_name); std::set get_topics_from_parameters(); @@ -70,6 +88,8 @@ class GreenwaveMonitor : public rclcpp::Node std::unique_ptr> greenwave_diagnostics_; std::vector> subscriptions_; rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Service::SharedPtr - manage_topic_service_; + 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/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 b4a8aac..ce3cfb0 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -16,6 +16,9 @@ // SPDX-License-Identifier: Apache-2.0 #include "greenwave_monitor.hpp" + +#include + #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" @@ -36,33 +39,36 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) this->declare_parameter>("topics", {""}, descriptor); } - auto topics = this->get_parameter("topics").as_string_array(); - for (const auto & topic : topics) { + // Build external topic map and collect all topics to monitor + fetch_external_topic_map(); + + // Merge topics into one set + 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()) { - std::string message; - add_topic(topic, message); + all_topics.insert(topic); } } - // Also check for topics defined via greenwave_diagnostics..* parameters - auto topics_from_params = get_topics_from_parameters(); - for (const auto & topic : topics_from_params) { - if (greenwave_diagnostics_.find(topic) == greenwave_diagnostics_.end()) { - std::string message; - add_topic(topic, message); - } + // Add all starting topics to the monitor + for (const auto & topic : all_topics) { + std::string message; + add_topic(topic, message); } + // 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)); + // Register parameter change callback for synchronous validation + 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( @@ -91,15 +97,141 @@ 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) +{ + 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; +} + +rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + auto topic_opt = parse_enabled_topic_param(param.get_name()); + if (!topic_opt.has_value()) { + continue; + } + + if (param.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + continue; + } + + 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 (greenwave_diagnostics_.find(topic) == greenwave_diagnostics_.end()) { + result.successful = false; + result.reason = "Topic not being monitored: " + topic; + return result; + } + } + } + + return result; +} + +void GreenwaveMonitor::on_parameter_event( + const rcl_interfaces::msg::ParameterEvent::SharedPtr msg) { - if (request->add_topic) { - // No retries for service calls - caller can retry if needed - response->success = add_topic(request->topic_name, response->message, 0); + if (msg->node == this->get_fully_qualified_name()) { + internal_on_parameter_event(msg); } else { - response->success = remove_topic(request->topic_name, response->message); + external_on_parameter_event(msg); + } +} + +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); +} + +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; + } + } + + // 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()); } } @@ -188,19 +320,17 @@ 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, - int max_retries, double retry_period_s) +TopicValidationResult GreenwaveMonitor::validate_add_topic( + const std::string & topic, int max_retries, double retry_period_s) { - // Check if topic already exists + TopicValidationResult result; + result.topic = topic; + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { - message = "Topic already being monitored"; - return false; + result.error_message = "Topic already monitored: " + topic; + return result; } - RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); - - // Get publishers for this topic with retry logic std::vector publishers; for (int attempt = 0; attempt <= max_retries; ++attempt) { publishers = this->get_publishers_info_by_topic(topic); @@ -218,31 +348,35 @@ bool GreenwaveMonitor::add_topic( } if (publishers.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to find publishers for topic '%s'", topic.c_str()); - message = "Failed to find publishers for topic"; - return false; + result.error_message = "No publishers found for topic: " + topic; + return result; } - // Get the message type from the first publisher - const std::string type = publishers[0].topic_type(); + // Check if any external node already has monitoring enabled for this 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; + } - // Try to enable monitoring on an existing node with the parameter - try { - if (try_set_external_enabled_param(topic, true, message)) { - return true; - } - // If already monitored externally, return false - if (message.find("already being monitored") != std::string::npos) { - return false; - } - } catch (const std::exception & e) { - RCLCPP_WARN( - this->get_logger(), - "Exception while checking for existing monitoring on topic '%s': %s", - topic.c_str(), e.what()); + 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; } - // No existing node with the parameter found, create local GreenwaveDiagnostics + const std::string & topic = validated.topic; + const std::string & type = validated.message_type; + + RCLCPP_INFO(this->get_logger(), "Adding subscription for topic '%s'", topic.c_str()); + auto sub = this->create_generic_subscription( topic, type, @@ -261,25 +395,29 @@ bool GreenwaveMonitor::add_topic( std::make_unique(*this, topic, diagnostics_config)); - message = "Successfully added topic"; + message = "Successfully added topic: " + topic; return true; } +bool GreenwaveMonitor::add_topic( + const std::string & topic, std::string & message, + int max_retries, double retry_period_s) +{ + 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()) { - // Topic not monitored locally, try to find a publisher node with the enabled parameter - try { - return try_set_external_enabled_param(topic, false, message); - } catch (const std::exception & e) { - RCLCPP_WARN( - this->get_logger(), - "Exception while checking for existing monitoring on topic '%s': %s", - topic.c_str(), e.what()); - message = "Exception while checking external nodes"; - return false; - } + message = "Nothing to remove, topic not being monitored"; + return true; } // Find and remove the subscription @@ -301,81 +439,83 @@ bool GreenwaveMonitor::remove_topic(const std::string & topic, std::string & mes return true; } -bool GreenwaveMonitor::try_set_external_enabled_param( - const std::string & topic, bool enabled, std::string & message) +void GreenwaveMonitor::fetch_external_topic_map() { - std::string enabled_param_name = - std::string(greenwave_diagnostics::constants::kTopicParamPrefix) + - topic + greenwave_diagnostics::constants::kEnabledSuffix; - - auto publishers = this->get_publishers_info_by_topic(topic); - if (publishers.empty()) { - message = "No publishers found for topic"; - return false; - } - - std::string our_ns = this->get_namespace(); - std::string our_name = this->get_name(); - std::string our_full_name = (our_ns == "/") ? - ("/" + our_name) : (our_ns + "/" + our_name); + 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 std::atomic temp_node_counter{0}; + 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_param_" + std::to_string(temp_node_counter++), + "_gw_init_" + std::to_string(temp_node_counter++), "/_greenwave_internal", temp_options); - for (const auto & pub_info : publishers) { - std::string node_name = pub_info.node_name(); - std::string node_namespace = pub_info.node_namespace(); - std::string full_node_name = (node_namespace == "/") ? - ("/" + node_name) : (node_namespace + "/" + node_name); + auto node_names = this->get_node_names(); + for (const auto & full_name : node_names) { + // Parse full node name to extract name and namespace + std::string node_namespace, node_name; + size_t last_slash = full_name.rfind('/'); + if (last_slash == std::string::npos || last_slash == 0) { + node_name = full_name; + node_namespace = "/"; + } else { + node_name = full_name.substr(last_slash + 1); + node_namespace = full_name.substr(0, last_slash); + if (node_namespace.empty()) { + node_namespace = "/"; + } + } + + std::string full_node_name = full_name; - if (full_node_name == our_full_name) { + if (full_node_name == our_node) { continue; } auto param_client = std::make_shared( temp_node, full_node_name); - if (!param_client->wait_for_service(std::chrono::milliseconds(500))) { + if (!param_client->wait_for_service(std::chrono::milliseconds(100))) { continue; } - if (!param_client->has_parameter(enabled_param_name)) { + auto param_names = param_client->list_parameters({"greenwave_diagnostics"}, 10).names; + if (param_names.empty()) { continue; } - auto current_params = param_client->get_parameters({enabled_param_name}); - if (!current_params.empty() && - current_params[0].get_type() == rclcpp::ParameterType::PARAMETER_BOOL && - current_params[0].as_bool() == enabled) - { - message = enabled ? - "Topic already being monitored on node: " + full_node_name : - "Topic already disabled on node: " + full_node_name; - return false; - } + auto params = param_client->get_parameters(param_names); + for (size_t i = 0; i < param_names.size(); ++i) { + const auto & name = param_names[i]; + const auto & param = params[i]; - auto results = param_client->set_parameters({ - rclcpp::Parameter(enabled_param_name, enabled) - }); - if (!results.empty() && results[0].successful) { - RCLCPP_INFO( - this->get_logger(), - "%s monitoring via parameter on node '%s' for topic '%s'", - enabled ? "Enabled" : "Disabled", - full_node_name.c_str(), topic.c_str()); - message = (enabled ? std::string("Enabled") : std::string("Disabled")) + - " monitoring on existing node: " + full_node_name; - return true; + 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_node_name; + RCLCPP_DEBUG(this->get_logger(), + "Found external monitoring for topic '%s' on node '%s'", + topic.c_str(), full_node_name.c_str()); + } } } - - message = "No external node with parameter " + enabled_param_name + " found"; - return false; } std::set GreenwaveMonitor::get_topics_from_parameters() diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 4221985..5eb18e5 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_manage_topic_client, create_minimal_publisher, create_monitor_node, 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,12 +54,12 @@ 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), - # Additional publishers for topic management tests (start with diagnostics disabled - # so add_topic can enable them) + 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', enable_diagnostics=False), create_minimal_publisher( @@ -120,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 = create_manage_topic_client( - 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.""" @@ -169,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's already disabled. - 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 enabled. - 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') - service_client = self.check_node_launches_successfully() + self.check_node_launches_successfully() TEST_TOPIC1 = '/test_topic1' TEST_TOPIC2 = '/test_topic2' @@ -224,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_parameters.py b/greenwave_monitor/test/test_parameters.py index a8c90ce..ff89c37 100644 --- a/greenwave_monitor/test/test_parameters.py +++ b/greenwave_monitor/test/test_parameters.py @@ -25,9 +25,7 @@ import unittest from greenwave_monitor.test_utils import ( - call_manage_topic_service, collect_diagnostics_for_topic, - create_manage_topic_client, create_minimal_publisher, delete_parameter, find_best_diagnostic, @@ -37,7 +35,6 @@ MONITOR_NODE_NAMESPACE, RosNodeTestCase, set_parameter, - wait_for_service_connection, ) from greenwave_monitor.ui_adaptor import ( ENABLED_SUFFIX, @@ -454,12 +451,12 @@ def test_delete_parameter_rejected(self): class TestEnableExistingNode(RosNodeTestCase): - """Tests for enabling/disabling monitoring on existing nodes.""" + """Tests for enabling/disabling monitoring via enabled parameter.""" TEST_NODE_NAME = 'enable_existing_test_node' - def test_add_topic_enables_existing_node_parameter(self): - """Test that add_topic enables parameter on existing nodes.""" + 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}' @@ -478,45 +475,31 @@ def test_add_topic_enables_existing_node_parameter(self): self.assertIsNotNone(future.result()) self.assertTrue(len(future.result().values) > 0) - # Create manage_topic client - manage_topic_client = create_manage_topic_client(self.test_node) - self.assertTrue( - wait_for_service_connection( - self.test_node, manage_topic_client, - timeout_sec=5.0, service_name='manage_topic')) + # Disable monitoring via enabled parameter + success = set_parameter( + self.test_node, enabled_param_name, False) + self.assertTrue(success, 'Failed to disable monitoring') - # Remove topic to disable monitoring - response = call_manage_topic_service( - self.test_node, manage_topic_client, add=False, topic=ENABLE_EXISTING_TOPIC) - self.assertIsNotNone(response) - self.assertTrue(response.success) + time.sleep(0.5) - # Verify enabled=false - 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.assertFalse(future.result().values[0].bool_value) + # Verify enabled=false on monitor node + success, value = get_parameter(self.test_node, enabled_param_name) + self.assertTrue(success) + self.assertFalse(value) - # Re-enable via add_topic - response = call_manage_topic_service( - self.test_node, manage_topic_client, add=True, topic=ENABLE_EXISTING_TOPIC) - self.assertIsNotNone(response) - self.assertTrue(response.success) - self.assertIn('Enabled monitoring on existing node', response.message) + # 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 - 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) - self.assertTrue(future.result().values[0].bool_value) + success, value = get_parameter(self.test_node, enabled_param_name) + self.assertTrue(success) + self.assertTrue(value) self.test_node.destroy_client(get_params_client) - self.test_node.destroy_client(manage_topic_client) if __name__ == '__main__': diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index 61e14ce..c8f2817 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) @@ -139,10 +142,6 @@ def tearDown(self): self.test_node.destroy_subscription(self.diagnostics_monitor.subscription) self.test_node.destroy_subscription( self.diagnostics_monitor.param_events_subscription) - self.test_node.destroy_client(self.diagnostics_monitor.manage_topic_client) - self.test_node.destroy_client(self.diagnostics_monitor.list_params_client) - self.test_node.destroy_client(self.diagnostics_monitor.get_params_client) - self.test_node.destroy_client(self.diagnostics_monitor.set_params_client) except Exception: pass # Ignore cleanup errors @@ -152,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_params_client) - - # Verify services are available - manage_available = self.diagnostics_monitor.manage_topic_client.wait_for_service( - timeout_sec=5.0) - set_params_available = ( - self.diagnostics_monitor.set_params_client - .wait_for_service(timeout_sec=5.0)) - - self.assertTrue(manage_available, 'ManageTopic service should be available') - self.assertTrue(set_params_available, 'SetParameters 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.""" @@ -371,26 +362,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 b6714fd..0000000 --- a/greenwave_monitor_interfaces/CMakeLists.txt +++ /dev/null @@ -1,32 +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" -) - -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/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 < Date: Thu, 8 Jan 2026 17:43:53 -0800 Subject: [PATCH 24/33] Fix CI tests Signed-off-by: Blake McHale --- .../greenwave_monitor/test_utils.py | 2 +- .../include/greenwave_diagnostics.hpp | 10 +++--- greenwave_monitor/src/greenwave_monitor.cpp | 33 ++++++++++++++----- .../test/test_greenwave_monitor.py | 2 +- 4 files changed, 32 insertions(+), 15 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/test_utils.py b/greenwave_monitor/greenwave_monitor/test_utils.py index 28fdbc6..0f2246a 100644 --- a/greenwave_monitor/greenwave_monitor/test_utils.py +++ b/greenwave_monitor/greenwave_monitor/test_utils.py @@ -53,7 +53,7 @@ ] # 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' diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index b7fa3d1..70cf20e 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -176,8 +176,9 @@ class GreenwaveDiagnostics { // 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()); + 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 @@ -247,8 +248,9 @@ class GreenwaveDiagnostics { // 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()); + 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 diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index ce3cfb0..2ad4c1b 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -112,8 +112,8 @@ std::optional parse_enabled_topic_param(const std::string & name) return std::nullopt; } - std::string topic = name.substr(prefix.size(), - name.size() - prefix.size() - suffix.size()); + std::string topic = name.substr( + prefix.size(), name.size() - prefix.size() - suffix.size()); if (topic.empty() || topic[0] != '/') { return std::nullopt; } @@ -375,6 +375,12 @@ bool GreenwaveMonitor::execute_add_topic( 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( @@ -392,8 +398,8 @@ bool GreenwaveMonitor::execute_add_topic( subscriptions_.push_back(sub); greenwave_diagnostics_.emplace( topic, - std::make_unique(*this, topic, - diagnostics_config)); + std::make_unique( + *this, topic, diagnostics_config)); message = "Successfully added topic: " + topic; return true; @@ -482,12 +488,21 @@ void GreenwaveMonitor::fetch_external_topic_map() continue; } - auto param_names = param_client->list_parameters({"greenwave_diagnostics"}, 10).names; - if (param_names.empty()) { + 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_node_name.c_str(), e.what()); continue; } - auto params = param_client->get_parameters(param_names); for (size_t i = 0; i < param_names.size(); ++i) { const auto & name = param_names[i]; const auto & param = params[i]; @@ -502,8 +517,8 @@ void GreenwaveMonitor::fetch_external_topic_map() continue; } - std::string topic = name.substr(prefix.size(), - name.size() - prefix.size() - suffix.size()); + std::string topic = name.substr( + prefix.size(), name.size() - prefix.size() - suffix.size()); if (topic.empty() || topic[0] != '/') { continue; } diff --git a/greenwave_monitor/test/test_greenwave_monitor.py b/greenwave_monitor/test/test_greenwave_monitor.py index 5eb18e5..556fe23 100644 --- a/greenwave_monitor/test/test_greenwave_monitor.py +++ b/greenwave_monitor/test/test_greenwave_monitor.py @@ -198,7 +198,7 @@ def test_manage_one_topic(self, expected_frequency, message_type, tolerance_hz): def test_manage_multiple_topics(self, expected_frequency, message_type, tolerance_hz): """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') self.check_node_launches_successfully() From 37aee84a34a051c8f2e242f823c120b32a91fd12 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 8 Jan 2026 18:02:34 -0800 Subject: [PATCH 25/33] Fix CI issues Signed-off-by: Blake McHale --- .../include/greenwave_diagnostics.hpp | 6 +++--- greenwave_monitor/src/greenwave_monitor.cpp | 14 +++++++++++--- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index 70cf20e..ec5d72b 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -617,8 +617,9 @@ class GreenwaveDiagnostics 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)"); + error_reasons.push_back( + param.get_name() + + ": must be a numeric type (int or double or NaN)"); continue; } @@ -636,7 +637,6 @@ class GreenwaveDiagnostics } new_tol = value; } - // Handle boolean types } else if (param.get_name() == enabled_param_name_) { new_enabled = param.as_bool(); } diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 2ad4c1b..22d5c76 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -333,7 +333,13 @@ TopicValidationResult GreenwaveMonitor::validate_add_topic( std::vector publishers; for (int attempt = 0; attempt <= max_retries; ++attempt) { - publishers = this->get_publishers_info_by_topic(topic); + 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; } @@ -497,7 +503,8 @@ void GreenwaveMonitor::fetch_external_topic_map() } params = param_client->get_parameters(param_names); } catch (const std::exception & e) { - RCLCPP_DEBUG(this->get_logger(), + RCLCPP_DEBUG( + this->get_logger(), "Failed to query parameters from node '%s': %s", full_node_name.c_str(), e.what()); continue; @@ -525,7 +532,8 @@ void GreenwaveMonitor::fetch_external_topic_map() if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL && param.as_bool()) { external_topic_to_node_[topic] = full_node_name; - RCLCPP_DEBUG(this->get_logger(), + RCLCPP_DEBUG( + this->get_logger(), "Found external monitoring for topic '%s' on node '%s'", topic.c_str(), full_node_name.c_str()); } From 136f3a7f9daf32b99736d7502164711b61e2a3d3 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 8 Jan 2026 20:40:07 -0800 Subject: [PATCH 26/33] Remove last monitor-interfaces reference Signed-off-by: Blake McHale --- .github/workflows/debian-packages.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 From 658dc502d64b6396e2078c53f530e5f8efb01ef8 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 8 Jan 2026 21:31:02 -0800 Subject: [PATCH 27/33] Test no destructor Signed-off-by: Blake McHale --- greenwave_monitor/include/greenwave_diagnostics.hpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index ec5d72b..a27e7f0 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -163,13 +163,7 @@ class GreenwaveDiagnostics } } - ~GreenwaveDiagnostics() - { - // Unregister parameter callback to avoid dangling references - if (param_callback_handle_) { - node_.remove_on_set_parameters_callback(param_callback_handle_.get()); - } - } + ~GreenwaveDiagnostics() = default; // Update diagnostics numbers. To be called in Subscriber and Publisher void updateDiagnostics(uint64_t msg_timestamp_ns) From 01c3f096904040b8c271143a2d4d596a3bc85701 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Thu, 8 Jan 2026 22:30:21 -0800 Subject: [PATCH 28/33] Try to fix CI Signed-off-by: Blake McHale --- greenwave_monitor/include/greenwave_monitor.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 1f59170..d5e262d 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -45,6 +45,12 @@ class GreenwaveMonitor : public rclcpp::Node { public: explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); + ~GreenwaveMonitor() + { + // Clear diagnostics before base Node destructor runs to avoid accessing invalid node state + greenwave_diagnostics_.clear(); + subscriptions_.clear(); + } private: void topic_callback( From c4273fd0414c660fcf81c05d19b566ff6c020eb2 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 9 Jan 2026 00:51:19 -0800 Subject: [PATCH 29/33] Small fixes to improve destructor safety Signed-off-by: Blake McHale --- .../greenwave_monitor/ui_adaptor.py | 6 ++++++ .../include/greenwave_diagnostics.hpp | 10 +++++++++- .../include/greenwave_monitor.hpp | 4 ++++ .../include/minimal_publisher_node.hpp | 8 ++++++++ greenwave_monitor/src/greenwave_monitor.cpp | 18 +++++++++++------- .../test/test_topic_monitoring_integration.py | 17 ++++++++++++++--- 6 files changed, 52 insertions(+), 11 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index 79817a8..d00b1f9 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -478,6 +478,11 @@ def _on_parameter_event(self, msg: ParameterEvent): # 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): + del self.ui_diagnostics[topic_name] + continue value = param_value_to_python(param.value) if not isinstance(value, (int, float)): @@ -503,6 +508,7 @@ def _on_parameter_event(self, msg: ParameterEvent): 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) + del self.ui_diagnostics[topic_name] elif field == 'freq': self.expected_frequencies.pop(topic_name, None) elif field == 'tol': diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index a27e7f0..934d7de 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -163,7 +163,15 @@ class GreenwaveDiagnostics } } - ~GreenwaveDiagnostics() = default; + ~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) diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index d5e262d..19f8265 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -47,6 +47,10 @@ class GreenwaveMonitor : public rclcpp::Node explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); ~GreenwaveMonitor() { + // Cancel timer first to stop callbacks from firing + if (timer_) { + timer_->cancel(); + } // Clear diagnostics before base Node destructor runs to avoid accessing invalid node state greenwave_diagnostics_.clear(); subscriptions_.clear(); diff --git a/greenwave_monitor/include/minimal_publisher_node.hpp b/greenwave_monitor/include/minimal_publisher_node.hpp index f6e2a43..f1fcbb3 100644 --- a/greenwave_monitor/include/minimal_publisher_node.hpp +++ b/greenwave_monitor/include/minimal_publisher_node.hpp @@ -35,6 +35,14 @@ class MinimalPublisher : public rclcpp::Node public: explicit MinimalPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~MinimalPublisher() + { + if (timer_) { + timer_->cancel(); + } + greenwave_diagnostics_.reset(); + } + private: void timer_callback(); diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 22d5c76..20e63cd 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -156,6 +156,11 @@ rcl_interfaces::msg::SetParametersResult GreenwaveMonitor::on_parameter_change( } 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; @@ -326,6 +331,12 @@ TopicValidationResult GreenwaveMonitor::validate_add_topic( 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; + } + if (greenwave_diagnostics_.find(topic) != greenwave_diagnostics_.end()) { result.error_message = "Topic already monitored: " + topic; return result; @@ -358,13 +369,6 @@ TopicValidationResult GreenwaveMonitor::validate_add_topic( return result; } - // Check if any external node already has monitoring enabled for this 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; - } - result.valid = true; result.message_type = publishers[0].topic_type(); return result; diff --git a/greenwave_monitor/test/test_topic_monitoring_integration.py b/greenwave_monitor/test/test_topic_monitoring_integration.py index c8f2817..95ca012 100644 --- a/greenwave_monitor/test/test_topic_monitoring_integration.py +++ b/greenwave_monitor/test/test_topic_monitoring_integration.py @@ -288,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 @@ -309,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) @@ -321,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') From c78675c1c3cf251e1b5408ecf2f1e55e5a300237 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 9 Jan 2026 09:35:40 -0800 Subject: [PATCH 30/33] Fix ROS race conditions during initialization Signed-off-by: Blake McHale --- .../include/greenwave_monitor.hpp | 8 +++- greenwave_monitor/src/greenwave_monitor.cpp | 48 ++++++++----------- .../src/greenwave_monitor_main.cpp | 10 +++- 3 files changed, 35 insertions(+), 31 deletions(-) diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index 19f8265..4b24f12 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -47,10 +47,13 @@ class GreenwaveMonitor : public rclcpp::Node explicit GreenwaveMonitor(const rclcpp::NodeOptions & options); ~GreenwaveMonitor() { - // Cancel timer first to stop callbacks from firing + // 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(); @@ -77,6 +80,8 @@ class GreenwaveMonitor : public rclcpp::Node bool execute_add_topic(const TopicValidationResult & validated, std::string & message); + void deferred_init(); + void fetch_external_topic_map(); bool add_topic( @@ -98,6 +103,7 @@ class GreenwaveMonitor : public rclcpp::Node std::unique_ptr> greenwave_diagnostics_; std::vector> subscriptions_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr init_timer_; rclcpp::Subscription::SharedPtr param_event_sub_; OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; std::unordered_map pending_validations_; diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index 20e63cd..e0a26f4 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -39,10 +39,21 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) this->declare_parameter>("topics", {""}, descriptor); } - // Build external topic map and collect all topics to monitor + // Timer callback to publish diagnostics and print feedback + timer_ = this->create_wall_timer( + 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); + + // 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(); + }); +} + +void GreenwaveMonitor::deferred_init() +{ fetch_external_topic_map(); - // Merge topics into one set std::set all_topics = get_topics_from_parameters(); auto topics_param = this->get_parameter("topics").as_string_array(); for (const auto & topic : topics_param) { @@ -51,17 +62,12 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) } } - // Add all starting topics to the monitor for (const auto & topic : all_topics) { std::string message; add_topic(topic, message); } - // Timer callback to publish diagnostics and print feedback - timer_ = this->create_wall_timer( - 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); - - // Register parameter change callback for synchronous validation + // 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)); @@ -472,28 +478,12 @@ void GreenwaveMonitor::fetch_external_topic_map() auto node_names = this->get_node_names(); for (const auto & full_name : node_names) { - // Parse full node name to extract name and namespace - std::string node_namespace, node_name; - size_t last_slash = full_name.rfind('/'); - if (last_slash == std::string::npos || last_slash == 0) { - node_name = full_name; - node_namespace = "/"; - } else { - node_name = full_name.substr(last_slash + 1); - node_namespace = full_name.substr(0, last_slash); - if (node_namespace.empty()) { - node_namespace = "/"; - } - } - - std::string full_node_name = full_name; - - if (full_node_name == our_node) { + if (full_name == our_node) { continue; } auto param_client = std::make_shared( - temp_node, full_node_name); + temp_node, full_name); if (!param_client->wait_for_service(std::chrono::milliseconds(100))) { continue; } @@ -510,7 +500,7 @@ void GreenwaveMonitor::fetch_external_topic_map() RCLCPP_DEBUG( this->get_logger(), "Failed to query parameters from node '%s': %s", - full_node_name.c_str(), e.what()); + full_name.c_str(), e.what()); continue; } @@ -535,11 +525,11 @@ void GreenwaveMonitor::fetch_external_topic_map() } if (param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL && param.as_bool()) { - external_topic_to_node_[topic] = full_node_name; + 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_node_name.c_str()); + topic.c_str(), full_name.c_str()); } } } 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; } From 7308ae08e75988ad9ff2b7f39de1fefb18dd68ab Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 9 Jan 2026 09:41:04 -0800 Subject: [PATCH 31/33] Fix lint Signed-off-by: Blake McHale --- greenwave_monitor/src/greenwave_monitor.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e0a26f4..f561939 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -44,10 +44,11 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) 1s, std::bind(&GreenwaveMonitor::timer_callback, this)); // 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(); - }); + init_timer_ = this->create_wall_timer( + 0ms, [this]() { + init_timer_->cancel(); + deferred_init(); + }); } void GreenwaveMonitor::deferred_init() From 7f1c64fe285f7df2eacdee721616b9c1b3432eb3 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 9 Jan 2026 09:54:24 -0800 Subject: [PATCH 32/33] Small greptile fixups Signed-off-by: Blake McHale --- greenwave_monitor/greenwave_monitor/ui_adaptor.py | 6 +++--- greenwave_monitor/include/greenwave_diagnostics.hpp | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/greenwave_monitor/greenwave_monitor/ui_adaptor.py b/greenwave_monitor/greenwave_monitor/ui_adaptor.py index d00b1f9..370ba03 100644 --- a/greenwave_monitor/greenwave_monitor/ui_adaptor.py +++ b/greenwave_monitor/greenwave_monitor/ui_adaptor.py @@ -481,7 +481,7 @@ def _on_parameter_event(self, msg: ParameterEvent): elif field == 'enabled' and param in msg.changed_parameters: if (param.value.type == ParameterType.PARAMETER_BOOL and not param.value.bool_value): - del self.ui_diagnostics[topic_name] + self.ui_diagnostics.pop(topic_name, None) continue value = param_value_to_python(param.value) @@ -508,7 +508,7 @@ def _on_parameter_event(self, msg: ParameterEvent): 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) - del self.ui_diagnostics[topic_name] + self.ui_diagnostics.pop(topic_name, None) elif field == 'freq': self.expected_frequencies.pop(topic_name, None) elif field == 'tol': @@ -536,7 +536,7 @@ def toggle_topic_monitoring(self, topic_name: str): with self.data_lock: if not new_enabled and topic_name in self.ui_diagnostics: - del self.ui_diagnostics[topic_name] + self.ui_diagnostics.pop(topic_name, None) def set_expected_frequency(self, topic_name: str, diff --git a/greenwave_monitor/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index 934d7de..a54a107 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -744,7 +745,7 @@ class GreenwaveDiagnostics } else { RCLCPP_WARN( node_.get_logger(), - "Iniital parameter %s failed to set for topic %s: %s", + "Initial parameter %s failed to set for topic %s: %s", param_name.c_str(), topic_name_.c_str(), result.reason.c_str()); } } From 97a8f80c14073944f46df0d5bd2e8f19f0b91238 Mon Sep 17 00:00:00 2001 From: Blake McHale Date: Fri, 9 Jan 2026 10:12:59 -0800 Subject: [PATCH 33/33] Fix lint Signed-off-by: Blake McHale --- greenwave_monitor/examples/example.launch.py | 6 ++++++ greenwave_monitor/include/greenwave_diagnostics.hpp | 6 +++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/greenwave_monitor/examples/example.launch.py b/greenwave_monitor/examples/example.launch.py index d182337..987c007 100644 --- a/greenwave_monitor/examples/example.launch.py +++ b/greenwave_monitor/examples/example.launch.py @@ -16,6 +16,7 @@ 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 @@ -98,5 +99,10 @@ def generate_launch_description(): name='greenwave_monitor', output='log', parameters=[config_file] + ), + LogInfo( + 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/include/greenwave_diagnostics.hpp b/greenwave_monitor/include/greenwave_diagnostics.hpp index a54a107..10eb7b3 100644 --- a/greenwave_monitor/include/greenwave_diagnostics.hpp +++ b/greenwave_monitor/include/greenwave_diagnostics.hpp @@ -18,18 +18,18 @@ #pragma once #include -#include -#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"