diff --git a/docs/config/fault-manager.rst b/docs/config/fault-manager.rst index 400de56c0..20d973bdc 100644 --- a/docs/config/fault-manager.rst +++ b/docs/config/fault-manager.rst @@ -221,12 +221,104 @@ threshold overrides: When multiple entities report the same ``fault_code``, each event applies the thresholds resolved from that event's ``source_id``. This means the debounce - behavior follows the reporting entity, not the fault. + behavior follows the reporting entity, not the fault. The debounce counter, + however, belongs to the fault code, so the two entities share one counter under + two policies - see `Per-Fault-Code Thresholds`_ for what that does and how to + settle it. The node warns when it happens. ``auto_confirm_after_sec`` is global-only and cannot be overridden per-entity. Critical faults skip debounce and confirm on their first occurrence; that is built in, not a parameter, so it can be neither disabled nor set per entity. +Per-Fault-Code Thresholds +~~~~~~~~~~~~~~~~~~~~~~~~~ + +Per-entity thresholds are the right tool when you know which subsystems are noisy but not +which codes they will emit. They have one limit: the debounce counter is kept per +``fault_code``, while the override is chosen per ``source_id``. Two entities reporting one +code therefore share a counter and debounce it under two policies, and the report that +happens to arrive decides the transition: + +.. code-block:: text + + motor (confirmation_threshold=-5) reports OVERHEAT -> counter=-1, PREFAILED + lidar (confirmation_threshold=-1) reports OVERHEAT -> counter=-2, CONFIRMED + +The motor's policy was bypassed. Per-fault_code thresholds are the layer that removes +that: they are matched on the code itself, so they resolve the same whoever reports. + +.. code-block:: yaml + + fault_manager: + ros__parameters: + # Path to YAML file with per-fault_code overrides + fault_thresholds: + config_file: "/etc/ros2_medkit/fault_thresholds.yaml" + +The file is a map of fault codes to threshold overrides, the same three fields an entity +override carries: + +.. code-block:: yaml + + # fault_thresholds.yaml + MOTOR_OVERHEAT: + confirmation_threshold: -5 # five events, whoever reports them + healing_threshold: 10 + + LIDAR_FAIL: + confirmation_threshold: -1 # instant + healing_threshold: 1 + +.. list-table:: + :header-rows: 1 + :widths: 35 15 50 + + * - Parameter + - Default + - Description + * - ``fault_thresholds.config_file`` + - ``""`` + - Path to YAML file with per-fault_code threshold overrides. Empty = disabled. + +**How the layers combine:** + +- Three layers, each applied on top of the last: the **global** defaults, then the + **entity** override whose prefix matches the reporting ``source_id``, then the + **fault code's** own override. A field a layer does not set is left as the layer below + had it, so ``fault_code`` > ``source_id`` > global for every field independently. +- Matching on the code is **exact**. A fault code is an identifier, not a path: an entry + for ``MOTOR`` does not capture ``MOTOR_OVERHEAT`` the way ``/sensors`` captures + ``/sensors/lidar``. +- A code that is not listed resolves exactly as it did before: entity override if one + matches, global otherwise. The layer is opt-in and changes nothing on its own. +- Like the entity file, this one is loaded once at node startup. Changes require a restart. + +.. note:: + + An override that names only some fields settles only those. If ``MOTOR_OVERHEAT`` pins + ``confirmation_threshold`` but not ``healing_threshold``, the healing direction still + follows whichever entity reported, and the node still reports the conflict below. Pin + all three fields to settle a code completely. + +**When one code is debounced two ways** + +The node resolves the policy for every report, and warns the first time two sources resolve +different policies for one code: + +.. code-block:: text + + [WARN] Fault code 'OVERHEAT' is debounced two ways: '/powertrain/motor/left' resolves + confirmation=-5 healing_enabled=false healing=10, '/sensors/lidar/front' resolves + confirmation=-1 healing_enabled=true healing=1. The debounce counter belongs to the + fault code, so whichever source reports decides the transition and the other policy is + bypassed. Give the code an entry in fault_thresholds.config_file to settle it. + +The warning names both sources and both resolved policies, and is emitted **once per fault +code** for the life of the node, so a busy reporter does not turn it into a log storm. It +is a diagnostic, not an error: the configuration is legal and the fault is still debounced, +just not under a policy an operator chose. Giving the code an entry in +``fault_thresholds.config_file`` that pins all three fields ends it. + Snapshot Configuration ---------------------- @@ -637,6 +729,10 @@ Complete Example entity_thresholds: config_file: "/etc/ros2_medkit/entity_thresholds.yaml" + # Per-fault_code debounce overrides (applied on top of the entity ones) + fault_thresholds: + config_file: "/etc/ros2_medkit/fault_thresholds.yaml" + # Snapshots snapshots: enabled: true diff --git a/docs/requirements/specs/faults.rst b/docs/requirements/specs/faults.rst index f3434a3f6..65852d8b4 100644 --- a/docs/requirements/specs/faults.rst +++ b/docs/requirements/specs/faults.rst @@ -48,6 +48,28 @@ Faults shall take precedence over global defaults. Unspecified fields shall inherit from global configuration. When no entity prefix matches, global defaults shall apply. +.. req:: Per-Fault-Code Debounce Thresholds + :id: REQ_INTEROP_107 + :status: verified + :tags: Faults + + The fault manager shall support per-fault_code debounce threshold configuration using + exact matching on the reported fault code. A fault-code override for + ``confirmation_threshold``, ``healing_enabled``, or ``healing_threshold`` shall take + precedence over both the per-entity override resolved for the reporting source and the + global defaults. Unspecified fields shall inherit from the layer below. When a fault code + has no override, the resolved per-entity or global configuration shall apply unchanged. + +.. req:: Conflicting Debounce Policies Are Reported + :id: REQ_INTEROP_108 + :status: verified + :tags: Faults + + The debounce counter belongs to the fault code while a per-entity override is resolved + from the reporting source. When two sources report one fault code and resolve to + different debounce policies, the fault manager shall warn, naming the fault code, both + sources and both resolved policies, at most once per fault code for the life of the node. + .. req:: Fault Snapshot and Rosbag Capture :id: REQ_INTEROP_088 :status: verified diff --git a/src/ros2_medkit_fault_manager/CMakeLists.txt b/src/ros2_medkit_fault_manager/CMakeLists.txt index 064cf44f1..773e4ced6 100644 --- a/src/ros2_medkit_fault_manager/CMakeLists.txt +++ b/src/ros2_medkit_fault_manager/CMakeLists.txt @@ -57,7 +57,7 @@ add_library(fault_manager_lib STATIC src/correlation/config_parser.cpp src/correlation/pattern_matcher.cpp src/correlation/correlation_engine.cpp - src/entity_threshold_resolver.cpp + src/threshold_resolver.cpp ) target_include_directories(fault_manager_lib PUBLIC @@ -173,6 +173,11 @@ if(BUILD_TESTING) target_link_libraries(test_entity_thresholds fault_manager_lib) medkit_target_dependencies(test_entity_thresholds rclcpp ros2_medkit_msgs) + # Fault-code threshold resolver tests + medkit_add_gtest(test_fault_code_thresholds test/test_fault_code_thresholds.cpp) + target_link_libraries(test_fault_code_thresholds fault_manager_lib) + medkit_target_dependencies(test_fault_code_thresholds rclcpp ros2_medkit_msgs) + # Integration tests install(DIRECTORY test DESTINATION share/${PROJECT_NAME} @@ -192,6 +197,9 @@ if(BUILD_TESTING) medkit_add_launch_test(test_entity_thresholds_integration test/integration/test_entity_thresholds_integration.test.py TIMEOUT 60 LABELS "integration") + medkit_add_launch_test(test_fault_code_thresholds_integration + test/integration/test_fault_code_thresholds_integration.test.py TIMEOUT 60 LABELS "integration") + # Drives healing with the event counts a one-event-per-transition reporter # actually sends: one FAILED per raise, one PASSED per clear. Parametrized over # healing_threshold, so the node launches twice, and one case holds a settled diff --git a/src/ros2_medkit_fault_manager/README.md b/src/ros2_medkit_fault_manager/README.md index 977f5a8c4..8f02ee502 100644 --- a/src/ros2_medkit_fault_manager/README.md +++ b/src/ros2_medkit_fault_manager/README.md @@ -51,7 +51,7 @@ ros2 service call /fault_manager/clear_fault ros2_medkit_msgs/srv/ClearFault \ when a cleared fault is raised again - and tracks all reporting sources - **Severity escalation**: Fault severity is updated if a higher severity is reported - **Persistent storage**: SQLite backend ensures faults survive node restarts -- **Debounce filtering** (optional): AUTOSAR DEM-style counter-based fault confirmation with per-entity threshold overrides +- **Debounce filtering** (optional): AUTOSAR DEM-style counter-based fault confirmation with per-entity and per-fault_code threshold overrides - **Snapshot capture**: Captures topic data when faults are confirmed for debugging (the value snapshots are deleted when the fault is cleared, unless `snapshots.retain_on_clear` is set) - **Near-miss series**: Appends one entry per FAILED report that moved the debounce counter without confirming, bounded per fault code and retained when the fault is cleared - **Freeze-frame retention**: One compact JSON freeze-frame per fault code, retained across `clear_fault` (see below) @@ -69,6 +69,7 @@ ros2 service call /fault_manager/clear_fault ros2_medkit_msgs/srv/ClearFault \ | `healing_threshold` | int | `3` | Counter value at which faults are healed | | `auto_confirm_after_sec` | double | `0.0` | Auto-confirm PREFAILED faults after timeout (0 = disabled) | | `entity_thresholds.config_file` | string | `""` | Path to YAML file with per-entity debounce threshold overrides | +| `fault_thresholds.config_file` | string | `""` | Path to YAML file with per-fault_code debounce threshold overrides, applied on top of the entity ones | | `near_miss.max_per_fault` | int | `200` | Near-miss entries retained per fault code, oldest evicted first (0 = unlimited) | ### Snapshot Parameters @@ -159,7 +160,10 @@ rows written before the field existed. With per-entity thresholds the recorded `confirmation_threshold` is the one belonging to the **reporting source**, while the debounce counter is shared by every source of that fault code. It -is therefore not by itself the distance to confirmation for the fault as a whole. +is therefore not by itself the distance to confirmation for the fault as a whole. Giving the code +an entry in `fault_thresholds.config_file` makes the two agree again: a fault-code override +resolves the same for every source, so the recorded threshold is the fault's own. The node warns +once per code when two sources resolve different policies for it. Entries are kept and evicted in **arrival order**, not by their timestamps. Reporters carry their own clocks, so a report can arrive carrying a timestamp behind one already stored; ordering the diff --git a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_manager_node.hpp b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_manager_node.hpp index 9157102d9..a8eef8d00 100644 --- a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_manager_node.hpp +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_manager_node.hpp @@ -24,11 +24,11 @@ #include "rclcpp/rclcpp.hpp" #include "ros2_medkit_fault_manager/capture_thread_pool.hpp" #include "ros2_medkit_fault_manager/correlation/correlation_engine.hpp" -#include "ros2_medkit_fault_manager/entity_threshold_resolver.hpp" #include "ros2_medkit_fault_manager/fault_audit_log.hpp" #include "ros2_medkit_fault_manager/fault_storage.hpp" #include "ros2_medkit_fault_manager/rosbag_capture.hpp" #include "ros2_medkit_fault_manager/snapshot_capture.hpp" +#include "ros2_medkit_fault_manager/threshold_resolver.hpp" #include "ros2_medkit_msgs/msg/fault_event.hpp" #include "ros2_medkit_msgs/srv/clear_fault.hpp" #include "ros2_medkit_msgs/srv/get_fault.hpp" @@ -187,9 +187,21 @@ class FaultManagerNode : public rclcpp::Node { /// Extract topic name from full topic path (last segment) static std::string extract_topic_name(const std::string & topic_path); - /// Resolve debounce config for a given source_id using entity threshold resolver. - /// Falls back to global config if no entity-specific overrides match. - DebounceConfig resolve_config(const std::string & source_id) const; + /// Resolve the debounce config a report is debounced under. + /// Three layers, each applied on top of the last: the global config, the + /// longest-prefix entity override matching @p source_id, and the exact-match + /// override for @p fault_code. A layer only sets the fields it configures. + DebounceConfig resolve_config(const std::string & source_id, const std::string & fault_code) const; + + /// Warn once per fault code when two sources debounce it under different + /// policies. The counter belongs to the fault code and the entity override to + /// the source, so the report that arrives decides the transition and the other + /// source's policy is silently bypassed (issue #276). + /// @param fault_code The code just reported. + /// @param source_id The source that reported it. + /// @param resolved The config that report resolved to. + void warn_on_conflicting_debounce_policy(const std::string & fault_code, const std::string & source_id, + const DebounceConfig & resolved); /// Create the tamper-evident audit log from parameters (nullptr if disabled). std::unique_ptr create_audit_log(); @@ -215,7 +227,21 @@ class FaultManagerNode : public rclcpp::Node { QueueFullPolicy capture_queue_full_policy_{QueueFullPolicy::kRejectNewest}; DebounceConfig global_config_; ///< Global debounce config (built from ROS params) std::unique_ptr storage_; - std::unique_ptr threshold_resolver_; ///< Per-entity threshold overrides + std::unique_ptr threshold_resolver_; ///< Per-entity threshold overrides + std::unique_ptr fault_code_resolver_; ///< Per-fault_code threshold overrides + + /// The first debounce policy seen for a fault code, and who reported it. + /// Kept only to notice a second source resolving a different policy for the + /// same code, and warned about once. One entry per fault code the node has + /// seen, so it is bounded by the same thing the fault store is. Written from + /// the ReportFault callback, which the node's single-threaded executor + /// serialises with every other callback that touches node state. + struct DebouncePolicyWitness { + DebounceConfig config; ///< The policy the first report resolved to + std::string source_id; ///< The source that reported it + bool warned{false}; ///< Whether the conflict has already been reported + }; + std::unordered_map debounce_policy_witness_; /// Tamper-evident audit log of fault transitions (nullptr when disabled). std::unique_ptr audit_log_; diff --git a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/entity_threshold_resolver.hpp b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/threshold_resolver.hpp similarity index 51% rename from src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/entity_threshold_resolver.hpp rename to src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/threshold_resolver.hpp index b771fb2cf..cbad4b970 100644 --- a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/entity_threshold_resolver.hpp +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/threshold_resolver.hpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -31,6 +32,15 @@ struct EntityDebounceOverride { std::optional healing_threshold; }; +/// Per-fault_code debounce overrides. Unset fields inherit from the layer below: +/// the entity override if one matched, otherwise the global DebounceConfig. +struct FaultCodeDebounceOverride { + std::string fault_code; ///< Exact fault code (e.g. "MOTOR_OVERHEAT") + std::optional confirmation_threshold; + std::optional healing_enabled; + std::optional healing_threshold; +}; + /// Resolves per-entity debounce thresholds using longest-prefix matching. /// /// Given a source_id (entity FQN like "/powertrain/motor_left") and the global @@ -62,4 +72,44 @@ class EntityThresholdResolver { std::vector entries_; }; +/// Resolves per-fault_code debounce thresholds by exact match on the fault code. +/// +/// The debounce counter lives on the fault code while an entity override is +/// selected by the reporting source, so two entities reporting one code debounce +/// it under two policies. A fault-code override is the layer that removes that: +/// it resolves the same whoever reports, and is applied on top of whatever the +/// entity layer produced. +class FaultCodeThresholdResolver { + public: + FaultCodeThresholdResolver() = default; + + /// Construct with a list of fault-code overrides. A code repeated in the list + /// keeps its first entry; YAML loading cannot produce one, a caller can. + explicit FaultCodeThresholdResolver(std::vector entries); + + /// Resolve the effective DebounceConfig for a fault code. + /// `base` is what the layers below already produced (the global config, then + /// any entity override). Fields the code does not set are left as `base` has + /// them. A code with no override returns `base` unchanged. + DebounceConfig resolve(const std::string & fault_code, const DebounceConfig & base) const; + + /// Number of configured fault-code entries. + size_t size() const; + + /// Load fault-code threshold overrides from a YAML file. + /// Returns an empty vector on parse error (logs warning via rcutils). + /// YAML format: map of fault code -> {confirmation_threshold, healing_enabled, healing_threshold} + static std::vector load_from_yaml(const std::string & path); + + private: + std::unordered_map entries_; +}; + +/// Whether two configs debounce a fault the same way. +/// +/// Compares only the three fields an override can carry. `auto_confirm_after_sec` +/// is global-only, so it is the same for every source by construction and would +/// only add noise to the comparison. +bool debounce_policy_equal(const DebounceConfig & a, const DebounceConfig & b); + } // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/src/entity_threshold_resolver.cpp b/src/ros2_medkit_fault_manager/src/entity_threshold_resolver.cpp deleted file mode 100644 index c59aa9518..000000000 --- a/src/ros2_medkit_fault_manager/src/entity_threshold_resolver.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright 2026 mfaferek93 -// -// 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. - -#include "ros2_medkit_fault_manager/entity_threshold_resolver.hpp" - -#include - -#include -#include - -#include "rcutils/logging_macros.h" - -namespace ros2_medkit_fault_manager { - -EntityThresholdResolver::EntityThresholdResolver(std::vector entries) - : entries_(std::move(entries)) { - // Sort by prefix length descending so longest-prefix match is found first - std::sort(entries_.begin(), entries_.end(), [](const EntityDebounceOverride & a, const EntityDebounceOverride & b) { - return a.prefix.size() > b.prefix.size(); - }); -} - -DebounceConfig EntityThresholdResolver::resolve(const std::string & source_id, - const DebounceConfig & global_default) const { - for (const auto & entry : entries_) { - // Check if source_id starts with the prefix at a path boundary - if (source_id.size() >= entry.prefix.size() && source_id.compare(0, entry.prefix.size(), entry.prefix) == 0 && - (source_id.size() == entry.prefix.size() || source_id[entry.prefix.size()] == '/')) { - // Merge: entry overrides take precedence, unset fields inherit global - DebounceConfig result = global_default; - if (entry.confirmation_threshold.has_value()) { - result.confirmation_threshold = *entry.confirmation_threshold; - } - if (entry.healing_enabled.has_value()) { - result.healing_enabled = *entry.healing_enabled; - } - if (entry.healing_threshold.has_value()) { - result.healing_threshold = *entry.healing_threshold; - } - return result; - } - } - return global_default; -} - -size_t EntityThresholdResolver::size() const { - return entries_.size(); -} - -std::vector EntityThresholdResolver::load_from_yaml(const std::string & path) { - std::vector entries; - - if (!std::filesystem::exists(path)) { - RCUTILS_LOG_ERROR_NAMED("entity_threshold_resolver", "Entity thresholds config file not found: %s", path.c_str()); - return entries; - } - - try { - YAML::Node root = YAML::LoadFile(path); - if (!root.IsMap()) { - RCUTILS_LOG_ERROR_NAMED("entity_threshold_resolver", "Entity thresholds config must be a YAML map, got %d in %s", - root.Type(), path.c_str()); - return entries; - } - - for (const auto & item : root) { - EntityDebounceOverride entry; - entry.prefix = item.first.as(); - - if (!item.second.IsMap()) { - RCUTILS_LOG_WARN_NAMED("entity_threshold_resolver", "Skipping non-map entry for prefix '%s' in %s", - entry.prefix.c_str(), path.c_str()); - continue; - } - - auto node = item.second; - if (node["confirmation_threshold"]) { - auto val = node["confirmation_threshold"].as(); - if (val > 0) { - RCUTILS_LOG_WARN_NAMED("entity_threshold_resolver", - "confirmation_threshold for '%s' should be <= 0, got %d. Using %d.", - entry.prefix.c_str(), val, -val); - val = -val; - } - entry.confirmation_threshold = static_cast(val); - } - - if (node["healing_enabled"]) { - entry.healing_enabled = node["healing_enabled"].as(); - } - - if (node["healing_threshold"]) { - auto val = node["healing_threshold"].as(); - if (val < 0) { - RCUTILS_LOG_WARN_NAMED("entity_threshold_resolver", - "healing_threshold for '%s' should be >= 0, got %d. Using %d.", entry.prefix.c_str(), - val, -val); - val = -val; - } - entry.healing_threshold = static_cast(val); - } - - entries.push_back(std::move(entry)); - } - - RCUTILS_LOG_INFO_NAMED("entity_threshold_resolver", "Loaded %zu entity threshold entries from %s", entries.size(), - path.c_str()); - - } catch (const YAML::Exception & e) { - RCUTILS_LOG_ERROR_NAMED("entity_threshold_resolver", "Failed to parse entity thresholds config %s: %s", - path.c_str(), e.what()); - } - - return entries; -} - -} // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp index 146f0e3b4..a2a56de6e 100644 --- a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp @@ -327,6 +327,48 @@ FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node(" } } + // Load per-fault_code threshold overrides (optional). Applied after the entity layer, so a + // code listed here debounces the same whichever source reports it. + auto fault_thresholds_file = declare_parameter("fault_thresholds.config_file", ""); + if (!fault_thresholds_file.empty()) { + auto entries = FaultCodeThresholdResolver::load_from_yaml(fault_thresholds_file); + // Same field-by-field merge as the entity layer, so the same validation: an override can + // break confirmation_threshold < 0 <= healing_threshold even when the global config is valid. + // Checked against the global config, which is the base when no entity override matches. + for (auto & entry : entries) { + DebounceConfig merged = global_config_; + if (entry.confirmation_threshold) { + merged.confirmation_threshold = *entry.confirmation_threshold; + } + if (entry.healing_threshold) { + merged.healing_threshold = *entry.healing_threshold; + } + if (!sanitize_debounce_config(merged)) { + RCLCPP_WARN(get_logger(), + "Fault code '%s' debounce thresholds invalid (need confirmation_threshold < 0 <= " + "healing_threshold); using safe defaults", + entry.fault_code.c_str()); + if (entry.confirmation_threshold) { + entry.confirmation_threshold = merged.confirmation_threshold; + } + if (entry.healing_threshold) { + entry.healing_threshold = merged.healing_threshold; + } + } + } + if (!entries.empty()) { + fault_code_resolver_ = std::make_unique(std::move(entries)); + RCLCPP_INFO(get_logger(), "Loaded %zu per-fault_code threshold overrides from %s", fault_code_resolver_->size(), + fault_thresholds_file.c_str()); + if (auto_confirm_after_sec_ > 0.0) { + RCLCPP_WARN(get_logger(), + "Per-fault_code thresholds are configured but auto_confirm_after_sec=%.1f is also set. " + "Auto-confirmation will bypass fault-code debounce policies for PREFAILED faults.", + auto_confirm_after_sec_); + } + } + } + // Create service servers report_fault_srv_ = create_service( "~/report_fault", [this](const std::shared_ptr & request, @@ -807,9 +849,10 @@ void FaultManagerNode::handle_report_fault( auto fault_before = storage_->get_fault(request->fault_code); std::string status_before = fault_before ? fault_before->status : ""; - // Resolve per-entity debounce config (longest-prefix match on source_id) - // TODO(#276): warn when different entities resolve different configs for the same fault_code - auto resolved_config = resolve_config(request->source_id); + // Resolve the debounce config: global, then the entity override matching + // source_id, then the fault code's own override. + auto resolved_config = resolve_config(request->source_id, request->fault_code); + warn_on_conflicting_debounce_policy(request->fault_code, request->source_id, resolved_config); // Report the fault event (use wall clock time, not sim time, for proper timestamps) const rclcpp::Time event_time = get_wall_clock_time(); @@ -1689,15 +1732,43 @@ bool FaultManagerNode::matches_entity(const std::vector & reporting return false; } -DebounceConfig FaultManagerNode::resolve_config(const std::string & source_id) const { +DebounceConfig FaultManagerNode::resolve_config(const std::string & source_id, const std::string & fault_code) const { DebounceConfig config = global_config_; if (threshold_resolver_) { config = threshold_resolver_->resolve(source_id, global_config_); } + // The fault code is the last layer: it is what the debounce counter belongs to, so an + // override on it settles the policy whichever source reported. + if (fault_code_resolver_) { + config = fault_code_resolver_->resolve(fault_code, config); + } // Defensive: the merged config is validated at load time, but never hand the storage backend a // config that violates confirmation_threshold < 0 <= healing_threshold (the counter would stick). sanitize_debounce_config(config); return config; } +void FaultManagerNode::warn_on_conflicting_debounce_policy(const std::string & fault_code, + const std::string & source_id, + const DebounceConfig & resolved) { + auto [it, inserted] = + debounce_policy_witness_.try_emplace(fault_code, DebouncePolicyWitness{resolved, source_id, false}); + if (inserted || it->second.warned || debounce_policy_equal(it->second.config, resolved)) { + return; + } + + // One counter, two policies: the report that arrives decides the transition, so the other + // source's debounce policy is bypassed for as long as both report this code. + it->second.warned = true; + RCLCPP_WARN(get_logger(), + "Fault code '%s' is debounced two ways: '%s' resolves confirmation=%d healing_enabled=%s healing=%d, " + "'%s' resolves confirmation=%d healing_enabled=%s healing=%d. The debounce counter belongs to the fault " + "code, so whichever source reports decides the transition and the other policy is bypassed. Give the " + "code an entry in fault_thresholds.config_file to settle it for every source.", + fault_code.c_str(), it->second.source_id.c_str(), it->second.config.confirmation_threshold, + it->second.config.healing_enabled ? "true" : "false", it->second.config.healing_threshold, + source_id.c_str(), resolved.confirmation_threshold, resolved.healing_enabled ? "true" : "false", + resolved.healing_threshold); +} + } // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/src/threshold_resolver.cpp b/src/ros2_medkit_fault_manager/src/threshold_resolver.cpp new file mode 100644 index 000000000..882ddbb56 --- /dev/null +++ b/src/ros2_medkit_fault_manager/src/threshold_resolver.cpp @@ -0,0 +1,231 @@ +// Copyright 2026 mfaferek93 +// +// 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. + +#include "ros2_medkit_fault_manager/threshold_resolver.hpp" + +#include + +#include +#include + +#include "rcutils/logging_macros.h" + +namespace ros2_medkit_fault_manager { + +namespace { + +/// The three fields an override may carry, as read from one YAML map node. +struct OverrideFields { + std::optional confirmation_threshold; + std::optional healing_enabled; + std::optional healing_threshold; +}; + +/// Read the three override fields out of a YAML map node, correcting a sign an +/// operator is likely to have written the other way round. `log_name` is the +/// rcutils logger name and `key` names the entry in any message. +OverrideFields parse_override_fields(const YAML::Node & node, const char * log_name, const std::string & key) { + OverrideFields fields; + + if (node["confirmation_threshold"]) { + auto val = node["confirmation_threshold"].as(); + if (val > 0) { + RCUTILS_LOG_WARN_NAMED(log_name, "confirmation_threshold for '%s' should be <= 0, got %d. Using %d.", key.c_str(), + val, -val); + val = -val; + } + fields.confirmation_threshold = static_cast(val); + } + + if (node["healing_enabled"]) { + fields.healing_enabled = node["healing_enabled"].as(); + } + + if (node["healing_threshold"]) { + auto val = node["healing_threshold"].as(); + if (val < 0) { + RCUTILS_LOG_WARN_NAMED(log_name, "healing_threshold for '%s' should be >= 0, got %d. Using %d.", key.c_str(), val, + -val); + val = -val; + } + fields.healing_threshold = static_cast(val); + } + + return fields; +} + +/// Open a threshold config file and hand back its top-level map, or an undefined +/// node when the file is missing, unparseable or not a map. Every rejection is +/// logged here so both loaders report the same way. +YAML::Node load_override_map(const std::string & path, const char * log_name, const char * what) { + if (!std::filesystem::exists(path)) { + RCUTILS_LOG_ERROR_NAMED(log_name, "%s config file not found: %s", what, path.c_str()); + return YAML::Node(YAML::NodeType::Undefined); + } + + try { + YAML::Node root = YAML::LoadFile(path); + if (!root.IsMap()) { + RCUTILS_LOG_ERROR_NAMED(log_name, "%s config must be a YAML map, got %d in %s", what, root.Type(), path.c_str()); + return YAML::Node(YAML::NodeType::Undefined); + } + return root; + } catch (const YAML::Exception & e) { + RCUTILS_LOG_ERROR_NAMED(log_name, "Failed to parse %s config %s: %s", what, path.c_str(), e.what()); + return YAML::Node(YAML::NodeType::Undefined); + } +} + +constexpr const char * kEntityLogName = "entity_threshold_resolver"; +constexpr const char * kFaultCodeLogName = "fault_code_threshold_resolver"; + +} // namespace + +EntityThresholdResolver::EntityThresholdResolver(std::vector entries) + : entries_(std::move(entries)) { + // Sort by prefix length descending so longest-prefix match is found first + std::sort(entries_.begin(), entries_.end(), [](const EntityDebounceOverride & a, const EntityDebounceOverride & b) { + return a.prefix.size() > b.prefix.size(); + }); +} + +DebounceConfig EntityThresholdResolver::resolve(const std::string & source_id, + const DebounceConfig & global_default) const { + for (const auto & entry : entries_) { + // Check if source_id starts with the prefix at a path boundary + if (source_id.size() >= entry.prefix.size() && source_id.compare(0, entry.prefix.size(), entry.prefix) == 0 && + (source_id.size() == entry.prefix.size() || source_id[entry.prefix.size()] == '/')) { + // Merge: entry overrides take precedence, unset fields inherit global + DebounceConfig result = global_default; + if (entry.confirmation_threshold.has_value()) { + result.confirmation_threshold = *entry.confirmation_threshold; + } + if (entry.healing_enabled.has_value()) { + result.healing_enabled = *entry.healing_enabled; + } + if (entry.healing_threshold.has_value()) { + result.healing_threshold = *entry.healing_threshold; + } + return result; + } + } + return global_default; +} + +size_t EntityThresholdResolver::size() const { + return entries_.size(); +} + +std::vector EntityThresholdResolver::load_from_yaml(const std::string & path) { + std::vector entries; + + YAML::Node root = load_override_map(path, kEntityLogName, "Entity thresholds"); + if (!root.IsMap()) { + return entries; + } + + for (const auto & item : root) { + EntityDebounceOverride entry; + entry.prefix = item.first.as(); + + if (!item.second.IsMap()) { + RCUTILS_LOG_WARN_NAMED(kEntityLogName, "Skipping non-map entry for prefix '%s' in %s", entry.prefix.c_str(), + path.c_str()); + continue; + } + + auto fields = parse_override_fields(item.second, kEntityLogName, entry.prefix); + entry.confirmation_threshold = fields.confirmation_threshold; + entry.healing_enabled = fields.healing_enabled; + entry.healing_threshold = fields.healing_threshold; + + entries.push_back(std::move(entry)); + } + + RCUTILS_LOG_INFO_NAMED(kEntityLogName, "Loaded %zu entity threshold entries from %s", entries.size(), path.c_str()); + + return entries; +} + +FaultCodeThresholdResolver::FaultCodeThresholdResolver(std::vector entries) { + for (auto & entry : entries) { + std::string key = entry.fault_code; + entries_.emplace(std::move(key), std::move(entry)); + } +} + +DebounceConfig FaultCodeThresholdResolver::resolve(const std::string & fault_code, const DebounceConfig & base) const { + auto it = entries_.find(fault_code); + if (it == entries_.end()) { + return base; + } + + // Merge on top of what the layers below produced: the code's own fields win, + // the rest stay as the entity override (or the global default) left them. + DebounceConfig result = base; + const auto & entry = it->second; + if (entry.confirmation_threshold.has_value()) { + result.confirmation_threshold = *entry.confirmation_threshold; + } + if (entry.healing_enabled.has_value()) { + result.healing_enabled = *entry.healing_enabled; + } + if (entry.healing_threshold.has_value()) { + result.healing_threshold = *entry.healing_threshold; + } + return result; +} + +size_t FaultCodeThresholdResolver::size() const { + return entries_.size(); +} + +std::vector FaultCodeThresholdResolver::load_from_yaml(const std::string & path) { + std::vector entries; + + YAML::Node root = load_override_map(path, kFaultCodeLogName, "Fault thresholds"); + if (!root.IsMap()) { + return entries; + } + + for (const auto & item : root) { + FaultCodeDebounceOverride entry; + entry.fault_code = item.first.as(); + + if (!item.second.IsMap()) { + RCUTILS_LOG_WARN_NAMED(kFaultCodeLogName, "Skipping non-map entry for fault code '%s' in %s", + entry.fault_code.c_str(), path.c_str()); + continue; + } + + auto fields = parse_override_fields(item.second, kFaultCodeLogName, entry.fault_code); + entry.confirmation_threshold = fields.confirmation_threshold; + entry.healing_enabled = fields.healing_enabled; + entry.healing_threshold = fields.healing_threshold; + + entries.push_back(std::move(entry)); + } + + RCUTILS_LOG_INFO_NAMED(kFaultCodeLogName, "Loaded %zu fault-code threshold entries from %s", entries.size(), + path.c_str()); + + return entries; +} + +bool debounce_policy_equal(const DebounceConfig & a, const DebounceConfig & b) { + return a.confirmation_threshold == b.confirmation_threshold && a.healing_enabled == b.healing_enabled && + a.healing_threshold == b.healing_threshold; +} + +} // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/test/integration/test_fault_code_thresholds_integration.test.py b/src/ros2_medkit_fault_manager/test/integration/test_fault_code_thresholds_integration.test.py new file mode 100644 index 000000000..6e06ef033 --- /dev/null +++ b/src/ros2_medkit_fault_manager/test/integration/test_fault_code_thresholds_integration.test.py @@ -0,0 +1,277 @@ +#!/usr/bin/env python3 +# Copyright 2026 selfpatch +# +# 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. + +""" +Integration tests for per-fault_code debounce thresholds. + +The node is launched with both layers configured, because the point of the +fault-code layer is what it does to the entity layer underneath it: the debounce +counter belongs to the fault code while an entity override is chosen by the +reporting source, so without this layer two entities reporting one code debounce +it two ways (issue #275). The last case here covers the warning that says so when +no fault-code override settles it (issue #276). +""" + +import os +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +import launch_ros.actions +import launch_testing.actions +import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_medkit_msgs.msg import Fault +from ros2_medkit_msgs.srv import GetFault, ReportFault + +# The node's own wording, so a reworded warning fails here rather than going quiet. +CONFLICT_WARNING = 'is debounced two ways' + +# Entities whose debounce policies differ, from test_entity_thresholds.yaml. +LIDAR = '/sensors/lidar/front' +MOTOR = '/powertrain/motor/left' + + +def _output_text(proc_output, process): + """ + Return everything the process has written so far, as one string. + + Concatenated with NO separator: proc_output yields raw stream chunks, not + lines, so joining on a newline would splice one into the middle of a log + line and break a substring match on a message that is plainly there. + """ + return ''.join( + output.text.decode(errors='replace') for output in proc_output[process] + ) + + +def generate_test_description(): + """Launch fault_manager with both the entity and fault-code layers.""" + pkg_share = get_package_share_directory('ros2_medkit_fault_manager') + entity_config = os.path.join( + pkg_share, 'test', 'test_entity_thresholds.yaml' + ) + fault_config = os.path.join(pkg_share, 'test', 'test_fault_thresholds.yaml') + + fault_manager_node = launch_ros.actions.Node( + package='ros2_medkit_fault_manager', + executable='fault_manager_node', + name='fault_manager', + output='screen', + parameters=[{ + 'storage_type': 'memory', + # Global: 5 events. Neither entity nor fault code inherits it below, + # so a case that passes by falling through to the global would fail. + 'confirmation_threshold': -5, + 'healing_enabled': False, + 'healing_threshold': 10, + 'entity_thresholds.config_file': entity_config, + 'fault_thresholds.config_file': fault_config, + }], + # Give the node room to flush coverage data at shutdown before SIGKILL. + sigterm_timeout='30', + sigkill_timeout='15', + ) + + return ( + LaunchDescription([ + fault_manager_node, + launch_testing.actions.ReadyToTest(), + ]), + { + 'fault_manager_node': fault_manager_node, + }, + ) + + +class TestPerFaultCodeThresholds(unittest.TestCase): + """Integration tests for per-fault_code debounce thresholds.""" + + @classmethod + def setUpClass(cls): + """Initialize ROS 2 context and service clients.""" + rclpy.init() + cls.node = Node('test_fault_code_thresholds_client') + + cls.report_client = cls.node.create_client( + ReportFault, '/fault_manager/report_fault' + ) + cls.get_client = cls.node.create_client( + GetFault, '/fault_manager/get_fault' + ) + + assert cls.report_client.wait_for_service(timeout_sec=10.0), \ + 'report_fault service not available' + assert cls.get_client.wait_for_service(timeout_sec=10.0), \ + 'get_fault service not available' + + @classmethod + def tearDownClass(cls): + """Shutdown ROS 2.""" + cls.node.destroy_node() + rclpy.shutdown() + + def _call(self, client, request): + """Call service synchronously.""" + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + self.assertIsNotNone(future.result(), 'Service call timed out') + return future.result() + + def _report(self, fault_code, source_id): + """Report a FAILED event.""" + req = ReportFault.Request() + req.fault_code = fault_code + req.event_type = ReportFault.Request.EVENT_FAILED + req.severity = Fault.SEVERITY_ERROR + req.description = f'Test fault from {source_id}' + req.source_id = source_id + resp = self._call(self.report_client, req) + self.assertTrue(resp.accepted) + + def _status(self, fault_code): + """Get a fault's current status.""" + req = GetFault.Request() + req.fault_code = fault_code + resp = self._call(self.get_client, req) + self.assertTrue(resp.success, resp.error_message) + return resp.fault.status + + # @verifies REQ_INTEROP_107 + def test_01_fault_code_beats_the_entity_that_reports_it(self): + """ + Pin SHARED.OVERHEAT to -3, over the lidar entity's -1. + + Lidar confirms on the first event for any other code, so a single + PREFAILED here is only possible if the fault-code layer was applied on + top of the entity one. + """ + self._report('SHARED.OVERHEAT', LIDAR) + self.assertEqual(self._status('SHARED.OVERHEAT'), Fault.STATUS_PREFAILED) + + self._report('SHARED.OVERHEAT', LIDAR) + self.assertEqual(self._status('SHARED.OVERHEAT'), Fault.STATUS_PREFAILED) + + self._report('SHARED.OVERHEAT', LIDAR) + self.assertEqual(self._status('SHARED.OVERHEAT'), Fault.STATUS_CONFIRMED) + + # @verifies REQ_INTEROP_107 + def test_02_one_code_debounces_alike_from_two_entities(self): + """ + Confirm SHARED.JAM on its own fourth event, whoever reported it. + + Alternating the two sources is the case issue #275 is about: under the + entity layer alone lidar's -1 would confirm this on the second event, + bypassing the motor's policy. + """ + self._report('SHARED.JAM', MOTOR) + self.assertEqual(self._status('SHARED.JAM'), Fault.STATUS_PREFAILED) + + self._report('SHARED.JAM', LIDAR) + self.assertEqual(self._status('SHARED.JAM'), Fault.STATUS_PREFAILED) + + self._report('SHARED.JAM', MOTOR) + self.assertEqual(self._status('SHARED.JAM'), Fault.STATUS_PREFAILED) + + self._report('SHARED.JAM', LIDAR) + self.assertEqual(self._status('SHARED.JAM'), Fault.STATUS_CONFIRMED) + + # @verifies REQ_INTEROP_107 + def test_03_a_code_with_no_override_keeps_the_entity_policy(self): + """The new layer is opt-in: an unlisted code still follows its entity.""" + self._report('MOTOR.ONLY', MOTOR) + self.assertEqual(self._status('MOTOR.ONLY'), Fault.STATUS_PREFAILED) + + self._report('LIDAR.ONLY', LIDAR) + self.assertEqual(self._status('LIDAR.ONLY'), Fault.STATUS_CONFIRMED) + + # @verifies REQ_INTEROP_107, REQ_INTEROP_108 + def test_04_a_half_pinned_code_is_still_reported(self, proc_output, + fault_manager_node): + """ + Report SHARED.PARTIAL, which pins confirmation but not healing. + + The override settles the direction it names and no more: the two sources + heal this code under thresholds 1 and 5. Warning on the whole policy + rather than on the presence of an override is what keeps that visible. + """ + self._report('SHARED.PARTIAL', LIDAR) + self.assertEqual(self._status('SHARED.PARTIAL'), Fault.STATUS_PREFAILED) + + self._report('SHARED.PARTIAL', MOTOR) + self.assertEqual(self._status('SHARED.PARTIAL'), Fault.STATUS_CONFIRMED) + + proc_output.assertWaitFor( + f"Fault code 'SHARED.PARTIAL' {CONFLICT_WARNING}", + process=fault_manager_node, + timeout=10.0, + ) + + # @verifies REQ_INTEROP_108 + def test_05_conflicting_policies_are_reported_once(self, proc_output, + fault_manager_node): + """ + Warn on two entities reporting one unlisted code, and warn once. + + Without the warning the bypass is invisible - the fault confirms under + whichever policy happened to report, and nothing in the log distinguishes + that from the policy the operator configured. + """ + self._report('CONFLICTING.CODE', LIDAR) + self._report('CONFLICTING.CODE', MOTOR) + + proc_output.assertWaitFor( + f"Fault code 'CONFLICTING.CODE' {CONFLICT_WARNING}", + process=fault_manager_node, + timeout=10.0, + ) + + # A warning per report would be a warning per fault event on a busy + # robot, which is why the node keeps a witness and warns once per code. + self._report('CONFLICTING.CODE', LIDAR) + self._report('CONFLICTING.CODE', MOTOR) + text = _output_text(proc_output, fault_manager_node) + self.assertEqual(text.count("Fault code 'CONFLICTING.CODE'"), 1) + + +@launch_testing.post_shutdown_test() +class TestShutdown(unittest.TestCase): + """Verify fault_manager exits cleanly and stayed quiet where it should.""" + + # @verifies REQ_INTEROP_108 + def test_an_overridden_code_never_warns(self, proc_output, + fault_manager_node): + """ + Stay quiet about a code whose override settles it for both sources. + + SHARED.JAM had two sources and no conflict: a fault-code override + resolves the same for both, so there is nothing to warn about. Checked + after shutdown, when the whole output is in hand - asserting an absence + against a stream still being written proves nothing. + """ + text = _output_text(proc_output, fault_manager_node) + self.assertNotIn("Fault code 'SHARED.JAM'", text) + + # And a code only one source ever reports has nothing to conflict with. + # A witness compared against the global config instead of against the + # first report would warn here, on every ordinary single-reporter robot. + self.assertNotIn("Fault code 'SHARED.OVERHEAT'", text) + self.assertNotIn("Fault code 'MOTOR.ONLY'", text) + self.assertNotIn("Fault code 'LIDAR.ONLY'", text) + + def test_exit_code(self, proc_info): + """Check process exit code.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/src/ros2_medkit_fault_manager/test/test_entity_thresholds.cpp b/src/ros2_medkit_fault_manager/test/test_entity_thresholds.cpp index 5a2670277..521ab568e 100644 --- a/src/ros2_medkit_fault_manager/test/test_entity_thresholds.cpp +++ b/src/ros2_medkit_fault_manager/test/test_entity_thresholds.cpp @@ -18,8 +18,8 @@ #include #include "rclcpp/rclcpp.hpp" -#include "ros2_medkit_fault_manager/entity_threshold_resolver.hpp" #include "ros2_medkit_fault_manager/fault_storage.hpp" +#include "ros2_medkit_fault_manager/threshold_resolver.hpp" #include "ros2_medkit_msgs/msg/fault.hpp" #include "ros2_medkit_msgs/srv/report_fault.hpp" diff --git a/src/ros2_medkit_fault_manager/test/test_fault_code_thresholds.cpp b/src/ros2_medkit_fault_manager/test/test_fault_code_thresholds.cpp new file mode 100644 index 000000000..102810e90 --- /dev/null +++ b/src/ros2_medkit_fault_manager/test/test_fault_code_thresholds.cpp @@ -0,0 +1,441 @@ +// Copyright 2026 selfpatch +// +// 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. + +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "ros2_medkit_fault_manager/fault_storage.hpp" +#include "ros2_medkit_fault_manager/threshold_resolver.hpp" +#include "ros2_medkit_msgs/msg/fault.hpp" +#include "ros2_medkit_msgs/srv/report_fault.hpp" + +namespace fs = std::filesystem; +using ros2_medkit_fault_manager::debounce_policy_equal; +using ros2_medkit_fault_manager::DebounceConfig; +using ros2_medkit_fault_manager::EntityDebounceOverride; +using ros2_medkit_fault_manager::EntityThresholdResolver; +using ros2_medkit_fault_manager::FaultCodeDebounceOverride; +using ros2_medkit_fault_manager::FaultCodeThresholdResolver; +using ros2_medkit_fault_manager::InMemoryFaultStorage; +using ros2_medkit_msgs::msg::Fault; +using ros2_medkit_msgs::srv::ReportFault; + +namespace { + +/// The layering the node applies: global, then the entity override matching the +/// source, then the fault code's own override. Mirrors +/// FaultManagerNode::resolve_config so the priority is asserted, not assumed. +DebounceConfig resolve_layered(const EntityThresholdResolver & entities, const FaultCodeThresholdResolver & codes, + const std::string & source_id, const std::string & fault_code, + const DebounceConfig & global) { + return codes.resolve(fault_code, entities.resolve(source_id, global)); +} + +} // namespace + +// --------------------------------------------------------------------------- +// FaultCodeThresholdResolver unit tests +// --------------------------------------------------------------------------- + +class FaultCodeResolverTest : public ::testing::Test { + protected: + DebounceConfig global_; + + void SetUp() override { + global_.confirmation_threshold = -1; + global_.healing_enabled = false; + global_.healing_threshold = 3; + } +}; + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeResolverTest, EmptyResolverReturnsBase) { + FaultCodeThresholdResolver resolver; + auto result = resolver.resolve("MOTOR_OVERHEAT", global_); + EXPECT_EQ(result.confirmation_threshold, -1); + EXPECT_FALSE(result.healing_enabled); + EXPECT_EQ(result.healing_threshold, 3); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeResolverTest, ExactMatchOverridesBase) { + FaultCodeDebounceOverride entry; + entry.fault_code = "MOTOR_OVERHEAT"; + entry.confirmation_threshold = -5; + entry.healing_threshold = 10; + + FaultCodeThresholdResolver resolver({entry}); + auto result = resolver.resolve("MOTOR_OVERHEAT", global_); + EXPECT_EQ(result.confirmation_threshold, -5); + EXPECT_EQ(result.healing_threshold, 10); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeResolverTest, UnknownCodeReturnsBase) { + FaultCodeDebounceOverride entry; + entry.fault_code = "MOTOR_OVERHEAT"; + entry.confirmation_threshold = -5; + + FaultCodeThresholdResolver resolver({entry}); + auto result = resolver.resolve("LIDAR_FAIL", global_); + EXPECT_EQ(result.confirmation_threshold, -1); // Base, untouched +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeResolverTest, MatchIsExactNotPrefix) { + FaultCodeDebounceOverride entry; + entry.fault_code = "MOTOR"; + entry.confirmation_threshold = -5; + + FaultCodeThresholdResolver resolver({entry}); + // A fault code is an identifier, not a path: "MOTOR" must not capture + // "MOTOR_OVERHEAT" the way an entity prefix captures a child entity. + EXPECT_EQ(resolver.resolve("MOTOR_OVERHEAT", global_).confirmation_threshold, -1); + EXPECT_EQ(resolver.resolve("MOTOR", global_).confirmation_threshold, -5); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeResolverTest, PartialOverrideKeepsTheRestOfBase) { + FaultCodeDebounceOverride entry; + entry.fault_code = "ESTOP"; + entry.healing_enabled = true; + // confirmation_threshold and healing_threshold not set + + FaultCodeThresholdResolver resolver({entry}); + auto result = resolver.resolve("ESTOP", global_); + EXPECT_EQ(result.confirmation_threshold, -1); // From base + EXPECT_TRUE(result.healing_enabled); // From the code + EXPECT_EQ(result.healing_threshold, 3); // From base +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeResolverTest, SizeReturnsEntryCount) { + FaultCodeThresholdResolver empty; + EXPECT_EQ(empty.size(), 0u); + + FaultCodeDebounceOverride a; + a.fault_code = "A"; + FaultCodeDebounceOverride b; + b.fault_code = "B"; + FaultCodeThresholdResolver two({a, b}); + EXPECT_EQ(two.size(), 2u); +} + +// --------------------------------------------------------------------------- +// Layering: fault_code override > entity override > global default +// --------------------------------------------------------------------------- + +class LayeredResolutionTest : public ::testing::Test { + protected: + DebounceConfig global_; + EntityThresholdResolver entities_; + + void SetUp() override { + global_.confirmation_threshold = -2; + global_.healing_enabled = false; + global_.healing_threshold = 3; + + EntityDebounceOverride motor; + motor.prefix = "/powertrain/motor"; + motor.confirmation_threshold = -5; + motor.healing_threshold = 10; + + EntityDebounceOverride lidar; + lidar.prefix = "/sensors/lidar"; + lidar.confirmation_threshold = -1; + + entities_ = EntityThresholdResolver({motor, lidar}); + } +}; + +// @verifies REQ_INTEROP_107 +TEST_F(LayeredResolutionTest, FaultCodeOverridesEntity) { + FaultCodeDebounceOverride entry; + entry.fault_code = "OVERHEAT"; + entry.confirmation_threshold = -3; + + FaultCodeThresholdResolver codes({entry}); + auto result = resolve_layered(entities_, codes, "/powertrain/motor/left", "OVERHEAT", global_); + EXPECT_EQ(result.confirmation_threshold, + -3); // The code wins over the entity's -5 + EXPECT_EQ(result.healing_threshold, + 10); // The entity still supplies what the code does not +} + +// @verifies REQ_INTEROP_107 +TEST_F(LayeredResolutionTest, EntityStillAppliesWhenCodeHasNoOverride) { + FaultCodeThresholdResolver codes; + auto result = resolve_layered(entities_, codes, "/powertrain/motor/left", "OVERHEAT", global_); + EXPECT_EQ(result.confirmation_threshold, -5); + EXPECT_EQ(result.healing_threshold, 10); +} + +// @verifies REQ_INTEROP_107 +TEST_F(LayeredResolutionTest, FaultCodeOverrideReachesAnUnconfiguredEntity) { + FaultCodeDebounceOverride entry; + entry.fault_code = "OVERHEAT"; + entry.confirmation_threshold = -3; + + FaultCodeThresholdResolver codes({entry}); + auto result = resolve_layered(entities_, codes, "/some/unconfigured/node", "OVERHEAT", global_); + EXPECT_EQ(result.confirmation_threshold, -3); + EXPECT_EQ(result.healing_threshold, 3); // Global, no entity matched +} + +// This is the cross-entity interference issue #275 describes: the debounce +// counter belongs to the fault code while the entity override is picked by the +// reporting source, so two entities reporting one code debounce it two ways. A +// fault-code override collapses that to a single policy. +// @verifies REQ_INTEROP_107 +TEST_F(LayeredResolutionTest, OneCodeResolvesTheSameForEverySource) { + FaultCodeDebounceOverride entry; + entry.fault_code = "OVERHEAT"; + entry.confirmation_threshold = -4; + entry.healing_enabled = true; + entry.healing_threshold = 6; + + FaultCodeThresholdResolver codes({entry}); + auto from_motor = resolve_layered(entities_, codes, "/powertrain/motor/left", "OVERHEAT", global_); + auto from_lidar = resolve_layered(entities_, codes, "/sensors/lidar/front", "OVERHEAT", global_); + auto from_unknown = resolve_layered(entities_, codes, "/some/unconfigured/node", "OVERHEAT", global_); + + EXPECT_TRUE(debounce_policy_equal(from_motor, from_lidar)); + EXPECT_TRUE(debounce_policy_equal(from_motor, from_unknown)); + EXPECT_EQ(from_motor.confirmation_threshold, -4); +} + +// Without a fault-code override the two sources still disagree - which is +// exactly what the node warns about (issue #276). +// @verifies REQ_INTEROP_108 +TEST_F(LayeredResolutionTest, WithoutACodeOverrideTwoSourcesDisagree) { + FaultCodeThresholdResolver codes; + auto from_motor = resolve_layered(entities_, codes, "/powertrain/motor/left", "OVERHEAT", global_); + auto from_lidar = resolve_layered(entities_, codes, "/sensors/lidar/front", "OVERHEAT", global_); + EXPECT_FALSE(debounce_policy_equal(from_motor, from_lidar)); +} + +// --------------------------------------------------------------------------- +// debounce_policy_equal +// --------------------------------------------------------------------------- + +// @verifies REQ_INTEROP_108 +TEST(DebouncePolicyEqualTest, DiffersOnEachOverridableField) { + DebounceConfig a; + a.confirmation_threshold = -2; + a.healing_enabled = true; + a.healing_threshold = 4; + + EXPECT_TRUE(debounce_policy_equal(a, a)); + + DebounceConfig b = a; + b.confirmation_threshold = -3; + EXPECT_FALSE(debounce_policy_equal(a, b)); + + DebounceConfig c = a; + c.healing_enabled = false; + EXPECT_FALSE(debounce_policy_equal(a, c)); + + DebounceConfig d = a; + d.healing_threshold = 5; + EXPECT_FALSE(debounce_policy_equal(a, d)); +} + +// @verifies REQ_INTEROP_108 +TEST(DebouncePolicyEqualTest, IgnoresGlobalOnlyFields) { + DebounceConfig a; + a.confirmation_threshold = -2; + a.healing_threshold = 4; + + // auto_confirm_after_sec cannot be overridden per entity or per fault code, + // so a difference in it is never a difference between two sources' policies. + DebounceConfig b = a; + b.auto_confirm_after_sec = 30.0; + EXPECT_TRUE(debounce_policy_equal(a, b)); +} + +// --------------------------------------------------------------------------- +// YAML loading tests +// --------------------------------------------------------------------------- + +class FaultCodeYamlLoadTest : public ::testing::Test { + protected: + fs::path tmpdir_; + + void SetUp() override { + tmpdir_ = fs::temp_directory_path() / "test_fault_code_thresholds"; + fs::create_directories(tmpdir_); + } + + void TearDown() override { + fs::remove_all(tmpdir_); + } +}; + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeYamlLoadTest, LoadValidFile) { + auto path = tmpdir_ / "fault_thresholds.yaml"; + { + std::ofstream f(path); + f << "MOTOR_OVERHEAT:\n" + << " confirmation_threshold: -5\n" + << " healing_threshold: 10\n" + << "LIDAR_FAIL:\n" + << " confirmation_threshold: -1\n" + << " healing_threshold: 1\n" + << "ESTOP:\n" + << " healing_enabled: false\n"; + } + + auto entries = FaultCodeThresholdResolver::load_from_yaml(path.string()); + ASSERT_EQ(entries.size(), 3u); + + bool found_motor = false; + for (const auto & e : entries) { + if (e.fault_code == "MOTOR_OVERHEAT") { + EXPECT_EQ(e.confirmation_threshold.value(), -5); + EXPECT_EQ(e.healing_threshold.value(), 10); + EXPECT_FALSE(e.healing_enabled.has_value()); + found_motor = true; + } + } + EXPECT_TRUE(found_motor); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeYamlLoadTest, MissingFileReturnsEmpty) { + auto entries = FaultCodeThresholdResolver::load_from_yaml("/nonexistent/fault_thresholds.yaml"); + EXPECT_TRUE(entries.empty()); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeYamlLoadTest, MalformedYamlReturnsEmpty) { + auto path = tmpdir_ / "bad.yaml"; + { + std::ofstream f(path); + f << "{{{{not valid yaml"; + } + + auto entries = FaultCodeThresholdResolver::load_from_yaml(path.string()); + EXPECT_TRUE(entries.empty()); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeYamlLoadTest, NonMapRootReturnsEmpty) { + auto path = tmpdir_ / "sequence.yaml"; + { + std::ofstream f(path); + f << "- MOTOR_OVERHEAT\n- LIDAR_FAIL\n"; + } + + auto entries = FaultCodeThresholdResolver::load_from_yaml(path.string()); + EXPECT_TRUE(entries.empty()); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeYamlLoadTest, NonMapEntryIsSkippedAndTheRestLoad) { + auto path = tmpdir_ / "mixed.yaml"; + { + std::ofstream f(path); + f << "MOTOR_OVERHEAT: -5\n" + << "LIDAR_FAIL:\n" + << " confirmation_threshold: -1\n"; + } + + auto entries = FaultCodeThresholdResolver::load_from_yaml(path.string()); + ASSERT_EQ(entries.size(), 1u); + EXPECT_EQ(entries[0].fault_code, "LIDAR_FAIL"); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeYamlLoadTest, PositiveConfirmationThresholdAutoNegated) { + auto path = tmpdir_ / "autonegate.yaml"; + { + std::ofstream f(path); + f << "MOTOR_OVERHEAT:\n" + << " confirmation_threshold: 5\n" + << " healing_threshold: -10\n"; + } + + auto entries = FaultCodeThresholdResolver::load_from_yaml(path.string()); + ASSERT_EQ(entries.size(), 1u); + EXPECT_EQ(entries[0].confirmation_threshold.value(), -5); + EXPECT_EQ(entries[0].healing_threshold.value(), 10); +} + +// --------------------------------------------------------------------------- +// Storage: what a fault-code override buys, against the real debounce counter +// --------------------------------------------------------------------------- + +class FaultCodeStorageTest : public ::testing::Test { + protected: + InMemoryFaultStorage storage_; + rclcpp::Clock clock_; + DebounceConfig global_; + EntityThresholdResolver entities_; + FaultCodeThresholdResolver codes_; + + void SetUp() override { + global_.confirmation_threshold = -1; + global_.healing_enabled = false; + global_.healing_threshold = 3; + storage_.set_debounce_config(global_); + + // Motor debounces hard, lidar confirms on the first event. + EntityDebounceOverride motor; + motor.prefix = "/powertrain/motor"; + motor.confirmation_threshold = -5; + EntityDebounceOverride lidar; + lidar.prefix = "/sensors/lidar"; + lidar.confirmation_threshold = -1; + entities_ = EntityThresholdResolver({motor, lidar}); + + // The code both of them report is pinned to three events. + FaultCodeDebounceOverride overheat; + overheat.fault_code = "OVERHEAT"; + overheat.confirmation_threshold = -3; + codes_ = FaultCodeThresholdResolver({overheat}); + } + + void report(const std::string & fault_code, const std::string & source_id) { + storage_.report_fault_event(fault_code, ReportFault::Request::EVENT_FAILED, Fault::SEVERITY_ERROR, "", source_id, + clock_.now(), resolve_layered(entities_, codes_, source_id, fault_code, global_)); + } +}; + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeStorageTest, LidarNoLongerConfirmsTheMotorsCodeEarly) { + // Without the fault-code override, lidar's threshold of -1 would confirm this + // on the second event (the counter is shared). With it, both sources debounce + // at -3. + report("OVERHEAT", "/powertrain/motor/left"); + EXPECT_EQ(storage_.get_fault("OVERHEAT")->status, Fault::STATUS_PREFAILED); + + report("OVERHEAT", "/sensors/lidar/front"); + EXPECT_EQ(storage_.get_fault("OVERHEAT")->status, Fault::STATUS_PREFAILED); + + report("OVERHEAT", "/sensors/lidar/front"); + EXPECT_EQ(storage_.get_fault("OVERHEAT")->status, Fault::STATUS_CONFIRMED); +} + +// @verifies REQ_INTEROP_107 +TEST_F(FaultCodeStorageTest, ACodeWithoutAnOverrideKeepsEntityBehaviour) { + report("MOTOR_STALL", "/powertrain/motor/left"); + EXPECT_EQ(storage_.get_fault("MOTOR_STALL")->status, Fault::STATUS_PREFAILED); + + report("LIDAR_BLOCKED", "/sensors/lidar/front"); + EXPECT_EQ(storage_.get_fault("LIDAR_BLOCKED")->status, Fault::STATUS_CONFIRMED); +} diff --git a/src/ros2_medkit_fault_manager/test/test_fault_thresholds.yaml b/src/ros2_medkit_fault_manager/test/test_fault_thresholds.yaml new file mode 100644 index 000000000..9753b5889 --- /dev/null +++ b/src/ros2_medkit_fault_manager/test/test_fault_thresholds.yaml @@ -0,0 +1,19 @@ +# Test fault-code thresholds config for integration tests. +# +# Every code here is reported by entities that carry their own overrides in +# test_entity_thresholds.yaml, so a run proves the fault code is the layer that +# wins and that it resolves the same whichever entity reports. +SHARED.OVERHEAT: + confirmation_threshold: -3 + +# Pins all three fields, so every source debounces this code identically. +SHARED.JAM: + confirmation_threshold: -4 + healing_enabled: false + healing_threshold: 8 + +# Pins only the confirmation direction. The healing fields still come from +# whichever entity reported, so this code is only half settled - and the node +# still says so. +SHARED.PARTIAL: + confirmation_threshold: -2