diff --git a/src/ros2_medkit_diagnostic_bridge/README.md b/src/ros2_medkit_diagnostic_bridge/README.md index 0943b696a..da42c3528 100644 --- a/src/ros2_medkit_diagnostic_bridge/README.md +++ b/src/ros2_medkit_diagnostic_bridge/README.md @@ -35,6 +35,8 @@ ros2 run ros2_medkit_diagnostic_bridge diagnostic_bridge_node |-----------|------|---------|-------------| | `diagnostics_topic` | string | `/diagnostics` | Topic to subscribe to | | `auto_generate_codes` | bool | `true` | Auto-generate fault codes from diagnostic names | +| `use_hardware_id_as_source_id` | bool | `false` | Use slash-containing diagnostic `hardware_id` values as fault `source_id` | +| `max_tracked_sources` | integer | `512` | Maximum number of per-source FaultReporter instances retained by the bridge | | `name_to_code.` | string | - | Custom mapping from diagnostic name to fault code | | `keyvalue_codes` | string[] | - | List of keys used to search the diagnostic values for the fault code | @@ -45,6 +47,9 @@ diagnostic_bridge: ros__parameters: diagnostics_topic: "/diagnostics" auto_generate_codes: true + # Opt in to source attribution from diagnostic hardware_id values + use_hardware_id_as_source_id: false + max_tracked_sources: 512 # Custom mappings (optional) # Format: "name_to_code.": "" @@ -64,6 +69,24 @@ When `auto_generate_codes` is enabled, diagnostic names are converted to fault c Custom mappings in `name_to_code` take priority over auto-generation. +### Fault Source Attribution + +By default, faults reported by this bridge use the bridge node's fully-qualified +name (`/diagnostic_bridge`) as their `source_id`. This preserves the behavior of +existing deployments because `DiagnosticStatus.hardware_id` commonly contains a +serial number, device path, or no value rather than a ROS node name. + +Set `use_hardware_id_as_source_id` to `true` to opt in to hardware ID attribution. +When enabled, a non-empty `hardware_id` is used exactly as provided only when it +contains `/`, which is treated as a heuristic for a node/FQN-like identifier. +Empty or non-slash hardware IDs fall back to the bridge FQN. This option does not +perform manifest lookup or normalize hardware IDs; the accepted value must already +match the runtime entity source ID used by the gateway. + +The bridge keeps one `FaultReporter` per active source so local filtering remains +isolated by source. `max_tracked_sources` bounds this cache with least-recently-used +eviction; values below `1` are clamped to `1`. + ## Launch ```bash diff --git a/src/ros2_medkit_diagnostic_bridge/config/diagnostic_bridge.yaml b/src/ros2_medkit_diagnostic_bridge/config/diagnostic_bridge.yaml index b53a3b876..af88d6aaf 100644 --- a/src/ros2_medkit_diagnostic_bridge/config/diagnostic_bridge.yaml +++ b/src/ros2_medkit_diagnostic_bridge/config/diagnostic_bridge.yaml @@ -7,6 +7,13 @@ diagnostic_bridge: # If false, only explicitly mapped diagnostics will be forwarded auto_generate_codes: true + # Use slash-containing diagnostic hardware_id values as fault source IDs. + # Disabled by default because hardware_id is often a serial number or device path. + use_hardware_id_as_source_id: false + + # Maximum number of per-source FaultReporter instances retained by the bridge. + max_tracked_sources: 512 + # Custom diagnostic name to fault code mappings # Format: "name_to_code.": "" # Example: diff --git a/src/ros2_medkit_diagnostic_bridge/include/ros2_medkit_diagnostic_bridge/diagnostic_bridge_node.hpp b/src/ros2_medkit_diagnostic_bridge/include/ros2_medkit_diagnostic_bridge/diagnostic_bridge_node.hpp index 9c1245338..302be9c90 100644 --- a/src/ros2_medkit_diagnostic_bridge/include/ros2_medkit_diagnostic_bridge/diagnostic_bridge_node.hpp +++ b/src/ros2_medkit_diagnostic_bridge/include/ros2_medkit_diagnostic_bridge/diagnostic_bridge_node.hpp @@ -14,11 +14,13 @@ #pragma once +#include #include #include #include #include #include +#include #include #include "rclcpp/rclcpp.hpp" @@ -66,6 +68,16 @@ class DiagnosticBridgeNode : public rclcpp::Node { /// Check if diagnostic level indicates OK status static bool is_ok_level(uint8_t diagnostic_level); + /// Get (creating on first use) the FaultReporter for a given source_id. + ros2_medkit_fault_reporter::FaultReporter * reporter_for(const std::string & source_id); + + /// Resolve fault source_id for a diagnostic status. + /// Uses a slash-containing hardware_id when enabled, else falls back to bridge FQN. + std::string source_id_for(const diagnostic_msgs::msg::DiagnosticStatus & status) const; + + /// Number of currently tracked source reporters. + size_t tracked_reporter_count(); + private: /// Callback for /diagnostics messages void diagnostics_callback(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg); @@ -82,12 +94,20 @@ class DiagnosticBridgeNode : public rclcpp::Node { // ROS2 components rclcpp::Subscription::SharedPtr diagnostics_sub_; - std::unique_ptr reporter_; - std::once_flag reporter_init_flag_; + // One FaultReporter per source_id, bounded with LRU eviction. + struct ReporterEntry { + std::string source_id; + std::unique_ptr reporter; + }; + std::list reporters_lru_; + std::unordered_map::iterator> reporters_; + std::mutex reporters_mutex_; // Configuration std::string diagnostics_topic_; bool auto_generate_codes_; + bool use_hardware_id_as_source_id_{false}; + int max_tracked_sources_{512}; std::map name_to_code_; std::vector keyvalue_codes_; }; diff --git a/src/ros2_medkit_diagnostic_bridge/src/diagnostic_bridge_node.cpp b/src/ros2_medkit_diagnostic_bridge/src/diagnostic_bridge_node.cpp index e86cb7e29..9bc3d87d2 100644 --- a/src/ros2_medkit_diagnostic_bridge/src/diagnostic_bridge_node.cpp +++ b/src/ros2_medkit_diagnostic_bridge/src/diagnostic_bridge_node.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include #include "ros2_medkit_msgs/msg/fault.hpp" @@ -30,13 +32,24 @@ DiagnosticBridgeNode::DiagnosticBridgeNode(const rclcpp::NodeOptions & options) diagnostics_callback(msg); }); - RCLCPP_INFO(get_logger(), "DiagnosticBridge started (topic=%s, auto_generate=%s, mappings=%zu)", - diagnostics_topic_.c_str(), auto_generate_codes_ ? "true" : "false", name_to_code_.size()); + RCLCPP_INFO(get_logger(), + "DiagnosticBridge started (topic=%s, auto_generate=%s, use_hardware_id_as_source_id=%s, mappings=%zu)", + diagnostics_topic_.c_str(), auto_generate_codes_ ? "true" : "false", + use_hardware_id_as_source_id_ ? "true" : "false", name_to_code_.size()); } void DiagnosticBridgeNode::load_parameters() { diagnostics_topic_ = declare_parameter("diagnostics_topic", "/diagnostics"); auto_generate_codes_ = declare_parameter("auto_generate_codes", true); + use_hardware_id_as_source_id_ = declare_parameter("use_hardware_id_as_source_id", false); + const int64_t max_tracked_sources = declare_parameter("max_tracked_sources", 512); + const int64_t max_tracked_sources_used = + std::clamp(max_tracked_sources, INT64_C(1), static_cast(std::numeric_limits::max())); + if (max_tracked_sources_used != max_tracked_sources) { + RCLCPP_WARN(get_logger(), "max_tracked_sources=%" PRId64 " clamped to %" PRId64, max_tracked_sources, + max_tracked_sources_used); + } + max_tracked_sources_ = static_cast(max_tracked_sources_used); std::vector keyvalue_codes = declare_parameter>("keyvalue_codes", std::vector()); @@ -60,17 +73,60 @@ void DiagnosticBridgeNode::load_parameters() { } void DiagnosticBridgeNode::diagnostics_callback(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg) { - // Thread-safe lazy initialization of reporter (can't use shared_from_this in constructor) - std::call_once(reporter_init_flag_, [this]() { - reporter_ = std::make_unique(this->shared_from_this(), - get_fully_qualified_name()); - }); - for (const auto & status : msg->status) { process_diagnostic(status); } } +ros2_medkit_fault_reporter::FaultReporter * DiagnosticBridgeNode::reporter_for(const std::string & source_id) { + std::lock_guard lock(reporters_mutex_); + auto it = reporters_.find(source_id); + if (it != reporters_.end()) { + reporters_lru_.splice(reporters_lru_.end(), reporters_lru_, it->second); + return it->second->reporter.get(); + } + + // Multiple FaultReporters on one node are safe: FaultReporter guards + // parameter declaration with has_parameter(). + auto reporter = std::make_unique(this->shared_from_this(), source_id); + auto * raw = reporter.get(); + reporters_lru_.push_back(ReporterEntry{source_id, std::move(reporter)}); + reporters_[source_id] = std::prev(reporters_lru_.end()); + + if (static_cast(reporters_lru_.size()) > max_tracked_sources_) { + RCLCPP_WARN_ONCE(get_logger(), "max_tracked_sources (%d) reached; evicting least-recently-used reporters", + max_tracked_sources_); + reporters_.erase(reporters_lru_.front().source_id); + reporters_lru_.pop_front(); + } + return raw; +} + +size_t DiagnosticBridgeNode::tracked_reporter_count() { + std::lock_guard lock(reporters_mutex_); + return reporters_.size(); +} + +std::string DiagnosticBridgeNode::source_id_for(const diagnostic_msgs::msg::DiagnosticStatus & status) const { + const std::string fqn = get_fully_qualified_name(); + if (!use_hardware_id_as_source_id_) { + return fqn; + } + + if (!status.hardware_id.empty() && status.hardware_id.find('/') != std::string::npos) { + return status.hardware_id; + } + + // Not RCLCPP_WARN_THROTTLE: this method is const, so get_clock() yields a const + // Clock, and Clock::now() is non-const on Humble. Once per process is also the + // right cadence here - the throttle keeps one static per call site, so it would + // name a single diagnostic every 10s and never the others. + RCLCPP_WARN_ONCE(get_logger(), + "Diagnostic '%s' hardware_id '%s' is not a slash-containing node ID, using bridge source_id '%s'", + status.name.c_str(), status.hardware_id.c_str(), fqn.c_str()); + return fqn; +} + void DiagnosticBridgeNode::process_diagnostic(const diagnostic_msgs::msg::DiagnosticStatus & status) { const std::string fault_code = map_to_fault_code(status); @@ -79,15 +135,21 @@ void DiagnosticBridgeNode::process_diagnostic(const diagnostic_msgs::msg::Diagno return; } + const std::string source_id = source_id_for(status); + auto * reporter = reporter_for(source_id); + if (reporter == nullptr) { + return; + } + if (is_ok_level(status.level)) { // OK status -> send PASSED event for healing - reporter_->report_passed(fault_code); + reporter->report_passed(fault_code); RCLCPP_DEBUG(get_logger(), "Diagnostic OK: %s -> PASSED for %s", status.name.c_str(), fault_code.c_str()); } else { // WARN, ERROR, STALE -> send FAILED event auto severity = map_to_severity(status.level); // severity is guaranteed to have value here (not OK level) - reporter_->report(fault_code, *severity, status.message); + reporter->report(fault_code, *severity, status.message); RCLCPP_DEBUG(get_logger(), "Diagnostic %s: %s -> fault %s (severity=%d)", status.name.c_str(), status.message.c_str(), fault_code.c_str(), *severity); } diff --git a/src/ros2_medkit_diagnostic_bridge/test/test_diagnostic_bridge.cpp b/src/ros2_medkit_diagnostic_bridge/test/test_diagnostic_bridge.cpp index 32e8a859c..8dc16554f 100644 --- a/src/ros2_medkit_diagnostic_bridge/test/test_diagnostic_bridge.cpp +++ b/src/ros2_medkit_diagnostic_bridge/test/test_diagnostic_bridge.cpp @@ -107,6 +107,47 @@ TEST_F(DiagnosticBridgeTest, NodeCreation) { EXPECT_STREQ(node->get_name(), "diagnostic_bridge"); } +TEST_F(DiagnosticBridgeTest, SourceId_DefaultsToBridgeFqn) { + auto node = std::make_shared(); + auto status = diagnostic_status("sensor", DiagStatus::ERROR); + status.hardware_id = "/sensor_node"; + + EXPECT_EQ(node->source_id_for(status), "/diagnostic_bridge"); +} + +TEST_F(DiagnosticBridgeTest, SourceId_UsesSlashContainingHardwareIdWhenEnabled) { + rclcpp::NodeOptions options; + options.append_parameter_override("use_hardware_id_as_source_id", true); + auto node = std::make_shared(options); + auto status = diagnostic_status("sensor", DiagStatus::ERROR); + status.hardware_id = "/sensor_node"; + + EXPECT_EQ(node->source_id_for(status), "/sensor_node"); +} + +TEST_F(DiagnosticBridgeTest, SourceId_NonSlashHardwareIdFallsBackToBridgeFqn) { + rclcpp::NodeOptions options; + options.append_parameter_override("use_hardware_id_as_source_id", true); + auto node = std::make_shared(options); + auto status = diagnostic_status("sensor", DiagStatus::ERROR); + status.hardware_id = "SERIAL123"; + + EXPECT_EQ(node->source_id_for(status), "/diagnostic_bridge"); +} + +TEST_F(DiagnosticBridgeTest, ReporterCache_IsBounded) { + rclcpp::NodeOptions options; + options.append_parameter_override("max_tracked_sources", 2); + auto node = std::make_shared(options); + + ASSERT_EQ(node->tracked_reporter_count(), 0u); + ASSERT_NE(node->reporter_for("/source_a"), nullptr); + ASSERT_NE(node->reporter_for("/source_b"), nullptr); + EXPECT_EQ(node->tracked_reporter_count(), 2u); + ASSERT_NE(node->reporter_for("/source_c"), nullptr); + EXPECT_EQ(node->tracked_reporter_count(), 2u); +} + // Test fault code mapping with auto-generate TEST_F(DiagnosticBridgeTest, MapToFaultCode_AutoGenerate) { auto node = std::make_shared(); diff --git a/src/ros2_medkit_diagnostic_bridge/test/test_integration.test.py b/src/ros2_medkit_diagnostic_bridge/test/test_integration.test.py index 39331f626..64931ef1e 100644 --- a/src/ros2_medkit_diagnostic_bridge/test/test_integration.test.py +++ b/src/ros2_medkit_diagnostic_bridge/test/test_integration.test.py @@ -55,6 +55,7 @@ def generate_test_description(): parameters=[{ 'diagnostics_topic': '/diagnostics', 'auto_generate_codes': True, + 'use_hardware_id_as_source_id': True, }], # Give the node room to flush coverage data at shutdown before SIGKILL. sigterm_timeout='30', @@ -137,7 +138,7 @@ def list_faults(self, statuses=None): result = future.result() return result.faults if result is not None else [] - def publish_diagnostic(self, name, level, message='Test message'): + def publish_diagnostic(self, name, level, message='Test message', hardware_id='test_hw'): """Publish a single diagnostic message.""" msg = DiagnosticArray() msg.header.stamp = self.node.get_clock().now().to_msg() @@ -145,7 +146,7 @@ def publish_diagnostic(self, name, level, message='Test message'): level=level, name=name, message=message, - hardware_id='test_hw', + hardware_id=hardware_id, )] self.diag_pub.publish(msg) @@ -153,7 +154,7 @@ def publish_diagnostic(self, name, level, message='Test message'): time.sleep(0.3) def publish_until(self, name, level, expected_code, *, predicate=None, - message='Test message', statuses=None, timeout=25.0): + message='Test message', hardware_id='test_hw', statuses=None, timeout=25.0): """ Republish a diagnostic until a matching fault satisfies *predicate*. @@ -175,7 +176,7 @@ def predicate(_fault): deadline = time.monotonic() + timeout last = None while time.monotonic() < deadline: - self.publish_diagnostic(name, level, message) + self.publish_diagnostic(name, level, message, hardware_id) fault = next( (f for f in self.list_faults(statuses=statuses) if f.fault_code == expected_code), @@ -252,6 +253,38 @@ def test_05_fault_code_generation(self): diag_name, DiagnosticStatus.ERROR, expected_code, ) + def test_06_hardware_id_is_forwarded_as_source(self): + """Test that hardware_id is forwarded into fault reporting_sources.""" + self.publish_until( + 'shared_sensor', DiagnosticStatus.STALE, 'SHARED_SENSOR', + message='No data from source A', + hardware_id='/my_lidar_driver', + predicate=lambda f: '/my_lidar_driver' in f.reporting_sources, + ) + + fault = self.publish_until( + 'shared_sensor', DiagnosticStatus.STALE, 'SHARED_SENSOR', + message='No data from source B', + hardware_id='/my_camera_driver', + predicate=lambda f: '/my_lidar_driver' in f.reporting_sources + and '/my_camera_driver' in f.reporting_sources, + ) + + self.assertEqual(fault.severity, Fault.SEVERITY_CRITICAL) + self.assertIn('/my_lidar_driver', fault.reporting_sources) + self.assertIn('/my_camera_driver', fault.reporting_sources) + + def test_07_empty_hardware_id_falls_back_to_bridge_source(self): + """Test that empty hardware_id falls back to the bridge node FQN.""" + fault = self.publish_until( + 'fallback_sensor', DiagnosticStatus.STALE, 'FALLBACK_SENSOR', + message='No data with empty hardware id', + hardware_id='', + predicate=lambda f: '/diagnostic_bridge' in f.reporting_sources, + ) + + self.assertIn('/diagnostic_bridge', fault.reporting_sources) + @launch_testing.post_shutdown_test() class TestDiagnosticBridgeShutdown(unittest.TestCase):