feat(fault_manager): entity-scoped rosbag capture by default

Default "entity" mode buffers broadly but writes only the faulting source
node's topics (+ /tf) on confirm, resolved from the fault's reporting source
and intersected with the subscribed set. Broad modes (all/auto/entity) keep
re-resolving so topics whose publishers appear after startup are still
captured. Adds faithful per-topic QoS matching, a ring-buffer RAM cap, and
segment-anchored sensor-topic auto-exclude. Entity resolution is guarded so a
sqlite get_fault() throw on the capture thread cannot crash the node.

Closes #426

Author: mfaferek93

docs/config/fault-manager.rst

@@ -217,12 +217,15 @@ Capture continuous rosbag recordings around fault events.
            enabled: false                  # Enable rosbag recording
            duration_sec: 5.0               # Pre-fault buffer duration
            duration_after_sec: 1.0         # Post-fault recording duration
-           topics: "config"                # Topic selection: "config", "all", or "none"
+           topics: "entity"                # Topic selection: "entity" (default), "config", "all", "explicit"
            include_topics: []              # Additional topics to include
            exclude_topics: []              # Topics to exclude
+           exclude_sensor_topics: true     # Auto-exclude image/points/depth/compressed in broad modes
            lazy_start: false               # Start recording on first fault
            format: "sqlite3"               # Storage format
+           qos_match: true                 # Match each topic's publisher QoS
            storage_path: ""                # Custom storage path
+           max_buffer_mb: 256              # Ring-buffer RAM cap
            max_bag_size_mb: 50             # Max size per bag file
            max_total_storage_mb: 500       # Max total storage
            auto_cleanup: true              # Auto-delete old bags
@@ -244,8 +247,21 @@ Capture continuous rosbag recordings around fault events.
      - ``1.0``
      - How long to record after fault.
    * - ``rosbag.topics``
-     - ``config``
-     - Topic selection mode: ``config`` (per-fault), ``all``, or ``none``.
+     - ``entity``
+     - Topic selection mode: ``entity`` (default; write only the faulting node's
+       topics + ``/tf``), ``config`` (per-fault), ``all``, or ``explicit``.
+   * - ``rosbag.exclude_sensor_topics``
+     - ``true``
+     - In broad modes (``all``/``entity``), auto-exclude high-bandwidth sensor
+       topics (image/points/depth/compressed) to bound memory. Excluded topics are
+       dropped silently; ``include_topics`` re-adds any you need.
+   * - ``rosbag.qos_match``
+     - ``true``
+     - Subscribe with each topic's publisher-offered QoS for faithful capture
+       instead of forcing best-effort.
+   * - ``rosbag.max_buffer_mb``
+     - ``256``
+     - Ring-buffer RAM cap; oldest buffered messages drop past it.
    * - ``rosbag.lazy_start``
      - ``false``
      - Start recording only when first fault occurs.

docs/tutorials/snapshots.rst

@@ -332,12 +332,27 @@ Rosbag Configuration Options
      - Post-fault recording duration. After a fault is confirmed, recording
        continues for this many seconds to capture immediate system response.
    * - ``snapshots.rosbag.topics``
-     - ``"config"``
+     - ``"entity"``
      - Topic selection mode:
 
+       - ``"entity"`` - Default. Subscribe broadly for pre-roll, but on fault
+         confirmation write only the faulting source node's topics (resolved from
+         the fault's reporting source) plus ``/tf`` and ``/tf_static``. Falls back
+         to the full buffer if the source is not a live node.
        - ``"config"`` - Use same topics as JSON snapshots (from config file)
        - ``"all"`` or ``"auto"`` - Auto-discover and record all available topics
        - ``"explicit"`` - Use only topics from ``include_topics`` list
+
+       .. note::
+
+          The default changed from ``"config"`` to ``"entity"`` so an enabled black
+          box produces a useful, fault-scoped bag with no per-topic configuration.
+          Set ``topics: "config"`` to restore the previous behavior.
+
+          Faults reported by the ``diagnostic_bridge`` carry the bridge's own node
+          as the source, so in ``entity`` mode the bag is scoped to the bridge
+          (e.g. ``/diagnostics``) rather than the node that actually failed. Use a
+          manual mode if you need a different scope for bridge-sourced faults.
    * - ``snapshots.rosbag.include_topics``
      - ``[]``
      - Explicit list of topics to record (only used when ``topics: "explicit"``).
@@ -346,6 +361,19 @@ Rosbag Configuration Options
      - ``[]``
      - Topics to exclude from recording (applies to all modes). Useful for
        filtering high-bandwidth topics like camera images.
+   * - ``snapshots.rosbag.exclude_sensor_topics``
+     - ``true``
+     - In broad modes (``all``/``entity``), auto-exclude high-bandwidth sensor
+       topics (image/points/depth/compressed) to bound memory. Dropped silently;
+       ``include_topics`` re-adds any you specifically need.
+   * - ``snapshots.rosbag.qos_match``
+     - ``true``
+     - Subscribe with each topic's publisher-offered QoS (reliable/transient-local
+       where offered) for faithful capture, instead of forcing best-effort.
+   * - ``snapshots.rosbag.max_buffer_mb``
+     - ``256``
+     - In-memory ring-buffer cap; oldest buffered messages drop once exceeded, so a
+       broad subscribe set cannot grow memory without bound.
    * - ``snapshots.rosbag.format``
      - ``"sqlite3"``
      - Bag storage format: ``"sqlite3"`` (default, widely compatible) or

src/ros2_medkit_fault_manager/CMakeLists.txt

@@ -217,6 +217,13 @@ if(BUILD_TESTING)
     TIMEOUT 60
   )
   set_tests_properties(test_entity_thresholds_integration PROPERTIES LABELS "integration")
+
+  add_launch_test(
+    test/test_rosbag_entity_scope.test.py
+    TARGET test_rosbag_entity_scope
+    TIMEOUT 120
+  )
+  set_tests_properties(test_rosbag_entity_scope PROPERTIES LABELS "integration")
   ros2_medkit_relax_vendor_warnings()
 endif()
 

src/ros2_medkit_fault_manager/config/snapshots.yaml

@@ -86,13 +86,18 @@ rosbag:
   # Continue recording after fault is confirmed to capture aftermath
   duration_after_sec: 1.0
 
-  # Topic selection mode (default: "config")
+  # Topic selection mode (default: "entity")
   # Options:
+  #   "entity"   - DEFAULT. Subscribe broadly for pre-roll, but on fault confirmation
+  #                write only the faulting source node's topics (+ /tf, /tf_static),
+  #                resolved from the fault's reporting source. Useful black box with
+  #                zero per-topic config. Falls back to the full buffer if the source
+  #                cannot be resolved to a live node.
   #   "config"   - Use same topics as JSON snapshots (from this config file)
-  #   "all"      - Record all available topics (WARNING: high memory usage!)
+  #   "all"      - Record all available topics
   #   "auto"     - Alias for "all" (auto-discover all topics)
   #   "explicit" - Use only topics from include_topics list below
-  topics: "config"
+  topics: "entity"
 
   # Explicit topic list (only used when topics: "explicit")
   # include_topics:
@@ -107,6 +112,11 @@ rosbag:
   #   - /camera/depth/image_raw
   #   - /pointcloud
 
+  # Auto-exclude high-bandwidth sensor topics (image/points/depth/compressed) in
+  # broad modes ("all"/"auto"/"entity") to bound memory (default: true).
+  # Excluded topics are dropped silently; list them in include_topics to re-add.
+  exclude_sensor_topics: true
+
   # Lazy start mode (default: false)
   # When true, ring buffer only starts when fault enters PREFAILED state
   # Saves resources but may miss early context if fault confirms quickly
@@ -134,6 +144,16 @@ rosbag:
   # Oldest bags are deleted when this limit is exceeded
   max_total_storage_mb: 500
 
+  # Maximum in-memory ring buffer size in MB (default: 256)
+  # Oldest buffered messages are dropped once the buffer exceeds this, so a broad
+  # subscribe set on a busy robot cannot grow memory without bound.
+  max_buffer_mb: 256
+
+  # Match each topic's publisher-offered QoS for faithful capture (default: true)
+  # Captures reliable / transient-local topics without dropping; set false to force
+  # best-effort SensorDataQoS on every subscription.
+  qos_match: true
+
   # Auto-cleanup bag files when fault is cleared (default: true)
   # When true, bag files are deleted when ClearFault is called
   # When false, bag files persist until manually deleted or storage limit hit

src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/rosbag_capture.hpp

@@ -109,6 +109,10 @@ class RosbagCapture {
     return config_.enabled;
   }
 
+  /// Whether a topic is a high-bandwidth sensor stream (image/points/depth/compressed),
+  /// auto-excluded from broad-mode capture. Static + public so it is directly testable.
+  static bool is_high_bandwidth_topic(const std::string & topic);
+
  private:
   /// Initialize subscriptions for configured topics
   void init_subscriptions();
@@ -123,6 +127,17 @@ class RosbagCapture {
   /// Resolve which topics to record based on config
   std::vector<std::string> resolve_topics() const;
 
+  /// Resolve the publisher-offered QoS for a topic so capture is faithful
+  /// (falls back to SensorDataQoS when no publisher is known or qos_match is off)
+  rclcpp::QoS resolve_topic_qos(const std::string & topic) const;
+
+  /// In "entity" mode, compute the set of topics to write for a confirmed fault
+  /// (the faulting source node's pub/sub topics + /tf). Empty set = write all.
+  void resolve_entity_topics(const std::string & fault_code);
+
+  /// Whether a topic should be written to the bag given the active entity filter
+  bool should_capture_topic(const std::string & topic) const;
+
   /// Get message type for a topic
   std::string get_topic_type(const std::string & topic) const;
 
@@ -167,6 +182,13 @@ class RosbagCapture {
   /// Ring buffer for messages
   mutable std::mutex buffer_mutex_;
   std::deque<BufferedMessage> message_buffer_;
+  /// Running byte size of message_buffer_ (guarded by buffer_mutex_), for the RAM cap
+  size_t buffer_bytes_{0};
+
+  /// Topics to write for the in-flight capture in "entity" mode (guarded below).
+  /// Empty = no entity filter, write everything buffered (manual modes / fallback).
+  mutable std::mutex capture_topics_mutex_;
+  std::set<std::string> active_capture_topics_;
 
   /// Subscriptions (kept alive for continuous recording)
   std::vector<rclcpp::GenericSubscription::SharedPtr> subscriptions_;
@@ -197,6 +219,16 @@ class RosbagCapture {
   rclcpp::TimerBase::SharedPtr discovery_retry_timer_;
   std::vector<std::string> pending_topics_;
   int discovery_retry_count_{0};
+
+  /// Topics already subscribed (guards against duplicate subscriptions when the
+  /// broad-mode discovery timer re-resolves the topic set).
+  std::set<std::string> subscribed_topics_;
+
+  /// True in broad modes ("all"/"auto"/"entity"): the discovery timer keeps
+  /// re-resolving for the capture's lifetime so topics whose publishers appear
+  /// after startup are still subscribed (dynamic capture). False in fixed modes
+  /// (config/explicit/list), where only the initial set is retried.
+  bool dynamic_discovery_{false};
 };
 
 }  // namespace ros2_medkit_fault_manager

src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/snapshot_capture.hpp

@@ -38,15 +38,26 @@ struct RosbagConfig {
   /// Duration in seconds to continue recording after fault confirmation
   double duration_after_sec{1.0};
 
-  /// Topic selection mode: "config" (reuse JSON config), "all", or comma-separated list
-  std::string topics{"config"};
+  /// Topic selection mode (default "entity"):
+  ///   "entity"     - subscribe broadly for pre-roll, but on confirm write only the
+  ///                  faulting entity's topics (resolved from the fault's reporting
+  ///                  source node) plus always-on context (/tf, /tf_static)
+  ///   "config"     - reuse JSON snapshot config topics
+  ///   "all"/"auto" - every discovered topic
+  ///   "explicit"   - only include_topics
+  ///   "<a,b,c>"    - comma-separated topic list
+  std::string topics{"entity"};
 
   /// Additional topics to include (added to resolved list)
   std::vector<std::string> include_topics;
 
   /// Topics to exclude from recording
   std::vector<std::string> exclude_topics;
 
+  /// In broad modes ("all"/"auto"/"entity"), skip high-bandwidth sensor topics
+  /// (image/points/depth/compressed) to bound memory; include_topics re-adds them.
+  bool exclude_sensor_topics{true};
+
   /// If true, start ring buffer only when fault enters PREFAILED state
   /// If false (default), ring buffer runs continuously from startup
   bool lazy_start{false};
@@ -64,6 +75,13 @@ struct RosbagConfig {
   /// Maximum total storage for all bag files in MB
   size_t max_total_storage_mb{500};
 
+  /// Cap on the in-memory ring buffer in MB; oldest messages drop past it
+  size_t max_buffer_mb{256};
+
+  /// Subscribe with each topic's publisher-offered QoS for faithful capture
+  /// (reliable/transient-local where offered) instead of forcing best-effort
+  bool qos_match{true};
+
   /// If true, delete bag file when fault is cleared
   bool auto_cleanup{true};
 };

src/ros2_medkit_fault_manager/package.xml

@@ -27,8 +27,10 @@
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_clang_tidy</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>launch_ros</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
+  <test_depend>rosbag2_py</test_depend>
   <test_depend>sensor_msgs</test_depend>
   <test_depend>std_msgs</test_depend>
 

src/ros2_medkit_fault_manager/src/fault_manager_node.cpp

@@ -744,11 +744,13 @@ SnapshotConfig FaultManagerNode::create_snapshot_config() {
       config.rosbag.duration_after_sec = 0.0;
     }
 
-    config.rosbag.topics = declare_parameter<std::string>("snapshots.rosbag.topics", "config");
+    config.rosbag.topics = declare_parameter<std::string>("snapshots.rosbag.topics", "entity");
     config.rosbag.include_topics =
         declare_parameter<std::vector<std::string>>("snapshots.rosbag.include_topics", std::vector<std::string>{});
     config.rosbag.exclude_topics =
         declare_parameter<std::vector<std::string>>("snapshots.rosbag.exclude_topics", std::vector<std::string>{});
+    config.rosbag.exclude_sensor_topics = declare_parameter<bool>("snapshots.rosbag.exclude_sensor_topics", true);
+    config.rosbag.qos_match = declare_parameter<bool>("snapshots.rosbag.qos_match", true);
 
     config.rosbag.lazy_start = declare_parameter<bool>("snapshots.rosbag.lazy_start", false);
     config.rosbag.format = declare_parameter<std::string>("snapshots.rosbag.format", "sqlite3");
@@ -768,14 +770,22 @@ SnapshotConfig FaultManagerNode::create_snapshot_config() {
     }
     config.rosbag.max_total_storage_mb = static_cast<size_t>(max_total_storage);
 
+    int64_t max_buffer = declare_parameter<int64_t>("snapshots.rosbag.max_buffer_mb", 256);
+    if (max_buffer <= 0) {
+      RCLCPP_WARN(get_logger(), "snapshots.rosbag.max_buffer_mb must be positive. Using 256MB");
+      max_buffer = 256;
+    }
+    config.rosbag.max_buffer_mb = static_cast<size_t>(max_buffer);
+
     config.rosbag.auto_cleanup = declare_parameter<bool>("snapshots.rosbag.auto_cleanup", true);
 
     RCLCPP_INFO(get_logger(),
-                "Rosbag capture enabled (duration=%.1fs+%.1fs, topics=%s, lazy=%s, format=%s, "
-                "max_bag=%zuMB, max_total=%zuMB)",
+                "Rosbag capture enabled (duration=%.1fs+%.1fs, topics=%s, qos_match=%s, lazy=%s, format=%s, "
+                "max_buffer=%zuMB, max_bag=%zuMB, max_total=%zuMB)",
                 config.rosbag.duration_sec, config.rosbag.duration_after_sec, config.rosbag.topics.c_str(),
-                config.rosbag.lazy_start ? "true" : "false", config.rosbag.format.c_str(),
-                config.rosbag.max_bag_size_mb, config.rosbag.max_total_storage_mb);
+                config.rosbag.qos_match ? "true" : "false", config.rosbag.lazy_start ? "true" : "false",
+                config.rosbag.format.c_str(), config.rosbag.max_buffer_mb, config.rosbag.max_bag_size_mb,
+                config.rosbag.max_total_storage_mb);
   }
 
   if (config.enabled) {