diff --git a/.gitignore b/.gitignore index 0bdc8bcb..b17fbd9c 100644 --- a/.gitignore +++ b/.gitignore @@ -246,4 +246,4 @@ AMENT_IGNORE # End of https://www.toptal.com/developers/gitignore/api/ros2,c++,pythonPLAN.md -PLAN.md \ No newline at end of file +PLAN.mdsrc/dynamic_message_introspection/ diff --git a/docs/api/messages.rst b/docs/api/messages.rst index 80b0ef88..678538b2 100644 --- a/docs/api/messages.rst +++ b/docs/api/messages.rst @@ -218,8 +218,8 @@ Clear/acknowledge a fault. string message # Status message or error description string[] auto_cleared_codes # Symptoms auto-cleared with root cause -GetFaults.srv -~~~~~~~~~~~~~ +ListFaults.srv +~~~~~~~~~~~~~~ Query faults from the FaultManager with optional filtering. @@ -247,7 +247,7 @@ Query faults from the FaultManager with optional filtering. .. code-block:: cpp - auto request = std::make_shared(); + auto request = std::make_shared(); request->filter_by_severity = false; request->statuses = {"CONFIRMED", "PREFAILED"}; diff --git a/docs/api/rest.rst b/docs/api/rest.rst index b05dba06..5a658534 100644 --- a/docs/api/rest.rst +++ b/docs/api/rest.rst @@ -383,7 +383,71 @@ Query and manage faults. } ``GET /api/v1/components/{id}/faults/{fault_code}`` - Get specific fault details. + Get details of a specific fault including environment data. + + **Example Response (200 OK):** + + .. code-block:: json + + { + "item": { + "code": "MOTOR_OVERHEAT", + "fault_name": "Motor temperature exceeded threshold", + "severity": 2, + "status": { + "aggregatedStatus": "active", + "testFailed": "1", + "confirmedDTC": "1", + "pendingDTC": "0" + } + }, + "environment_data": { + "extended_data_records": { + "first_occurrence": "2026-02-04T10:30:00.000Z", + "last_occurrence": "2026-02-04T10:35:00.000Z" + }, + "snapshots": [ + { + "type": "freeze_frame", + "name": "motor_temperature", + "data": 105.5, + "x-medkit": { + "topic": "/motor/temperature", + "message_type": "sensor_msgs/msg/Temperature", + "full_data": {"temperature": 105.5, "variance": 0.1}, + "captured_at": "2026-02-04T10:30:00.123Z" + } + }, + { + "type": "rosbag", + "name": "fault_recording", + "bulk_data_uri": "/apps/motor_controller/bulk-data/rosbags/550e8400-e29b-41d4-a716-446655440000", + "size_bytes": 1234567, + "duration_sec": 6.0, + "format": "mcap" + } + ] + }, + "x-medkit": { + "occurrence_count": 3, + "reporting_sources": ["/powertrain/motor_controller"], + "severity_label": "ERROR" + } + } + + **Status Object:** + + The ``status`` object follows SOVD fault status specification: + + - ``aggregatedStatus``: Overall status (``active``, ``passive``, ``cleared``) + - ``testFailed``: Test failed indicator (``0`` or ``1``) + - ``confirmedDTC``: Confirmed DTC indicator (``0`` or ``1``) + - ``pendingDTC``: Pending DTC indicator (``0`` or ``1``) + + **Snapshot Types:** + + - ``freeze_frame``: Topic data captured at fault confirmation + - ``rosbag``: Recording file available via bulk-data endpoint ``DELETE /api/v1/components/{id}/faults/{fault_code}`` Clear a fault. @@ -391,6 +455,100 @@ Query and manage faults. - **200:** Fault cleared - **404:** Fault not found +Bulk Data Endpoints +------------------- + +Download large binary data (rosbags, logs) associated with entities. +All entity types are supported: apps, components, areas, functions, and nested entities. + +List Categories +~~~~~~~~~~~~~~~ + +``GET /api/v1/{entity-path}/bulk-data`` + +List available bulk-data categories for an entity. + +**Supported entity paths:** + +- ``/apps/{app-id}`` +- ``/components/{component-id}`` +- ``/areas/{area-id}`` +- ``/functions/{function-id}`` +- ``/areas/{area-id}/subareas/{subarea-id}`` +- ``/components/{component-id}/subcomponents/{subcomponent-id}`` + +**Example:** + +.. code-block:: bash + + curl http://localhost:8080/api/v1/apps/motor_controller/bulk-data + +**Response (200 OK):** + +.. code-block:: json + + { + "items": ["rosbags"] + } + +List Bulk Data Items +~~~~~~~~~~~~~~~~~~~~ + +``GET /api/v1/{entity-path}/bulk-data/{category}`` + +List all bulk-data items in a category for the entity. + +**Example:** + +.. code-block:: bash + + curl http://localhost:8080/api/v1/apps/motor_controller/bulk-data/rosbags + +**Response (200 OK):** + +.. code-block:: json + + { + "items": [ + { + "id": "550e8400-e29b-41d4-a716-446655440000", + "name": "MOTOR_OVERHEAT recording 2026-02-04T10:30:00Z", + "mimetype": "application/x-mcap", + "size": 1234567, + "creation_date": "2026-02-04T10:30:00.000Z", + "x-medkit": { + "fault_code": "MOTOR_OVERHEAT", + "duration_sec": 6.0, + "format": "mcap" + } + } + ] + } + +Download Bulk Data +~~~~~~~~~~~~~~~~~~ + +``GET /api/v1/{entity-path}/bulk-data/{category}/{id}`` + +Download a specific bulk-data file. + +**Response Headers:** + +- ``Content-Type``: ``application/x-mcap`` (MCAP format) or ``application/x-sqlite3`` (db3) +- ``Content-Disposition``: ``attachment; filename="FAULT_CODE.mcap"`` +- ``Access-Control-Expose-Headers``: ``Content-Disposition`` + +**Example:** + +.. code-block:: bash + + curl -O -J http://localhost:8080/api/v1/apps/motor_controller/bulk-data/rosbags/550e8400-e29b-41d4-a716-446655440000 + +**Response Codes:** + +- **200 OK**: File content +- **404 Not Found**: Entity, category, or bulk-data ID not found + Authentication Endpoints ------------------------ @@ -525,7 +683,8 @@ The gateway implements a subset of the SOVD (Service-Oriented Vehicle Diagnostic - Data access (``/data``) - Operations (``/operations``, ``/executions``) - Configurations (``/configurations``) -- Faults (``/faults``) +- Faults (``/faults``) with ``environment_data`` and SOVD status object +- Bulk Data (``/bulk-data``) for binary data downloads (rosbags, logs) **ros2_medkit Extensions:** @@ -533,6 +692,7 @@ The gateway implements a subset of the SOVD (Service-Oriented Vehicle Diagnostic - ``/version-info`` - Gateway version information - ``/manifest/status`` - Manifest discovery status - SSE fault streaming - Real-time fault notifications +- ``x-medkit`` extension fields in responses See Also -------- diff --git a/docs/changelog.rst b/docs/changelog.rst index 8debaaa7..54140c6a 100644 --- a/docs/changelog.rst +++ b/docs/changelog.rst @@ -6,6 +6,50 @@ All notable changes to ros2_medkit are documented in this file. The format is based on `Keep a Changelog `_, and this project adheres to `Semantic Versioning `_. +[Unreleased] +------------ + +Added +~~~~~ + +* SOVD bulk-data endpoints for all entity types: + + - ``GET /{entity}/bulk-data`` - list available bulk-data categories + - ``GET /{entity}/bulk-data/{category}`` - list bulk-data descriptors + - ``GET /{entity}/bulk-data/{category}/{id}`` - download bulk-data file + +* Inline ``environment_data`` in fault response with: + + - ``extended_data_records``: First/last occurrence timestamps + - ``snapshots[]``: Array of freeze_frame and rosbag entries + +* SOVD-compliant ``status`` object in fault response with aggregatedStatus, + testFailed, confirmedDTC, pendingDTC fields +* UUID identifiers for rosbag bulk-data items +* ``x-medkit`` extensions with occurrence_count, severity_label + +Changed +~~~~~~~ + +* Fault response structure now SOVD-compliant with ``item`` wrapper +* Rosbag downloads use SOVD bulk-data pattern instead of legacy endpoints +* Rosbag IDs changed from timestamps to UUIDs + +Removed +~~~~~~~ + +* ``GET /faults/{code}/snapshots`` - use ``environment_data`` in fault response +* ``GET /faults/{code}/snapshots/bag`` - use bulk-data endpoint +* ``GET /{entity}/faults/{code}/snapshots`` - use ``environment_data`` +* ``GET /{entity}/faults/{code}/snapshots/bag`` - use bulk-data endpoint + +**Breaking Changes:** + +* Fault response structure changed - clients must update to handle ``item`` wrapper + and ``environment_data`` structure +* Legacy snapshot endpoints removed - migrate to inline snapshots and bulk-data +* Rosbag identifiers changed from timestamps to UUIDs + [0.1.0] - 2026-02-01 -------------------- @@ -34,7 +78,7 @@ Added **Fault Manager (ros2_medkit_fault_manager)** - Centralized fault storage and management node -- ROS 2 services: ``report_fault``, ``get_faults``, ``clear_fault`` +- ROS 2 services: ``report_fault``, ``list_faults``, ``clear_fault`` - AUTOSAR DEM-style debounce lifecycle (PREFAILED → CONFIRMED → HEALED → CLEARED) - Fault aggregation from multiple sources - Severity escalation @@ -65,7 +109,7 @@ Added - ``Fault.msg``: Fault status message with severity, timestamps, sources - ``FaultEvent.msg``: Fault event for subscriptions - ``ReportFault.srv``: Service for reporting faults -- ``GetFaults.srv``: Service for querying faults with filters +- ``ListFaults.srv``: Service for querying faults with filters - ``ClearFault.srv``: Service for clearing faults **Documentation** @@ -102,11 +146,11 @@ specification adapted for ROS 2: - Data access (read, write) - Operations (services, actions with executions) - Configurations (parameters) -- Faults (query, clear) +- Faults (query, clear) with environment_data +- Bulk data transfer (rosbags via bulk-data endpoints) Not yet implemented: -- Bulk data transfer - Software updates - Locks - Triggers diff --git a/docs/requirements/specs/bulkdata.rst b/docs/requirements/specs/bulkdata.rst index 1fd6d7b9..36264aa0 100644 --- a/docs/requirements/specs/bulkdata.rst +++ b/docs/requirements/specs/bulkdata.rst @@ -3,25 +3,34 @@ BulkData .. req:: GET /{entity}/bulk-data :id: REQ_INTEROP_071 - :status: open - :tags: BulkData + :status: verified + :tags: BulkData, SOVD The endpoint shall provide an overview of bulk-data categories supported by the addressed entity. + Gateway supports all entity types: apps, components, areas, functions, and nested entities. + Currently implemented category: ``rosbags``. + .. req:: GET /{entity}/bulk-data/{category} :id: REQ_INTEROP_072 - :status: open - :tags: BulkData + :status: verified + :tags: BulkData, SOVD The endpoint shall list all bulk-data items available in the addressed bulk-data category on the entity. + Returns BulkDataDescriptor array with id, name, mimetype, size, creation_date fields. + Includes ``x-medkit`` extensions with fault_code, duration_sec, format. + .. req:: GET /{entity}/bulk-data/{category}/{bulk-data-id} :id: REQ_INTEROP_073 - :status: open - :tags: BulkData + :status: verified + :tags: BulkData, SOVD The endpoint shall return the content of the addressed bulk-data item or its access information (e.g. download location). + Returns binary file content with appropriate Content-Type (application/x-mcap or application/x-sqlite3) + and Content-Disposition headers for browser download. + .. req:: POST /{entity}/bulk-data/{category} :id: REQ_INTEROP_074 :status: open diff --git a/docs/requirements/specs/faults.rst b/docs/requirements/specs/faults.rst index aba1c74b..2c3be3f8 100644 --- a/docs/requirements/specs/faults.rst +++ b/docs/requirements/specs/faults.rst @@ -11,9 +11,17 @@ Faults .. req:: GET /{entity}/faults/{code} :id: REQ_INTEROP_013 :status: verified - :tags: Faults + :tags: Faults, SOVD + + The endpoint shall return detailed information for the addressed diagnostic fault code, + including environment data captured at fault confirmation. + + Response includes: - The endpoint shall return detailed information for the addressed diagnostic fault code. + - ``item``: Fault details with SOVD-compliant ``status`` object (aggregatedStatus, testFailed, confirmedDTC, pendingDTC) + - ``environment_data``: Extended data records (timestamps) and snapshots array + - ``environment_data.snapshots[]``: Array of freeze_frame (topic data) and rosbag (bulk-data reference) entries + - ``x-medkit``: Extension fields (occurrence_count, reporting_sources, severity_label) .. req:: DELETE /{entity}/faults :id: REQ_INTEROP_014 @@ -29,17 +37,13 @@ Faults The endpoint shall clear the addressed diagnostic fault code for the entity, if permitted. -.. req:: GET /{entity}/faults/{code}/snapshots +.. req:: Fault Snapshot and Rosbag Capture :id: REQ_INTEROP_088 :status: verified - :tags: Faults, Snapshots - - The endpoint shall return topic data snapshots captured when the addressed fault transitioned to CONFIRMED status, enabling post-mortem debugging of system state at the time of fault occurrence. - - .. note:: + :tags: Faults, BulkData - This endpoint corresponds to SOVD "environment data" concept. In SOVD terminology, - environment data refers to system state captured at the time of fault occurrence - (similar to UDS freeze frames). We use "snapshots" as ROS 2-specific terminology - for consistency with the ROS 2 ecosystem. + When a fault is confirmed, the system shall automatically capture diagnostic snapshots + (freeze-frame topic data) and rosbag recordings as configured. Captured data shall be + stored as environment data associated with the fault and accessible via the bulk-data + endpoints on the fault's reporting entity. diff --git a/docs/tutorials/fault-correlation.rst b/docs/tutorials/fault-correlation.rst index 3849e279..a69c2d1c 100644 --- a/docs/tutorials/fault-correlation.rst +++ b/docs/tutorials/fault-correlation.rst @@ -68,7 +68,7 @@ How It Works fm -> db: Store fault (muted) == Query Faults == - src -> fm: GetFaults() + src -> fm: ListFaults() fm --> src: [ESTOP_001], muted_count=2 == Clear Root Cause == @@ -116,7 +116,7 @@ How It Works ce --> fm: should_mute=true (not representative) == Query Result == - src -> fm: GetFaults(include_clusters=true) + src -> fm: ListFaults(include_clusters=true) fm --> src: [SENSOR_003]\nclusters: [{id: "sensor_storm_1",\n fault_codes: [001,002,003,004],\n representative: "SENSOR_003"}] @enduml diff --git a/docs/tutorials/snapshots.rst b/docs/tutorials/snapshots.rst index a1845f64..631589a5 100644 --- a/docs/tutorials/snapshots.rst +++ b/docs/tutorials/snapshots.rst @@ -124,48 +124,73 @@ For fault-specific topic capture, create a YAML configuration file: Querying Snapshots ------------------ -**Get all snapshots for a fault:** +Snapshots are included inline in the fault response as ``environment_data``: + +**Get fault details with snapshots:** .. code-block:: bash - curl http://localhost:8080/api/v1/faults/MOTOR_OVERHEAT/snapshots + curl http://localhost:8080/api/v1/apps/motor_controller/faults/MOTOR_OVERHEAT **Response:** .. code-block:: json { - "fault_code": "MOTOR_OVERHEAT", - "captured_at": 1735830000.123, - "topics": { - "/joint_states": { - "message_type": "sensor_msgs/msg/JointState", - "data": { - "name": ["joint1", "joint2"], - "position": [1.57, 0.0] - } + "item": { + "code": "MOTOR_OVERHEAT", + "fault_name": "Motor temperature exceeded threshold", + "severity": 2, + "status": { + "aggregatedStatus": "active", + "testFailed": "1", + "confirmedDTC": "1" + } + }, + "environment_data": { + "extended_data_records": { + "first_occurrence": "2026-02-04T10:30:00.000Z", + "last_occurrence": "2026-02-04T10:35:00.000Z" }, - "/motor/temperature": { - "message_type": "sensor_msgs/msg/Temperature", - "data": { - "temperature": 85.5, - "variance": 0.1 + "snapshots": [ + { + "type": "freeze_frame", + "name": "motor_temperature", + "data": 85.5, + "x-medkit": { + "topic": "/motor/temperature", + "message_type": "sensor_msgs/msg/Temperature", + "full_data": {"temperature": 85.5, "variance": 0.1}, + "captured_at": "2026-02-04T10:30:00.123Z" + } + }, + { + "type": "rosbag", + "name": "fault_recording", + "bulk_data_uri": "/apps/motor_controller/bulk-data/rosbags/550e8400-e29b-41d4-a716-446655440000", + "size_bytes": 1234567, + "duration_sec": 6.0, + "format": "mcap" } - } + ] + }, + "x-medkit": { + "occurrence_count": 3, + "reporting_sources": ["/powertrain/motor_controller"] } } -**Filter by specific topic:** - -.. code-block:: bash +**Snapshot Types:** - curl "http://localhost:8080/api/v1/faults/MOTOR_OVERHEAT/snapshots?topic=/joint_states" +- ``freeze_frame``: Topic data captured at fault confirmation (JSON format) +- ``rosbag``: Recording file available via bulk-data endpoint (binary format) -**Component-scoped snapshots:** +**Get snapshots from fault response using jq:** .. code-block:: bash - curl http://localhost:8080/api/v1/components/motor_controller/faults/MOTOR_OVERHEAT/snapshots + curl http://localhost:8080/api/v1/apps/motor_controller/faults/MOTOR_OVERHEAT | \ + jq '.environment_data.snapshots' Example Workflow ---------------- @@ -199,7 +224,8 @@ This example demonstrates the complete snapshot capture workflow. .. code-block:: bash - curl http://localhost:8080/api/v1/faults/NAV_ERROR/snapshots + curl http://localhost:8080/api/v1/apps/nav_node/faults/NAV_ERROR | \ + jq '.environment_data.snapshots' The response will contain the odometry data that was captured at the moment the fault was confirmed. @@ -430,24 +456,53 @@ Saves resources but may miss context if fault confirms before buffer fills. Downloading Rosbag Files ^^^^^^^^^^^^^^^^^^^^^^^^ -**Via REST API:** +Rosbag files are downloaded via SOVD bulk-data endpoints. + +**1. List available rosbags for an entity:** .. code-block:: bash - # Download bag archive (.tar.gz containing full bag directory) - curl -O -J http://localhost:8080/api/v1/faults/MOTOR_OVERHEAT/snapshots/bag + curl http://localhost:8080/api/v1/apps/motor_controller/bulk-data/rosbags - # Extract the archive - tar -xzf fault_MOTOR_OVERHEAT_20260124_153045.tar.gz +**Response:** + +.. code-block:: json + + { + "items": [ + { + "id": "550e8400-e29b-41d4-a716-446655440000", + "name": "MOTOR_OVERHEAT recording", + "mimetype": "application/x-mcap", + "size": 1234567, + "creation_date": "2026-02-04T10:30:00.000Z", + "x-medkit": { + "fault_code": "MOTOR_OVERHEAT", + "duration_sec": 6.0, + "format": "mcap" + } + } + ] + } + +**2. Download a specific rosbag:** + +Use the ``bulk_data_uri`` from the fault response, or construct from listing: + +.. code-block:: bash - # Play back the bag - ros2 bag play fault_MOTOR_OVERHEAT_1735830000/ + # Using bulk_data_uri from fault response + curl -O -J http://localhost:8080/api/v1/apps/motor_controller/bulk-data/rosbags/550e8400-e29b-41d4-a716-446655440000 -The REST API returns a compressed tar.gz archive containing the complete bag -directory structure (``metadata.yaml`` and all storage segments). This allows -direct playback with ``ros2 bag play`` after extraction. +The ``-J`` flag uses the server-provided filename from ``Content-Disposition`` header. -**Via ROS 2 service:** +**3. Play back the rosbag:** + +.. code-block:: bash + + ros2 bag play MOTOR_OVERHEAT.mcap + +**Via ROS 2 service (alternative):** .. code-block:: bash @@ -500,6 +555,45 @@ For development with maximum context: See Also -------- +- :doc:`../api/rest` - REST API reference (Bulk Data section) - :doc:`../requirements/specs/faults` - Fault API requirements - `Gateway README `_ - REST API reference - `config/snapshots.yaml `_ - Full configuration reference + +Migration from Legacy Endpoints +------------------------------- + +If you were using the legacy snapshot endpoints, migrate to the new SOVD-compliant API: + +**Snapshots:** + +.. list-table:: + :widths: 45 55 + :header-rows: 1 + + * - Previous (removed) + - Current + * - ``GET /faults/{code}/snapshots`` + - ``GET /apps/{app}/faults/{code}`` → ``environment_data.snapshots[]`` + * - ``GET /components/{id}/faults/{code}/snapshots`` + - ``GET /components/{id}/faults/{code}`` → ``environment_data.snapshots[]`` + +**Rosbag Downloads:** + +.. list-table:: + :widths: 45 55 + :header-rows: 1 + + * - Previous (removed) + - Current + * - ``GET /faults/{code}/snapshots/bag`` + - ``GET /apps/{app}/bulk-data/rosbags/{id}`` + * - ``GET /components/{id}/faults/{code}/snapshots/bag`` + - ``GET /components/{id}/bulk-data/rosbags/{id}`` + +**Key Changes:** + +1. **Snapshots inline**: No separate snapshot endpoint; data is in fault response +2. **Bulk-data pattern**: Rosbags use SOVD bulk-data with UUID identifiers +3. **Entity-scoped**: Bulk-data endpoints require entity path (e.g., ``/apps/motor``) +4. **SOVD status**: Fault response includes SOVD-compliant ``status`` object 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 685999a6..e026a2d9 100644 --- a/src/ros2_medkit_diagnostic_bridge/test/test_integration.test.py +++ b/src/ros2_medkit_diagnostic_bridge/test/test_integration.test.py @@ -26,7 +26,7 @@ import rclpy from rclpy.node import Node from ros2_medkit_msgs.msg import Fault -from ros2_medkit_msgs.srv import GetFaults +from ros2_medkit_msgs.srv import ListFaults def generate_test_description(): @@ -80,14 +80,14 @@ def setUpClass(cls): # Create publisher for diagnostics cls.diag_pub = cls.node.create_publisher(DiagnosticArray, '/diagnostics', 10) - # Create client for GetFaults service - cls.get_faults_client = cls.node.create_client( - GetFaults, '/fault_manager/get_faults' + # Create client for ListFaults service + cls.list_faults_client = cls.node.create_client( + ListFaults, '/fault_manager/list_faults' ) # Wait for services - assert cls.get_faults_client.wait_for_service(timeout_sec=10.0), \ - 'GetFaults service not available' + assert cls.list_faults_client.wait_for_service(timeout_sec=10.0), \ + 'ListFaults service not available' # Wait for bridge to connect time.sleep(1.0) @@ -98,13 +98,13 @@ def tearDownClass(cls): cls.node.destroy_node() rclpy.shutdown() - def get_faults(self, statuses=None): + def list_faults(self, statuses=None): """Get faults from FaultManager.""" - request = GetFaults.Request() + request = ListFaults.Request() request.filter_by_severity = False request.statuses = statuses or [] - future = self.get_faults_client.call_async(request) + future = self.list_faults_client.call_async(request) rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) return future.result().faults @@ -127,7 +127,7 @@ def test_01_error_diagnostic_creates_fault(self): """Test that ERROR diagnostic creates a fault in FaultManager.""" self.publish_diagnostic('test_sensor', DiagnosticStatus.ERROR, 'Sensor failure') - faults = self.get_faults() + faults = self.list_faults() fault_codes = [f.fault_code for f in faults] self.assertIn('TEST_SENSOR', fault_codes) @@ -140,7 +140,7 @@ def test_02_warn_diagnostic_creates_fault(self): for _ in range(3): self.publish_diagnostic('warning_component', DiagnosticStatus.WARN, 'Low battery') - faults = self.get_faults() + faults = self.list_faults() fault_codes = [f.fault_code for f in faults] self.assertIn('WARNING_COMPONENT', fault_codes) @@ -151,7 +151,7 @@ def test_03_stale_diagnostic_creates_critical_fault(self): """Test that STALE diagnostic creates a CRITICAL fault.""" self.publish_diagnostic('stale_sensor', DiagnosticStatus.STALE, 'No data') - faults = self.get_faults() + faults = self.list_faults() fault_codes = [f.fault_code for f in faults] self.assertIn('STALE_SENSOR', fault_codes) @@ -163,7 +163,7 @@ def test_04_ok_diagnostic_heals_fault(self): # First create a fault self.publish_diagnostic('healing_test', DiagnosticStatus.ERROR, 'Error') - faults = self.get_faults() + faults = self.list_faults() self.assertIn('HEALING_TEST', [f.fault_code for f in faults]) # Send OK multiple times to ensure PASSED event reaches FaultManager @@ -171,7 +171,7 @@ def test_04_ok_diagnostic_heals_fault(self): self.publish_diagnostic('healing_test', DiagnosticStatus.OK) # Check fault is healed (query all statuses to find it) - faults = self.get_faults(statuses=[Fault.STATUS_CONFIRMED, Fault.STATUS_HEALED]) + faults = self.list_faults(statuses=[Fault.STATUS_CONFIRMED, Fault.STATUS_HEALED]) fault = next((f for f in faults if f.fault_code == 'HEALING_TEST'), None) self.assertIsNotNone(fault, 'HEALING_TEST fault not found') # After PASSED events with healing_threshold=1, fault should be HEALED @@ -188,7 +188,7 @@ def test_05_fault_code_generation(self): for diag_name, expected_code in test_cases: self.publish_diagnostic(diag_name, DiagnosticStatus.ERROR) - faults = self.get_faults() + faults = self.list_faults() fault_codes = [f.fault_code for f in faults] self.assertIn(expected_code, fault_codes, diff --git a/src/ros2_medkit_fault_manager/README.md b/src/ros2_medkit_fault_manager/README.md index 171402c2..c9b2e771 100644 --- a/src/ros2_medkit_fault_manager/README.md +++ b/src/ros2_medkit_fault_manager/README.md @@ -21,7 +21,7 @@ ros2 service call /fault_manager/report_fault ros2_medkit_msgs/srv/ReportFault \ "{fault_code: 'MOTOR_OVERHEAT', event_type: 0, severity: 2, description: 'Motor temp exceeded', source_id: '/motor_node'}" # Query faults -ros2 service call /fault_manager/get_faults ros2_medkit_msgs/srv/GetFaults \ +ros2 service call /fault_manager/list_faults ros2_medkit_msgs/srv/ListFaults \ "{statuses: ['CONFIRMED']}" # Clear a fault @@ -34,7 +34,7 @@ ros2 service call /fault_manager/clear_fault ros2_medkit_msgs/srv/ClearFault \ | Service | Type | Description | |---------|------|-------------| | `~/report_fault` | `ros2_medkit_msgs/srv/ReportFault` | Report a fault occurrence | -| `~/get_faults` | `ros2_medkit_msgs/srv/GetFaults` | Query faults with filtering | +| `~/list_faults` | `ros2_medkit_msgs/srv/ListFaults` | Query faults with filtering | | `~/clear_fault` | `ros2_medkit_msgs/srv/ClearFault` | Clear/acknowledge a fault | | `~/get_snapshots` | `ros2_medkit_msgs/srv/GetSnapshots` | Get topic snapshots for a fault | @@ -172,7 +172,7 @@ ros2 service call /fault_manager/report_fault ros2_medkit_msgs/srv/ReportFault \ "{fault_code: 'SENSOR_FAIL', event_type: 1, severity: 0, description: '', source_id: '/sensor'}" # Query all statuses including PREFAILED -ros2 service call /fault_manager/get_faults ros2_medkit_msgs/srv/GetFaults \ +ros2 service call /fault_manager/list_faults ros2_medkit_msgs/srv/ListFaults \ "{statuses: ['PREFAILED', 'CONFIRMED', 'HEALED']}" ``` @@ -266,7 +266,7 @@ Use `include_muted` and `include_clusters` to retrieve correlation information: ```bash # Get faults with muted fault details -ros2 service call /fault_manager/get_faults ros2_medkit_msgs/srv/GetFaults \ +ros2 service call /fault_manager/list_faults ros2_medkit_msgs/srv/ListFaults \ "{statuses: ['CONFIRMED'], include_muted: true, include_clusters: true}" ``` diff --git a/src/ros2_medkit_fault_manager/design/architecture.puml b/src/ros2_medkit_fault_manager/design/architecture.puml index e91742e7..f74270c9 100644 --- a/src/ros2_medkit_fault_manager/design/architecture.puml +++ b/src/ros2_medkit_fault_manager/design/architecture.puml @@ -30,7 +30,7 @@ package "ros2_medkit_msgs" { +Response: success, message } - class "srv::GetFaults" { + class "srv::ListFaults" { +Request: filter_by_severity, severity, statuses +Response: faults[] } @@ -49,7 +49,7 @@ package "ros2_medkit_fault_manager" { abstract class FaultStorage <> { + {abstract} report_fault(): bool - + {abstract} get_faults(): vector + + {abstract} list_faults(): vector + {abstract} get_fault(): optional + {abstract} clear_fault(): bool + {abstract} size(): size_t @@ -58,7 +58,7 @@ package "ros2_medkit_fault_manager" { class InMemoryFaultStorage { + report_fault(): bool - + get_faults(): vector + + list_faults(): vector + get_fault(): optional + clear_fault(): bool + size(): size_t @@ -87,7 +87,7 @@ FaultState ..> "msg::Fault" : converts to ' Node uses service types FaultManagerNode ..> "srv::ReportFault" : handles -FaultManagerNode ..> "srv::GetFaults" : handles +FaultManagerNode ..> "srv::ListFaults" : handles FaultManagerNode ..> "srv::ClearFault" : handles @enduml diff --git a/src/ros2_medkit_fault_manager/design/index.rst b/src/ros2_medkit_fault_manager/design/index.rst index 808d6b24..a95ab6fc 100644 --- a/src/ros2_medkit_fault_manager/design/index.rst +++ b/src/ros2_medkit_fault_manager/design/index.rst @@ -43,7 +43,7 @@ The following diagram shows the relationships between the main components of the +Response: success, message } - class "srv::GetFaults" { + class "srv::ListFaults" { +Request: filter_by_severity, severity, statuses +Response: faults[] } @@ -62,7 +62,7 @@ The following diagram shows the relationships between the main components of the abstract class FaultStorage <> { + {abstract} report_fault(): bool - + {abstract} get_faults(): vector + + {abstract} list_faults(): vector + {abstract} get_fault(): optional + {abstract} clear_fault(): bool + {abstract} size(): size_t @@ -71,7 +71,7 @@ The following diagram shows the relationships between the main components of the class InMemoryFaultStorage { + report_fault(): bool - + get_faults(): vector + + list_faults(): vector + get_fault(): optional + clear_fault(): bool + size(): size_t @@ -100,7 +100,7 @@ The following diagram shows the relationships between the main components of the ' Node uses service types FaultManagerNode ..> "srv::ReportFault" : handles - FaultManagerNode ..> "srv::GetFaults" : handles + FaultManagerNode ..> "srv::ListFaults" : handles FaultManagerNode ..> "srv::ClearFault" : handles @enduml @@ -148,8 +148,8 @@ Reports a new fault or updates an existing one. - **Severity escalation**: Fault severity is updated if a higher severity is reported - **Returns**: ``accepted=true`` if event was processed -~/get_faults -~~~~~~~~~~~~ +~/list_faults +~~~~~~~~~~~~~ Queries faults with optional filtering. diff --git a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/correlation/correlation_engine.hpp b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/correlation/correlation_engine.hpp index dacad284..d976f114 100644 --- a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/correlation/correlation_engine.hpp +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/correlation/correlation_engine.hpp @@ -60,7 +60,7 @@ struct ProcessClearResult { std::vector auto_cleared_codes; }; -/// Information about a muted fault (for GetFaults response) +/// Information about a muted fault (for ListFaults response) struct MutedFaultData { std::string fault_code; std::string root_cause_code; @@ -68,7 +68,7 @@ struct MutedFaultData { uint32_t delay_ms{0}; }; -/// Information about an active cluster (for GetFaults response) +/// Information about an active cluster (for ListFaults response) struct ClusterData { std::string cluster_id; std::string rule_id; 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 3c3bfca1..267d2854 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,9 +24,12 @@ #include "ros2_medkit_fault_manager/snapshot_capture.hpp" #include "ros2_medkit_msgs/msg/fault_event.hpp" #include "ros2_medkit_msgs/srv/clear_fault.hpp" -#include "ros2_medkit_msgs/srv/get_faults.hpp" +#include "ros2_medkit_msgs/srv/get_fault.hpp" #include "ros2_medkit_msgs/srv/get_rosbag.hpp" #include "ros2_medkit_msgs/srv/get_snapshots.hpp" +#include "ros2_medkit_msgs/srv/list_faults.hpp" +#include "ros2_medkit_msgs/srv/list_faults_for_entity.hpp" +#include "ros2_medkit_msgs/srv/list_rosbags.hpp" #include "ros2_medkit_msgs/srv/report_fault.hpp" namespace ros2_medkit_fault_manager { @@ -54,6 +57,12 @@ class FaultManagerNode : public rclcpp::Node { return storage_type_; } + /// Check if entity matches any reporting source + /// @param reporting_sources List of reporting sources from fault + /// @param entity_id Entity ID to match (exact match or as suffix of FQN) + /// @return true if entity_id matches any source + static bool matches_entity(const std::vector & reporting_sources, const std::string & entity_id); + private: /// Create storage backend based on configuration std::unique_ptr create_storage(); @@ -62,9 +71,13 @@ class FaultManagerNode : public rclcpp::Node { void handle_report_fault(const std::shared_ptr & request, const std::shared_ptr & response); - /// Handle GetFaults service request - void handle_get_faults(const std::shared_ptr & request, - const std::shared_ptr & response); + /// Handle ListFaults service request + void handle_list_faults(const std::shared_ptr & request, + const std::shared_ptr & response); + + /// Handle GetFault service request (single fault with environment_data) + void handle_get_fault(const std::shared_ptr & request, + const std::shared_ptr & response); /// Handle ClearFault service request void handle_clear_fault(const std::shared_ptr & request, @@ -78,6 +91,15 @@ class FaultManagerNode : public rclcpp::Node { void handle_get_rosbag(const std::shared_ptr & request, const std::shared_ptr & response); + /// Handle ListRosbags batch service request + void handle_list_rosbags(const std::shared_ptr & request, + const std::shared_ptr & response); + + /// Handle ListFaultsForEntity service request + void + handle_list_faults_for_entity(const std::shared_ptr & request, + const std::shared_ptr & response); + /// Create snapshot configuration from parameters SnapshotConfig create_snapshot_config(); @@ -100,6 +122,9 @@ class FaultManagerNode : public rclcpp::Node { /// Validate severity value static bool is_valid_severity(uint8_t severity); + /// Extract topic name from full topic path (last segment) + static std::string extract_topic_name(const std::string & topic_path); + std::string storage_type_; std::string database_path_; int32_t confirmation_threshold_{-1}; @@ -109,10 +134,13 @@ class FaultManagerNode : public rclcpp::Node { std::unique_ptr storage_; rclcpp::Service::SharedPtr report_fault_srv_; - rclcpp::Service::SharedPtr get_faults_srv_; + rclcpp::Service::SharedPtr list_faults_srv_; + rclcpp::Service::SharedPtr get_fault_srv_; rclcpp::Service::SharedPtr clear_fault_srv_; rclcpp::Service::SharedPtr get_snapshots_srv_; rclcpp::Service::SharedPtr get_rosbag_srv_; + rclcpp::Service::SharedPtr list_rosbags_srv_; + rclcpp::Service::SharedPtr list_faults_for_entity_srv_; rclcpp::TimerBase::SharedPtr auto_confirm_timer_; /// Timer for periodic cleanup of expired correlation data @@ -121,11 +149,13 @@ class FaultManagerNode : public rclcpp::Node { /// Publisher for fault events (SSE streaming via gateway) rclcpp::Publisher::SharedPtr event_publisher_; - /// Snapshot capture for capturing topic data on fault confirmation - std::unique_ptr snapshot_capture_; + /// Snapshot capture for capturing topic data on fault confirmation. + /// shared_ptr to allow safe capture-by-value in detached capture threads. + std::shared_ptr snapshot_capture_; - /// Rosbag capture for time-window recording (nullptr if disabled) - std::unique_ptr rosbag_capture_; + /// Rosbag capture for time-window recording (nullptr if disabled). + /// shared_ptr to allow safe capture-by-value in detached capture threads. + std::shared_ptr rosbag_capture_; /// Correlation engine for fault correlation/muting (nullptr if disabled) std::unique_ptr correlation_engine_; diff --git a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_storage.hpp b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_storage.hpp index 37646f59..89a061b4 100644 --- a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_storage.hpp +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/fault_storage.hpp @@ -121,8 +121,8 @@ class FaultStorage { /// @param severity Severity level to filter (if filter_by_severity is true) /// @param statuses List of statuses to include (empty = CONFIRMED only) /// @return Vector of matching faults - virtual std::vector get_faults(bool filter_by_severity, uint8_t severity, - const std::vector & statuses) const = 0; + virtual std::vector list_faults(bool filter_by_severity, uint8_t severity, + const std::vector & statuses) const = 0; /// Get a single fault by fault_code /// @param fault_code The fault code to look up @@ -178,6 +178,15 @@ class FaultStorage { /// @return Vector of rosbag file info virtual std::vector get_all_rosbag_files() const = 0; + /// Get rosbags for all faults associated with an entity + /// @param entity_fqn The entity's fully qualified name to filter by + /// @return Vector of rosbag file info for faults reported by this entity + virtual std::vector list_rosbags_for_entity(const std::string & entity_fqn) const = 0; + + /// Get all stored faults regardless of status (for filtering) + /// @return Vector of all faults in storage + virtual std::vector get_all_faults() const = 0; + protected: FaultStorage() = default; FaultStorage(const FaultStorage &) = default; @@ -198,8 +207,8 @@ class InMemoryFaultStorage : public FaultStorage { const std::string & description, const std::string & source_id, const rclcpp::Time & timestamp) override; - std::vector get_faults(bool filter_by_severity, uint8_t severity, - const std::vector & statuses) const override; + std::vector list_faults(bool filter_by_severity, uint8_t severity, + const std::vector & statuses) const override; std::optional get_fault(const std::string & fault_code) const override; @@ -220,6 +229,8 @@ class InMemoryFaultStorage : public FaultStorage { bool delete_rosbag_file(const std::string & fault_code) override; size_t get_total_rosbag_storage_bytes() const override; std::vector get_all_rosbag_files() const override; + std::vector list_rosbags_for_entity(const std::string & entity_fqn) const override; + std::vector get_all_faults() const override; private: /// Update fault status based on debounce counter @@ -228,7 +239,7 @@ class InMemoryFaultStorage : public FaultStorage { mutable std::mutex mutex_; std::map faults_; std::vector snapshots_; - std::map rosbag_files_; + std::map rosbag_files_; ///< fault_code -> rosbag info DebounceConfig config_; }; diff --git a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/sqlite_fault_storage.hpp b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/sqlite_fault_storage.hpp index 9b467b84..4b62f029 100644 --- a/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/sqlite_fault_storage.hpp +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/sqlite_fault_storage.hpp @@ -48,8 +48,8 @@ class SqliteFaultStorage : public FaultStorage { const std::string & description, const std::string & source_id, const rclcpp::Time & timestamp) override; - std::vector get_faults(bool filter_by_severity, uint8_t severity, - const std::vector & statuses) const override; + std::vector list_faults(bool filter_by_severity, uint8_t severity, + const std::vector & statuses) const override; std::optional get_fault(const std::string & fault_code) const override; @@ -70,6 +70,8 @@ class SqliteFaultStorage : public FaultStorage { bool delete_rosbag_file(const std::string & fault_code) override; size_t get_total_rosbag_storage_bytes() const override; std::vector get_all_rosbag_files() const override; + std::vector list_rosbags_for_entity(const std::string & entity_fqn) const override; + std::vector get_all_faults() const override; /// Get the database path const std::string & db_path() const { 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 f215d8e4..e9c2620c 100644 --- a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp @@ -14,19 +14,23 @@ #include "ros2_medkit_fault_manager/fault_manager_node.hpp" +#include + #include #include #include -#include - #include -#include +#include +#include #include "ros2_medkit_fault_manager/correlation/config_parser.hpp" #include "ros2_medkit_fault_manager/sqlite_fault_storage.hpp" #include "ros2_medkit_fault_manager/time_utils.hpp" #include "ros2_medkit_msgs/msg/cluster_info.hpp" +#include "ros2_medkit_msgs/msg/environment_data.hpp" +#include "ros2_medkit_msgs/msg/extended_data_records.hpp" #include "ros2_medkit_msgs/msg/muted_fault_info.hpp" +#include "ros2_medkit_msgs/msg/snapshot.hpp" namespace ros2_medkit_fault_manager { @@ -118,10 +122,16 @@ FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node(" handle_report_fault(request, response); }); - get_faults_srv_ = create_service( - "~/get_faults", [this](const std::shared_ptr & request, - const std::shared_ptr & response) { - handle_get_faults(request, response); + list_faults_srv_ = create_service( + "~/list_faults", [this](const std::shared_ptr & request, + const std::shared_ptr & response) { + handle_list_faults(request, response); + }); + + get_fault_srv_ = create_service( + "~/get_fault", [this](const std::shared_ptr & request, + const std::shared_ptr & response) { + handle_get_fault(request, response); }); clear_fault_srv_ = create_service( @@ -142,15 +152,28 @@ FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node(" handle_get_rosbag(request, response); }); + list_rosbags_srv_ = create_service( + "~/list_rosbags", [this](const std::shared_ptr & request, + const std::shared_ptr & response) { + handle_list_rosbags(request, response); + }); + + list_faults_for_entity_srv_ = create_service( + "~/list_faults_for_entity", + [this](const std::shared_ptr & request, + const std::shared_ptr & response) { + handle_list_faults_for_entity(request, response); + }); + // Initialize snapshot capture auto snapshot_config = create_snapshot_config(); if (snapshot_config.enabled) { - snapshot_capture_ = std::make_unique(this, storage_.get(), snapshot_config); + snapshot_capture_ = std::make_shared(this, storage_.get(), snapshot_config); } // Initialize rosbag capture if enabled if (snapshot_config.rosbag.enabled) { - rosbag_capture_ = std::make_unique(this, storage_.get(), snapshot_config.rosbag, snapshot_config); + rosbag_capture_ = std::make_shared(this, storage_.get(), snapshot_config.rosbag, snapshot_config); } // Initialize correlation engine (nullptr if disabled or not configured) @@ -318,14 +341,24 @@ void FaultManagerNode::handle_report_fault( } // Note: PREFAILED/PREPASSED status changes don't emit events (debounce in progress) - // Capture snapshots when fault is confirmed (even if muted) - if (just_confirmed && snapshot_capture_) { - snapshot_capture_->capture(request->fault_code); - } - - // Trigger rosbag capture when fault is confirmed (even if muted) - if (just_confirmed && rosbag_capture_) { - rosbag_capture_->on_fault_confirmed(request->fault_code); + // Capture snapshots and rosbag when fault is confirmed (even if muted). + // Run asynchronously to avoid blocking the service callback for seconds, + // which would prevent other service calls (list_faults, get_fault, etc.) + // from being processed during capture. SnapshotCapture::capture_topic_on_demand + // uses a local callback group + local executor, so it's safe from a separate thread. + if (just_confirmed && (snapshot_capture_ || rosbag_capture_)) { + std::string fault_code = request->fault_code; + auto snapshot_cap = snapshot_capture_; + auto rosbag_cap = rosbag_capture_; + auto logger = get_logger(); + std::thread([snapshot_cap, rosbag_cap, fault_code, logger]() { + if (snapshot_cap) { + snapshot_cap->capture(fault_code); + } + if (rosbag_cap) { + rosbag_cap->on_fault_confirmed(fault_code); + } + }).detach(); } // Handle PREFAILED state for lazy_start rosbag capture @@ -351,9 +384,10 @@ void FaultManagerNode::handle_report_fault( } } -void FaultManagerNode::handle_get_faults(const std::shared_ptr & request, - const std::shared_ptr & response) { - response->faults = storage_->get_faults(request->filter_by_severity, request->severity, request->statuses); +void FaultManagerNode::handle_list_faults( + const std::shared_ptr & request, + const std::shared_ptr & response) { + response->faults = storage_->list_faults(request->filter_by_severity, request->severity, request->statuses); // Include correlation data if engine is enabled if (correlation_engine_) { @@ -400,7 +434,7 @@ void FaultManagerNode::handle_get_faults(const std::shared_ptrfaults.size(), + RCLCPP_DEBUG(get_logger(), "ListFaults returned %zu faults (muted=%u, clusters=%u)", response->faults.size(), response->muted_count, response->cluster_count); } @@ -467,6 +501,76 @@ bool FaultManagerNode::is_valid_severity(uint8_t severity) { return severity <= ros2_medkit_msgs::msg::Fault::SEVERITY_CRITICAL; } +std::string FaultManagerNode::extract_topic_name(const std::string & topic_path) { + auto pos = topic_path.rfind('/'); + if (pos != std::string::npos && pos < topic_path.length() - 1) { + return topic_path.substr(pos + 1); + } + return topic_path; +} + +void FaultManagerNode::handle_get_fault(const std::shared_ptr & request, + const std::shared_ptr & response) { + RCLCPP_DEBUG(get_logger(), "GetFault request for: %s", request->fault_code.c_str()); + + // Validate fault_code + std::string validation_error = validate_fault_code(request->fault_code); + if (!validation_error.empty()) { + response->success = false; + response->error_message = validation_error; + return; + } + + // Get fault from storage + auto fault = storage_->get_fault(request->fault_code); + if (!fault) { + response->success = false; + response->error_message = "Fault not found: " + request->fault_code; + return; + } + + response->success = true; + response->fault = *fault; + + // Populate extended_data_records with timestamps + ros2_medkit_msgs::msg::ExtendedDataRecords extended_records; + extended_records.first_occurrence_ns = rclcpp::Time(fault->first_occurred).nanoseconds(); + extended_records.last_occurrence_ns = rclcpp::Time(fault->last_occurred).nanoseconds(); + response->environment_data.extended_data_records = extended_records; + + // Get freeze frame snapshots from storage + auto stored_snapshots = storage_->get_snapshots(request->fault_code); + for (const auto & stored_snapshot : stored_snapshots) { + ros2_medkit_msgs::msg::Snapshot snapshot; + snapshot.type = ros2_medkit_msgs::msg::Snapshot::TYPE_FREEZE_FRAME; + snapshot.name = extract_topic_name(stored_snapshot.topic); + snapshot.data = stored_snapshot.data; + snapshot.topic = stored_snapshot.topic; + snapshot.message_type = stored_snapshot.message_type; + snapshot.captured_at_ns = stored_snapshot.captured_at_ns; + // Rosbag fields left empty for freeze_frame type + response->environment_data.snapshots.push_back(snapshot); + } + + // Get rosbag info if available + auto rosbag_info = storage_->get_rosbag_file(request->fault_code); + if (rosbag_info) { + ros2_medkit_msgs::msg::Snapshot rosbag_snapshot; + rosbag_snapshot.type = ros2_medkit_msgs::msg::Snapshot::TYPE_ROSBAG; + rosbag_snapshot.name = "rosbag_" + request->fault_code; + rosbag_snapshot.bulk_data_id = request->fault_code; + rosbag_snapshot.size_bytes = rosbag_info->size_bytes; + rosbag_snapshot.duration_sec = rosbag_info->duration_sec; + rosbag_snapshot.format = rosbag_info->format; + rosbag_snapshot.captured_at_ns = rosbag_info->created_at_ns; + // Freeze frame fields left empty for rosbag type + response->environment_data.snapshots.push_back(rosbag_snapshot); + } + + RCLCPP_DEBUG(get_logger(), "GetFault returned fault '%s' with %zu snapshots", request->fault_code.c_str(), + response->environment_data.snapshots.size()); +} + void FaultManagerNode::publish_fault_event(const std::string & event_type, const ros2_medkit_msgs::msg::Fault & fault, const std::vector & auto_cleared_codes) { ros2_medkit_msgs::msg::FaultEvent event; @@ -796,4 +900,82 @@ void FaultManagerNode::handle_get_rosbag(const std::shared_ptrfault_code.c_str()); } +void FaultManagerNode::handle_list_rosbags( + const std::shared_ptr & request, + const std::shared_ptr & response) { + RCLCPP_DEBUG(get_logger(), "ListRosbags request for entity: %s", request->entity_fqn.c_str()); + + if (request->entity_fqn.empty()) { + response->success = false; + response->error_message = "entity_fqn cannot be empty"; + return; + } + + // Use batch storage API to get all rosbags for this entity + auto rosbags = storage_->list_rosbags_for_entity(request->entity_fqn); + + for (const auto & info : rosbags) { + // Skip rosbags whose files no longer exist on disk + if (!std::filesystem::exists(info.file_path)) { + storage_->delete_rosbag_file(info.fault_code); + continue; + } + + response->fault_codes.push_back(info.fault_code); + response->file_paths.push_back(info.file_path); + response->formats.push_back(info.format); + response->durations_sec.push_back(info.duration_sec); + response->sizes_bytes.push_back(info.size_bytes); + } + + response->success = true; + RCLCPP_DEBUG(get_logger(), "ListRosbags returned %zu rosbags for entity '%s'", response->fault_codes.size(), + request->entity_fqn.c_str()); +} + +void FaultManagerNode::handle_list_faults_for_entity( + const std::shared_ptr & request, + const std::shared_ptr & response) { + RCLCPP_DEBUG(get_logger(), "ListFaultsForEntity request for entity: %s", request->entity_id.c_str()); + + // Validate entity_id is not empty + if (request->entity_id.empty()) { + response->success = false; + response->error_message = "entity_id cannot be empty"; + return; + } + + // Get all faults from storage + auto all_faults = storage_->get_all_faults(); + + // Filter faults that have this entity in their reporting_sources + for (const auto & fault : all_faults) { + if (matches_entity(fault.reporting_sources, request->entity_id)) { + response->faults.push_back(fault); + } + } + + response->success = true; + RCLCPP_DEBUG(get_logger(), "ListFaultsForEntity returned %zu faults for entity '%s'", response->faults.size(), + request->entity_id.c_str()); +} + +bool FaultManagerNode::matches_entity(const std::vector & reporting_sources, + const std::string & entity_id) { + for (const auto & source : reporting_sources) { + // Exact match + if (source == entity_id) { + return true; + } + + // FQN suffix match: source ends with "/" + entity_id + // e.g., source="/powertrain/motor_controller" matches entity_id="motor_controller" + std::string suffix = "/" + entity_id; + if (source.size() > suffix.size() && source.substr(source.size() - suffix.size()) == suffix) { + return true; + } + } + return false; +} + } // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/src/fault_storage.cpp b/src/ros2_medkit_fault_manager/src/fault_storage.cpp index dce066b0..e31049c2 100644 --- a/src/ros2_medkit_fault_manager/src/fault_storage.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_storage.cpp @@ -183,8 +183,8 @@ bool InMemoryFaultStorage::report_fault_event(const std::string & fault_code, ui } std::vector -InMemoryFaultStorage::get_faults(bool filter_by_severity, uint8_t severity, - const std::vector & statuses) const { +InMemoryFaultStorage::list_faults(bool filter_by_severity, uint8_t severity, + const std::vector & statuses) const { std::lock_guard lock(mutex_); // Determine which statuses to include @@ -315,10 +315,12 @@ void InMemoryFaultStorage::store_rosbag_file(const RosbagFileInfo & info) { // Delete existing bag file if present (prevent orphaned files on re-confirm) auto it = rosbag_files_.find(info.fault_code); - if (it != rosbag_files_.end() && it->second.file_path != info.file_path) { - std::error_code ec; - std::filesystem::remove_all(it->second.file_path, ec); - // Ignore errors - file may already be deleted + if (it != rosbag_files_.end()) { + if (it->second.file_path != info.file_path) { + std::error_code ec; + std::filesystem::remove_all(it->second.file_path, ec); + // Ignore errors - file may already be deleted + } } rosbag_files_[info.fault_code] = info; @@ -378,4 +380,36 @@ std::vector InMemoryFaultStorage::get_all_rosbag_files() const { return result; } +std::vector InMemoryFaultStorage::list_rosbags_for_entity(const std::string & entity_fqn) const { + std::lock_guard lock(mutex_); + + std::vector result; + + for (const auto & [fault_code, rosbag_info] : rosbag_files_) { + // Check if any of the fault's reporting sources contain this entity + auto fault_it = faults_.find(fault_code); + if (fault_it != faults_.end()) { + const auto & fault_state = fault_it->second; + if (fault_state.reporting_sources.find(entity_fqn) != fault_state.reporting_sources.end()) { + result.push_back(rosbag_info); + } + } + } + + return result; +} + +std::vector InMemoryFaultStorage::get_all_faults() const { + std::lock_guard lock(mutex_); + + std::vector result; + result.reserve(faults_.size()); + + for (const auto & [code, state] : faults_) { + result.push_back(state.to_msg()); + } + + return result; +} + } // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp b/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp index 43d32c69..aea33fd1 100644 --- a/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp +++ b/src/ros2_medkit_fault_manager/src/snapshot_capture.cpp @@ -14,12 +14,11 @@ #include "ros2_medkit_fault_manager/snapshot_capture.hpp" -#include -#include - #include #include #include +#include +#include #include "ros2_medkit_fault_manager/time_utils.hpp" #include "ros2_medkit_serialization/json_serializer.hpp" @@ -156,8 +155,10 @@ bool SnapshotCapture::capture_topic_on_demand(const std::string & fault_code, co } } - // Create a local callback group for this capture operation (ensures clean executor lifecycle) - auto local_callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // Create a local callback group for this capture operation (ensures clean executor lifecycle). + // Pass false to prevent automatic association with the node's main executor — + // we manually add this group to a local SingleThreadedExecutor below. + auto local_callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); rclcpp::GenericSubscription::SharedPtr subscription; try { diff --git a/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp b/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp index 250e96c0..2359ebd0 100644 --- a/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp +++ b/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp @@ -415,12 +415,13 @@ bool SqliteFaultStorage::report_fault_event(const std::string & fault_code, uint // Update with new values SqliteStatement update_stmt( - db_, - description.empty() - ? "UPDATE faults SET severity = ?, last_occurred_ns = ?, last_failed_ns = ?, occurrence_count = ?, " - "reporting_sources = ?, status = ?, debounce_counter = ? WHERE fault_code = ?" - : "UPDATE faults SET severity = ?, description = ?, last_occurred_ns = ?, last_failed_ns = ?, " - "occurrence_count = ?, reporting_sources = ?, status = ?, debounce_counter = ? WHERE fault_code = ?"); + db_, description.empty() ? "UPDATE faults SET severity = ?, last_occurred_ns = ?, last_failed_ns = ?, " + "occurrence_count = ?, " + "reporting_sources = ?, status = ?, debounce_counter = ? WHERE fault_code = ?" + : "UPDATE faults SET severity = ?, description = ?, last_occurred_ns = ?, " + "last_failed_ns = ?, " + "occurrence_count = ?, reporting_sources = ?, status = ?, debounce_counter = ? " + "WHERE fault_code = ?"); if (description.empty()) { update_stmt.bind_int(1, new_severity); @@ -463,7 +464,8 @@ bool SqliteFaultStorage::report_fault_event(const std::string & fault_code, uint SqliteStatement update_stmt( db_, - "UPDATE faults SET last_occurred_ns = ?, last_passed_ns = ?, status = ?, debounce_counter = ? WHERE " + "UPDATE faults SET last_occurred_ns = ?, last_passed_ns = ?, status = ?, debounce_counter " + "= ? WHERE " "fault_code = ?"); update_stmt.bind_int64(1, timestamp_ns); update_stmt.bind_int64(2, timestamp_ns); @@ -524,8 +526,8 @@ bool SqliteFaultStorage::report_fault_event(const std::string & fault_code, uint } std::vector -SqliteFaultStorage::get_faults(bool filter_by_severity, uint8_t severity, - const std::vector & statuses) const { +SqliteFaultStorage::list_faults(bool filter_by_severity, uint8_t severity, + const std::vector & statuses) const { std::lock_guard lock(mutex_); // Determine which statuses to include @@ -712,7 +714,9 @@ std::vector SqliteFaultStorage::get_snapshots(const std::string & std::vector result; - std::string sql = "SELECT fault_code, topic, message_type, data, captured_at_ns FROM snapshots WHERE fault_code = ?"; + std::string sql = + "SELECT fault_code, topic, message_type, data, captured_at_ns FROM snapshots WHERE fault_code " + "= ?"; if (!topic_filter.empty()) { sql += " AND topic = ?"; } @@ -859,4 +863,64 @@ std::vector SqliteFaultStorage::get_all_rosbag_files() const { return result; } +std::vector SqliteFaultStorage::list_rosbags_for_entity(const std::string & entity_fqn) const { + std::lock_guard lock(mutex_); + + std::vector result; + + // Join rosbag_files with faults table and filter by reporting_sources containing entity_fqn. + // Use json_each() for proper JSON array querying instead of LIKE, which treats + // '_' as a single-char wildcard and would produce false positives on ROS names. + SqliteStatement stmt(db_, + "SELECT r.fault_code, r.file_path, r.format, r.duration_sec, r.size_bytes, " + "r.created_at_ns " + "FROM rosbag_files r " + "JOIN faults f ON r.fault_code = f.fault_code " + "JOIN json_each(f.reporting_sources) j ON j.value = ?"); + + stmt.bind_text(1, entity_fqn); + + while (stmt.step() == SQLITE_ROW) { + RosbagFileInfo info; + info.fault_code = stmt.column_text(0); + info.file_path = stmt.column_text(1); + info.format = stmt.column_text(2); + info.duration_sec = sqlite3_column_double(stmt.get(), 3); + info.size_bytes = static_cast(stmt.column_int64(4)); + info.created_at_ns = stmt.column_int64(5); + result.push_back(info); + } + + return result; +} + +std::vector SqliteFaultStorage::get_all_faults() const { + std::lock_guard lock(mutex_); + + SqliteStatement stmt(db_, + "SELECT fault_code, severity, description, first_occurred_ns, last_occurred_ns, " + "occurrence_count, status, reporting_sources FROM faults"); + + std::vector result; + while (stmt.step() == SQLITE_ROW) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = stmt.column_text(0); + fault.severity = static_cast(stmt.column_int(1)); + fault.description = stmt.column_text(2); + + int64_t first_ns = stmt.column_int64(3); + int64_t last_ns = stmt.column_int64(4); + fault.first_occurred = rclcpp::Time(first_ns, RCL_SYSTEM_TIME); + fault.last_occurred = rclcpp::Time(last_ns, RCL_SYSTEM_TIME); + + fault.occurrence_count = static_cast(stmt.column_int64(5)); + fault.status = stmt.column_text(6); + fault.reporting_sources = parse_json_array(stmt.column_text(7)); + + result.push_back(fault); + } + + return result; +} + } // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp index a6932606..993adeb4 100644 --- a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp +++ b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -25,6 +26,8 @@ #include "ros2_medkit_msgs/msg/fault.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" +#include "ros2_medkit_msgs/srv/list_faults_for_entity.hpp" #include "ros2_medkit_msgs/srv/report_fault.hpp" using ros2_medkit_fault_manager::DebounceConfig; @@ -33,6 +36,8 @@ using ros2_medkit_fault_manager::InMemoryFaultStorage; using ros2_medkit_msgs::msg::Fault; using ros2_medkit_msgs::msg::FaultEvent; using ros2_medkit_msgs::srv::ClearFault; +using ros2_medkit_msgs::srv::GetFault; +using ros2_medkit_msgs::srv::ListFaultsForEntity; using ros2_medkit_msgs::srv::ReportFault; class FaultStorageTest : public ::testing::Test { @@ -84,7 +89,7 @@ TEST_F(FaultStorageTest, ReportExistingFaultEventUpdates) { EXPECT_EQ(fault->reporting_sources.size(), 2u); } -TEST_F(FaultStorageTest, GetFaultsDefaultReturnsConfirmedOnly) { +TEST_F(FaultStorageTest, ListFaultsDefaultReturnsConfirmedOnly) { rclcpp::Clock clock; auto timestamp = clock.now(); @@ -93,12 +98,12 @@ TEST_F(FaultStorageTest, GetFaultsDefaultReturnsConfirmedOnly) { timestamp); // Default query should return the CONFIRMED fault - auto faults = storage_.get_faults(false, 0, {}); + auto faults = storage_.list_faults(false, 0, {}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].status, Fault::STATUS_CONFIRMED); } -TEST_F(FaultStorageTest, GetFaultsWithPrefailedStatus) { +TEST_F(FaultStorageTest, ListFaultsWithPrefailedStatus) { rclcpp::Clock clock; auto timestamp = clock.now(); @@ -111,13 +116,13 @@ TEST_F(FaultStorageTest, GetFaultsWithPrefailedStatus) { timestamp); // Query with PREFAILED status - auto faults = storage_.get_faults(false, 0, {Fault::STATUS_PREFAILED}); + auto faults = storage_.list_faults(false, 0, {Fault::STATUS_PREFAILED}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].fault_code, "FAULT_1"); EXPECT_EQ(faults[0].status, Fault::STATUS_PREFAILED); } -TEST_F(FaultStorageTest, GetFaultsFilterBySeverity) { +TEST_F(FaultStorageTest, ListFaultsFilterBySeverity) { rclcpp::Clock clock; auto timestamp = clock.now(); @@ -128,7 +133,7 @@ TEST_F(FaultStorageTest, GetFaultsFilterBySeverity) { "/node1", timestamp); // Filter by ERROR severity (query CONFIRMED since that's the default status now) - auto faults = storage_.get_faults(true, Fault::SEVERITY_ERROR, {Fault::STATUS_CONFIRMED}); + auto faults = storage_.list_faults(true, Fault::SEVERITY_ERROR, {Fault::STATUS_CONFIRMED}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].fault_code, "FAULT_ERROR"); } @@ -162,7 +167,7 @@ TEST_F(FaultStorageTest, GetClearedFaults) { storage_.clear_fault("FAULT_1"); // Query cleared faults - auto faults = storage_.get_faults(false, 0, {Fault::STATUS_CLEARED}); + auto faults = storage_.list_faults(false, 0, {Fault::STATUS_CLEARED}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].status, Fault::STATUS_CLEARED); } @@ -176,7 +181,7 @@ TEST_F(FaultStorageTest, InvalidStatusDefaultsToConfirmed) { timestamp); // Query with invalid status - defaults to CONFIRMED, which now matches our fault - auto faults = storage_.get_faults(false, 0, {"INVALID_STATUS"}); + auto faults = storage_.list_faults(false, 0, {"INVALID_STATUS"}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].status, Fault::STATUS_CONFIRMED); } @@ -600,6 +605,27 @@ TEST_F(FaultStorageTest, HealedFaultCanRecurWithFailedEvents) { EXPECT_EQ(fault->status, Fault::STATUS_CONFIRMED); } +TEST_F(FaultStorageTest, GetAllFaultsReturnsAllFaults) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + // Add faults with different statuses + storage_.report_fault_event("FAULT_1", ReportFault::Request::EVENT_FAILED, Fault::SEVERITY_ERROR, "Test 1", "/node1", + timestamp); + storage_.report_fault_event("FAULT_2", ReportFault::Request::EVENT_FAILED, Fault::SEVERITY_WARN, "Test 2", "/node2", + timestamp); + storage_.report_fault_event("FAULT_3", ReportFault::Request::EVENT_FAILED, Fault::SEVERITY_INFO, "Test 3", "/node3", + timestamp); + + auto all_faults = storage_.get_all_faults(); + EXPECT_EQ(all_faults.size(), 3u); +} + +TEST_F(FaultStorageTest, GetAllFaultsReturnsEmptyForEmptyStorage) { + auto all_faults = storage_.get_all_faults(); + EXPECT_TRUE(all_faults.empty()); +} + // FaultManagerNode tests class FaultManagerNodeTest : public ::testing::Test { protected: @@ -704,16 +730,23 @@ class FaultEventPublishingTest : public ::testing::Test { // Create service clients report_fault_client_ = test_node_->create_client("/fault_manager/report_fault"); clear_fault_client_ = test_node_->create_client("/fault_manager/clear_fault"); + get_fault_client_ = test_node_->create_client("/fault_manager/get_fault"); + list_faults_for_entity_client_ = + test_node_->create_client("/fault_manager/list_faults_for_entity"); // Wait for services ASSERT_TRUE(report_fault_client_->wait_for_service(std::chrono::seconds(5))); ASSERT_TRUE(clear_fault_client_->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(get_fault_client_->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(list_faults_for_entity_client_->wait_for_service(std::chrono::seconds(5))); } void TearDown() override { event_subscription_.reset(); report_fault_client_.reset(); clear_fault_client_.reset(); + get_fault_client_.reset(); + list_faults_for_entity_client_.reset(); test_node_.reset(); fault_manager_.reset(); received_events_.clear(); @@ -756,11 +789,37 @@ class FaultEventPublishingTest : public ::testing::Test { return future.get()->success; } + std::optional call_get_fault(const std::string & fault_code) { + auto request = std::make_shared(); + request->fault_code = fault_code; + + auto future = get_fault_client_->async_send_request(request); + spin_for(std::chrono::milliseconds(100)); + if (future.wait_for(std::chrono::seconds(0)) != std::future_status::ready) { + return std::nullopt; + } + return *future.get(); + } + + std::optional call_list_faults_for_entity(const std::string & entity_id) { + auto request = std::make_shared(); + request->entity_id = entity_id; + + auto future = list_faults_for_entity_client_->async_send_request(request); + spin_for(std::chrono::milliseconds(100)); + if (future.wait_for(std::chrono::seconds(0)) != std::future_status::ready) { + return std::nullopt; + } + return *future.get(); + } + std::shared_ptr fault_manager_; std::shared_ptr test_node_; rclcpp::Subscription::SharedPtr event_subscription_; rclcpp::Client::SharedPtr report_fault_client_; rclcpp::Client::SharedPtr clear_fault_client_; + rclcpp::Client::SharedPtr get_fault_client_; + rclcpp::Client::SharedPtr list_faults_for_entity_client_; std::vector received_events_; }; @@ -887,6 +946,143 @@ TEST_F(FaultEventPublishingTest, TimestampUsesWallClockNotSimTime) { EXPECT_GT(event_ns, YEAR_2020_NS); } +// GetFault Service Tests +TEST_F(FaultEventPublishingTest, GetFaultReturnsExpectedFault) { + // Report a fault first + ASSERT_TRUE(call_report_fault("GET_FAULT_TEST", Fault::SEVERITY_ERROR, "/test_node")); + spin_for(std::chrono::milliseconds(100)); + + // Get fault via service + auto response = call_get_fault("GET_FAULT_TEST"); + ASSERT_TRUE(response.has_value()); + EXPECT_TRUE(response->success); + EXPECT_EQ(response->fault.fault_code, "GET_FAULT_TEST"); + EXPECT_EQ(response->fault.severity, Fault::SEVERITY_ERROR); + EXPECT_EQ(response->fault.status, Fault::STATUS_CONFIRMED); +} + +TEST_F(FaultEventPublishingTest, GetFaultReturnsNotFoundForMissingFault) { + // Get non-existent fault + auto response = call_get_fault("NON_EXISTENT_FAULT"); + ASSERT_TRUE(response.has_value()); + EXPECT_FALSE(response->success); + EXPECT_FALSE(response->error_message.empty()); +} + +TEST_F(FaultEventPublishingTest, GetFaultReturnsEnvironmentData) { + // Report a fault + ASSERT_TRUE(call_report_fault("ENV_DATA_TEST", Fault::SEVERITY_WARN, "/sensor/temp")); + spin_for(std::chrono::milliseconds(100)); + + auto response = call_get_fault("ENV_DATA_TEST"); + ASSERT_TRUE(response.has_value()); + EXPECT_TRUE(response->success); + + // Environment data should be present + const auto & env_data = response->environment_data; + + // Should have freeze-frame type snapshot for the first occurrence + // Note: The actual snapshot content depends on the storage implementation + // but we can verify the service returns environment_data correctly + EXPECT_TRUE(env_data.snapshots.empty() || !env_data.snapshots[0].type.empty()); +} + +TEST_F(FaultEventPublishingTest, GetFaultReturnsExtendedDataRecords) { + // Report fault twice to have first and last occurrence timestamps differ + ASSERT_TRUE(call_report_fault("EDR_TEST", Fault::SEVERITY_ERROR, "/node1")); + spin_for(std::chrono::milliseconds(100)); + ASSERT_TRUE(call_report_fault("EDR_TEST", Fault::SEVERITY_ERROR, "/node2")); + spin_for(std::chrono::milliseconds(100)); + + auto response = call_get_fault("EDR_TEST"); + ASSERT_TRUE(response.has_value()); + EXPECT_TRUE(response->success); + + // Verify extended data records contain timestamp information + const auto & edr = response->environment_data.extended_data_records; + EXPECT_NE(edr.first_occurrence_ns, 0); + EXPECT_NE(edr.last_occurrence_ns, 0); +} + +// @verifies REQ_INTEROP_071 +TEST_F(FaultEventPublishingTest, ListFaultsForEntitySuccess) { + // Report faults from different sources + ASSERT_TRUE(call_report_fault("MOTOR_FAULT", Fault::SEVERITY_ERROR, "/powertrain/motor_controller")); + ASSERT_TRUE(call_report_fault("SENSOR_FAULT", Fault::SEVERITY_WARN, "/powertrain/motor_controller")); + ASSERT_TRUE(call_report_fault("BRAKE_FAULT", Fault::SEVERITY_ERROR, "/chassis/brake_system")); + spin_for(std::chrono::milliseconds(100)); + + // Query faults for motor_controller entity + auto response = call_list_faults_for_entity("motor_controller"); + ASSERT_TRUE(response.has_value()); + EXPECT_TRUE(response->success); + EXPECT_EQ(response->faults.size(), 2u); + + // Verify the returned faults are from motor_controller + std::set codes; + for (const auto & fault : response->faults) { + codes.insert(fault.fault_code); + } + EXPECT_TRUE(codes.count("MOTOR_FAULT")); + EXPECT_TRUE(codes.count("SENSOR_FAULT")); + EXPECT_FALSE(codes.count("BRAKE_FAULT")); +} + +// @verifies REQ_INTEROP_071 +TEST_F(FaultEventPublishingTest, ListFaultsForEntityEmptyResult) { + // Report faults from a different entity + ASSERT_TRUE(call_report_fault("SOME_FAULT", Fault::SEVERITY_ERROR, "/some/other_entity")); + spin_for(std::chrono::milliseconds(100)); + + // Query faults for non-existent entity + auto response = call_list_faults_for_entity("motor_controller"); + ASSERT_TRUE(response.has_value()); + EXPECT_TRUE(response->success); + EXPECT_TRUE(response->faults.empty()); +} + +// @verifies REQ_INTEROP_071 +TEST_F(FaultEventPublishingTest, ListFaultsForEntityWithEmptyId) { + auto response = call_list_faults_for_entity(""); + ASSERT_TRUE(response.has_value()); + EXPECT_FALSE(response->success); + EXPECT_FALSE(response->error_message.empty()); +} + +// matches_entity helper tests +TEST(MatchesEntityTest, ExactMatch) { + std::vector sources = {"motor_controller"}; + EXPECT_TRUE(FaultManagerNode::matches_entity(sources, "motor_controller")); + EXPECT_FALSE(FaultManagerNode::matches_entity(sources, "other")); +} + +TEST(MatchesEntityTest, FQNSuffixMatch) { + std::vector sources = {"/powertrain/motor_controller"}; + EXPECT_TRUE(FaultManagerNode::matches_entity(sources, "motor_controller")); + EXPECT_FALSE(FaultManagerNode::matches_entity(sources, "powertrain")); + EXPECT_FALSE(FaultManagerNode::matches_entity(sources, "controller")); // Partial - not a suffix +} + +TEST(MatchesEntityTest, HierarchicalMatch) { + std::vector sources = {"/perception/lidar/front_sensor"}; + EXPECT_TRUE(FaultManagerNode::matches_entity(sources, "front_sensor")); + EXPECT_FALSE(FaultManagerNode::matches_entity(sources, "lidar")); + EXPECT_FALSE(FaultManagerNode::matches_entity(sources, "perception")); // Root only +} + +TEST(MatchesEntityTest, MultipleSources) { + std::vector sources = {"/chassis/brake_system", "/powertrain/motor_controller", "/sensor/temperature"}; + EXPECT_TRUE(FaultManagerNode::matches_entity(sources, "brake_system")); + EXPECT_TRUE(FaultManagerNode::matches_entity(sources, "motor_controller")); + EXPECT_TRUE(FaultManagerNode::matches_entity(sources, "temperature")); + EXPECT_FALSE(FaultManagerNode::matches_entity(sources, "unknown")); +} + +TEST(MatchesEntityTest, EmptySources) { + std::vector sources; + EXPECT_FALSE(FaultManagerNode::matches_entity(sources, "motor_controller")); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); diff --git a/src/ros2_medkit_fault_manager/test/test_integration.test.py b/src/ros2_medkit_fault_manager/test/test_integration.test.py index cad919f5..f2d79421 100644 --- a/src/ros2_medkit_fault_manager/test/test_integration.test.py +++ b/src/ros2_medkit_fault_manager/test/test_integration.test.py @@ -29,7 +29,7 @@ import rclpy from rclpy.node import Node from ros2_medkit_msgs.msg import Fault -from ros2_medkit_msgs.srv import ClearFault, GetFaults, GetSnapshots, ReportFault +from ros2_medkit_msgs.srv import ClearFault, GetSnapshots, ListFaults, ReportFault from sensor_msgs.msg import Temperature @@ -118,8 +118,8 @@ def setUpClass(cls): cls.report_fault_client = cls.node.create_client( ReportFault, '/fault_manager/report_fault' ) - cls.get_faults_client = cls.node.create_client( - GetFaults, '/fault_manager/get_faults' + cls.list_faults_client = cls.node.create_client( + ListFaults, '/fault_manager/list_faults' ) cls.clear_fault_client = cls.node.create_client( ClearFault, '/fault_manager/clear_fault' @@ -136,8 +136,8 @@ def setUpClass(cls): # Wait for services to be available assert cls.report_fault_client.wait_for_service(timeout_sec=10.0), \ 'report_fault service not available' - assert cls.get_faults_client.wait_for_service(timeout_sec=10.0), \ - 'get_faults service not available' + assert cls.list_faults_client.wait_for_service(timeout_sec=10.0), \ + 'list_faults service not available' assert cls.clear_fault_client.wait_for_service(timeout_sec=10.0), \ 'clear_fault service not available' assert cls.get_snapshots_client.wait_for_service(timeout_sec=10.0), \ @@ -212,7 +212,7 @@ def test_04_report_fault_invalid_severity_fails(self): self.assertFalse(response.accepted) print('Invalid severity rejected as expected') - def test_05_get_faults_prefailed(self): + def test_05_list_faults_prefailed(self): """Test getting faults with PREFAILED status.""" # First report a fault report_request = ReportFault.Request() @@ -224,19 +224,19 @@ def test_05_get_faults_prefailed(self): self._call_service(self.report_fault_client, report_request) # Query PREFAILED faults - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_PREFAILED] - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) self.assertGreater(len(response.faults), 0) fault_codes = [f.fault_code for f in response.faults] self.assertIn('TEST_FAULT_GET_001', fault_codes) print(f'Get faults returned {len(response.faults)} faults') - def test_06_get_faults_filter_by_severity(self): + def test_06_list_faults_filter_by_severity(self): """Test filtering faults by severity.""" # Report faults with different severities for i, severity in enumerate([Fault.SEVERITY_INFO, Fault.SEVERITY_ERROR]): @@ -249,12 +249,12 @@ def test_06_get_faults_filter_by_severity(self): self._call_service(self.report_fault_client, request) # Query ERROR severity only - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = True get_request.severity = Fault.SEVERITY_ERROR get_request.statuses = [Fault.STATUS_PREFAILED] - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) for fault in response.faults: self.assertEqual(fault.severity, Fault.SEVERITY_ERROR) @@ -282,12 +282,12 @@ def test_07_clear_fault_success(self): print(f'Clear fault response: {response.message}') # Verify fault is now CLEARED - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_CLEARED] - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) cleared_codes = [f.fault_code for f in get_response.faults] self.assertIn('TEST_FAULT_CLEAR', cleared_codes) @@ -341,12 +341,12 @@ def test_10_report_updates_existing_fault(self): self.assertTrue(response2.accepted) # Verify fault was updated - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_PREFAILED] - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) updated_fault = None for f in get_response.faults: @@ -376,12 +376,12 @@ def test_11_confirmation_workflow(self): self._call_service(self.report_fault_client, request) # Query CONFIRMED faults (should include our fault now) - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_CONFIRMED] - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) confirmed_codes = [f.fault_code for f in response.faults] self.assertIn(fault_code, confirmed_codes) @@ -407,12 +407,12 @@ def test_12_prefailed_excluded_from_default_query(self): self._call_service(self.report_fault_client, request) # Query with empty statuses (default = CONFIRMED only) - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [] # Empty = CONFIRMED only - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) # Fault should NOT be in results (it's PREFAILED) fault_codes = [f.fault_code for f in response.faults] @@ -420,12 +420,12 @@ def test_12_prefailed_excluded_from_default_query(self): print('Default query excluded PREFAILED fault as expected') # But it should be visible when querying PREFAILED explicitly - get_prefailed = GetFaults.Request() + get_prefailed = ListFaults.Request() get_prefailed.filter_by_severity = False get_prefailed.severity = 0 get_prefailed.statuses = [Fault.STATUS_PREFAILED] - prefailed_response = self._call_service(self.get_faults_client, get_prefailed) + prefailed_response = self._call_service(self.list_faults_client, get_prefailed) prefailed_codes = [f.fault_code for f in prefailed_response.faults] self.assertIn(fault_code, prefailed_codes) print('PREFAILED query found the fault as expected') @@ -446,12 +446,12 @@ def test_13_multi_source_confirmation(self): self._call_service(self.report_fault_client, request) # Query and verify - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_CONFIRMED] - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) fault = next((f for f in response.faults if f.fault_code == fault_code), None) self.assertIsNotNone(fault) @@ -474,12 +474,12 @@ def test_14_critical_severity_immediate_confirmation(self): self._call_service(self.report_fault_client, request) # Query CONFIRMED faults - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_CONFIRMED] - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) fault = next((f for f in response.faults if f.fault_code == fault_code), None) self.assertIsNotNone(fault) @@ -502,12 +502,12 @@ def test_15_passed_event_increments_counter(self): self._call_service(self.report_fault_client, request) # Verify fault is PREFAILED - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_PREFAILED] - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) fault = next((f for f in response.faults if f.fault_code == fault_code), None) self.assertIsNotNone(fault) self.assertEqual(fault.status, Fault.STATUS_PREFAILED) @@ -523,12 +523,12 @@ def test_15_passed_event_increments_counter(self): self._call_service(self.report_fault_client, request) # Verify fault is PREPASSED (counter > 0) - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.severity = 0 get_request.statuses = [Fault.STATUS_PREPASSED] - response = self._call_service(self.get_faults_client, get_request) + response = self._call_service(self.list_faults_client, get_request) fault = next((f for f in response.faults if f.fault_code == fault_code), None) self.assertIsNotNone(fault) self.assertEqual(fault.status, Fault.STATUS_PREPASSED) @@ -747,11 +747,11 @@ def test_21_correlation_hierarchical_root_cause(self): self.assertTrue(response.accepted) # Verify fault is confirmed - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.statuses = [Fault.STATUS_CONFIRMED] - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) fault_codes = [f.fault_code for f in get_response.faults] self.assertIn('ESTOP_001', fault_codes) @@ -787,12 +787,12 @@ def test_22_correlation_symptoms_muted(self): self.assertTrue(response.accepted) # Query with include_muted=true to see muted faults - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.statuses = [Fault.STATUS_CONFIRMED] get_request.include_muted = True - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) # Check muted_count > 0 self.assertGreater(get_response.muted_count, 0) @@ -833,12 +833,12 @@ def test_23_correlation_auto_clear_with_root(self): self.assertGreater(len(auto_cleared), 0) # Verify muted count is now 0 - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.statuses = [Fault.STATUS_CONFIRMED] get_request.include_muted = True - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) # Muted faults related to ESTOP_001 should be cleared remaining_muted = [m for m in get_response.muted_faults @@ -867,12 +867,12 @@ def test_24_correlation_auto_cluster(self): time.sleep(0.1) # Small delay to stay within window # Query with include_clusters=true - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.statuses = [Fault.STATUS_CONFIRMED] get_request.include_clusters = True - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) # Check cluster_count > 0 self.assertGreater(get_response.cluster_count, 0) @@ -895,13 +895,13 @@ def test_25_correlation_include_muted_false_excludes_details(self): The muted_count should still be returned, but muted_faults array should be empty when include_muted=false (default). """ - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.filter_by_severity = False get_request.statuses = [Fault.STATUS_CONFIRMED] get_request.include_muted = False # Default get_request.include_clusters = False # Default - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) # muted_count is always returned self.assertIsNotNone(get_response.muted_count) @@ -945,9 +945,9 @@ def test_26_cleared_fault_can_be_reactivated(self): self.assertTrue(response.accepted) # Verify fault is CONFIRMED again - get_request = GetFaults.Request() + get_request = ListFaults.Request() get_request.statuses = [Fault.STATUS_CONFIRMED] - get_response = self._call_service(self.get_faults_client, get_request) + get_response = self._call_service(self.list_faults_client, get_request) fault = next((f for f in get_response.faults if f.fault_code == fault_code), None) self.assertIsNotNone(fault) diff --git a/src/ros2_medkit_fault_manager/test/test_sqlite_storage.cpp b/src/ros2_medkit_fault_manager/test/test_sqlite_storage.cpp index 2e93a604..c0900de8 100644 --- a/src/ros2_medkit_fault_manager/test/test_sqlite_storage.cpp +++ b/src/ros2_medkit_fault_manager/test/test_sqlite_storage.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "ros2_medkit_fault_manager/sqlite_fault_storage.hpp" @@ -98,7 +99,7 @@ TEST_F(SqliteFaultStorageTest, ReportExistingFaultEventUpdates) { EXPECT_EQ(fault->reporting_sources.size(), 2u); } -TEST_F(SqliteFaultStorageTest, GetFaultsDefaultReturnsConfirmedOnly) { +TEST_F(SqliteFaultStorageTest, ListFaultsDefaultReturnsConfirmedOnly) { rclcpp::Clock clock; auto timestamp = clock.now(); @@ -107,12 +108,12 @@ TEST_F(SqliteFaultStorageTest, GetFaultsDefaultReturnsConfirmedOnly) { timestamp); // Default query should return the CONFIRMED fault - auto faults = storage_->get_faults(false, 0, {}); + auto faults = storage_->list_faults(false, 0, {}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].status, Fault::STATUS_CONFIRMED); } -TEST_F(SqliteFaultStorageTest, GetFaultsWithPrefailedStatus) { +TEST_F(SqliteFaultStorageTest, ListFaultsWithPrefailedStatus) { rclcpp::Clock clock; auto timestamp = clock.now(); @@ -125,13 +126,13 @@ TEST_F(SqliteFaultStorageTest, GetFaultsWithPrefailedStatus) { timestamp); // Query with PREFAILED status - auto faults = storage_->get_faults(false, 0, {Fault::STATUS_PREFAILED}); + auto faults = storage_->list_faults(false, 0, {Fault::STATUS_PREFAILED}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].fault_code, "FAULT_1"); EXPECT_EQ(faults[0].status, Fault::STATUS_PREFAILED); } -TEST_F(SqliteFaultStorageTest, GetFaultsFilterBySeverity) { +TEST_F(SqliteFaultStorageTest, ListFaultsFilterBySeverity) { rclcpp::Clock clock; auto timestamp = clock.now(); @@ -142,7 +143,7 @@ TEST_F(SqliteFaultStorageTest, GetFaultsFilterBySeverity) { "/node1", timestamp); // Filter by ERROR severity (query CONFIRMED since that's the default status now) - auto faults = storage_->get_faults(true, Fault::SEVERITY_ERROR, {Fault::STATUS_CONFIRMED}); + auto faults = storage_->list_faults(true, Fault::SEVERITY_ERROR, {Fault::STATUS_CONFIRMED}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].fault_code, "FAULT_ERROR"); } @@ -176,7 +177,7 @@ TEST_F(SqliteFaultStorageTest, GetClearedFaults) { storage_->clear_fault("FAULT_1"); // Query cleared faults - auto faults = storage_->get_faults(false, 0, {Fault::STATUS_CLEARED}); + auto faults = storage_->list_faults(false, 0, {Fault::STATUS_CLEARED}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].status, Fault::STATUS_CLEARED); } @@ -190,7 +191,7 @@ TEST_F(SqliteFaultStorageTest, InvalidStatusDefaultsToConfirmed) { timestamp); // Query with invalid status - defaults to CONFIRMED, which now matches our fault - auto faults = storage_->get_faults(false, 0, {"INVALID_STATUS"}); + auto faults = storage_->list_faults(false, 0, {"INVALID_STATUS"}); EXPECT_EQ(faults.size(), 1u); EXPECT_EQ(faults[0].status, Fault::STATUS_CONFIRMED); } @@ -708,6 +709,85 @@ TEST_F(SqliteFaultStorageTest, ClearFaultDeletesAssociatedSnapshots) { EXPECT_TRUE(snapshots_after.empty()); } +// Rosbag entity-scoped listing tests + +// @verifies REQ_INTEROP_071 +TEST_F(SqliteFaultStorageTest, ListRosbagsForEntityFiltersCorrectly) { + using ros2_medkit_fault_manager::RosbagFileInfo; + rclcpp::Clock clock; + + // Create fault with reporting source for entity + storage_->report_fault_event("ENTITY_FAULT_1", ReportFault::Request::EVENT_FAILED, Fault::SEVERITY_ERROR, + "Fault from entity", "/powertrain/motor", clock.now()); + + // Create another fault with different reporting source + storage_->report_fault_event("ENTITY_FAULT_2", ReportFault::Request::EVENT_FAILED, Fault::SEVERITY_WARN, + "Fault from other entity", "/chassis/brake", clock.now()); + + // Store rosbags for both faults + RosbagFileInfo info1; + info1.fault_code = "ENTITY_FAULT_1"; + info1.file_path = "/tmp/entity1.mcap"; + info1.format = "mcap"; + info1.duration_sec = 5.0; + info1.size_bytes = 1024; + info1.created_at_ns = clock.now().nanoseconds(); + storage_->store_rosbag_file(info1); + + RosbagFileInfo info2; + info2.fault_code = "ENTITY_FAULT_2"; + info2.file_path = "/tmp/entity2.mcap"; + info2.format = "mcap"; + info2.duration_sec = 3.0; + info2.size_bytes = 512; + info2.created_at_ns = clock.now().nanoseconds(); + storage_->store_rosbag_file(info2); + + // Get rosbags for motor entity + auto rosbags = storage_->list_rosbags_for_entity("/powertrain/motor"); + ASSERT_EQ(rosbags.size(), 1u); + EXPECT_EQ(rosbags[0].fault_code, "ENTITY_FAULT_1"); + + // Get rosbags for brake entity + auto brake_rosbags = storage_->list_rosbags_for_entity("/chassis/brake"); + ASSERT_EQ(brake_rosbags.size(), 1u); + EXPECT_EQ(brake_rosbags[0].fault_code, "ENTITY_FAULT_2"); + + // Get rosbags for unknown entity + auto unknown_rosbags = storage_->list_rosbags_for_entity("/unknown/entity"); + EXPECT_TRUE(unknown_rosbags.empty()); +} + +// @verifies REQ_INTEROP_073 +TEST_F(SqliteFaultStorageTest, GetAllRosbagFilesReturnsSortedByCreatedAt) { + using ros2_medkit_fault_manager::RosbagFileInfo; + + RosbagFileInfo info1; + info1.fault_code = "FAULT_A"; + info1.file_path = "/tmp/a.mcap"; + info1.format = "mcap"; + info1.duration_sec = 1.0; + info1.size_bytes = 100; + info1.created_at_ns = 1000; + storage_->store_rosbag_file(info1); + + RosbagFileInfo info2; + info2.fault_code = "FAULT_B"; + info2.file_path = "/tmp/b.mcap"; + info2.format = "mcap"; + info2.duration_sec = 2.0; + info2.size_bytes = 200; + info2.created_at_ns = 2000; + storage_->store_rosbag_file(info2); + + auto all_rosbags = storage_->get_all_rosbag_files(); + ASSERT_EQ(all_rosbags.size(), 2u); + + // Should be sorted by created_at_ns (oldest first) + EXPECT_EQ(all_rosbags[0].fault_code, "FAULT_A"); + EXPECT_EQ(all_rosbags[1].fault_code, "FAULT_B"); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); diff --git a/src/ros2_medkit_fault_reporter/test/test_integration.test.py b/src/ros2_medkit_fault_reporter/test/test_integration.test.py index b2a2ab45..76f15b4f 100644 --- a/src/ros2_medkit_fault_reporter/test/test_integration.test.py +++ b/src/ros2_medkit_fault_reporter/test/test_integration.test.py @@ -24,7 +24,7 @@ import rclpy from rclpy.node import Node from ros2_medkit_msgs.msg import Fault -from ros2_medkit_msgs.srv import GetFaults, ReportFault +from ros2_medkit_msgs.srv import ListFaults, ReportFault def get_coverage_env(): @@ -98,15 +98,15 @@ def setUpClass(cls): cls.report_fault_client = cls.node.create_client( ReportFault, '/fault_manager/report_fault' ) - cls.get_faults_client = cls.node.create_client( - GetFaults, '/fault_manager/get_faults' + cls.list_faults_client = cls.node.create_client( + ListFaults, '/fault_manager/list_faults' ) # Wait for services to be available assert cls.report_fault_client.wait_for_service(timeout_sec=10.0), \ 'report_fault service not available' - assert cls.get_faults_client.wait_for_service(timeout_sec=10.0), \ - 'get_faults service not available' + assert cls.list_faults_client.wait_for_service(timeout_sec=10.0), \ + 'list_faults service not available' @classmethod def tearDownClass(cls): @@ -132,18 +132,18 @@ def _report_fault(self, fault_code, severity, description, source_id, request.source_id = source_id return self._call_service(self.report_fault_client, request) - def _get_faults(self, statuses=None): + def _list_faults(self, statuses=None): """Get faults with given statuses via service call.""" - request = GetFaults.Request() + request = ListFaults.Request() request.filter_by_severity = False request.severity = 0 request.statuses = statuses or ['PREFAILED', 'CONFIRMED'] - return self._call_service(self.get_faults_client, request) + return self._call_service(self.list_faults_client, request) def test_01_service_connectivity(self): """Test that FaultManager services are available.""" self.assertTrue(self.report_fault_client.service_is_ready()) - self.assertTrue(self.get_faults_client.service_is_ready()) + self.assertTrue(self.list_faults_client.service_is_ready()) def test_02_report_fault_is_stored(self): """Test that reported faults are stored in FaultManager.""" @@ -158,7 +158,7 @@ def test_02_report_fault_is_stored(self): self.assertTrue(response.accepted) # Verify fault is in storage with CONFIRMED status - faults_response = self._get_faults(statuses=['CONFIRMED']) + faults_response = self._list_faults(statuses=['CONFIRMED']) fault_codes = [f.fault_code for f in faults_response.faults] self.assertIn('REPORTER_TEST_001', fault_codes) @@ -189,7 +189,7 @@ def test_03_multiple_reports_aggregate(self): ) # Verify aggregation - faults_response = self._get_faults(statuses=['CONFIRMED']) + faults_response = self._list_faults(statuses=['CONFIRMED']) fault = next( (f for f in faults_response.faults if f.fault_code == 'REPORTER_TEST_002'), None @@ -211,7 +211,7 @@ def test_04_source_id_tracked(self): source_id='/perception/lidar/scanner_node' ) - faults_response = self._get_faults(statuses=['CONFIRMED']) + faults_response = self._list_faults(statuses=['CONFIRMED']) fault = next( (f for f in faults_response.faults if f.fault_code == 'REPORTER_TEST_003'), None diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index c08474e8..b7337b27 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -142,10 +142,12 @@ add_library(gateway_lib STATIC src/http/handlers/operation_handlers.cpp src/http/handlers/config_handlers.cpp src/http/handlers/fault_handlers.cpp + src/http/handlers/bulkdata_handlers.cpp src/http/handlers/sse_fault_handler.cpp src/http/handlers/auth_handlers.cpp # HTTP utilities src/http/x_medkit.cpp + src/http/entity_path_utils.cpp # Auth module (subfolder) src/auth/auth_config.cpp src/auth/auth_models.cpp @@ -229,13 +231,13 @@ if(BUILD_TESTING) ) ament_lint_auto_find_test_dependencies() - # Configure clang-tidy manually with increased timeout (600s instead of default 300s) + # Configure clang-tidy manually with increased timeout (1500s instead of default 300s) # This is needed because the project has many files and clang-tidy analysis takes time ament_clang_tidy( "${CMAKE_CURRENT_BINARY_DIR}" CONFIG_FILE "${ament_cmake_clang_tidy_CONFIG_FILE}" HEADER_FILTER "${ament_cmake_clang_tidy_HEADER_FILTER}" - TIMEOUT 1200 + TIMEOUT 1500 ) # Add GTest @@ -318,6 +320,18 @@ if(BUILD_TESTING) ament_add_gtest(test_entity_resource_model test/test_entity_resource_model.cpp) target_link_libraries(test_entity_resource_model gateway_lib) + # Add entity path utils tests + ament_add_gtest(test_entity_path_utils test/test_entity_path_utils.cpp) + target_link_libraries(test_entity_path_utils gateway_lib) + + # Add fault handlers tests (SOVD response building) + ament_add_gtest(test_fault_handlers test/test_fault_handlers.cpp) + target_link_libraries(test_fault_handlers gateway_lib) + + # Add bulk data handlers tests + ament_add_gtest(test_bulkdata_handlers test/test_bulkdata_handlers.cpp) + target_link_libraries(test_bulkdata_handlers gateway_lib) + # Apply coverage flags to test targets if(ENABLE_COVERAGE) target_compile_options(test_gateway_node PRIVATE --coverage -O0 -g) @@ -350,6 +364,12 @@ if(BUILD_TESTING) target_link_options(test_auth_config PRIVATE --coverage) target_compile_options(test_data_access_manager PRIVATE --coverage -O0 -g) target_link_options(test_data_access_manager PRIVATE --coverage) + target_compile_options(test_entity_path_utils PRIVATE --coverage -O0 -g) + target_link_options(test_entity_path_utils PRIVATE --coverage) + target_compile_options(test_fault_handlers PRIVATE --coverage -O0 -g) + target_link_options(test_fault_handlers PRIVATE --coverage) + target_compile_options(test_bulkdata_handlers PRIVATE --coverage -O0 -g) + target_link_options(test_bulkdata_handlers PRIVATE --coverage) endif() # Integration testing @@ -365,7 +385,7 @@ if(BUILD_TESTING) add_launch_test( test/test_integration.test.py TARGET test_integration - TIMEOUT 120 + TIMEOUT 600 ) # Add CORS integration tests diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp index 03c454bd..28a816d0 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp @@ -21,11 +21,14 @@ #include #include +#include "ros2_medkit_msgs/msg/environment_data.hpp" #include "ros2_medkit_msgs/msg/fault.hpp" #include "ros2_medkit_msgs/srv/clear_fault.hpp" -#include "ros2_medkit_msgs/srv/get_faults.hpp" +#include "ros2_medkit_msgs/srv/get_fault.hpp" #include "ros2_medkit_msgs/srv/get_rosbag.hpp" #include "ros2_medkit_msgs/srv/get_snapshots.hpp" +#include "ros2_medkit_msgs/srv/list_faults.hpp" +#include "ros2_medkit_msgs/srv/list_rosbags.hpp" #include "ros2_medkit_msgs/srv/report_fault.hpp" namespace ros2_medkit_gateway { @@ -39,6 +42,14 @@ struct FaultResult { std::string error_message; }; +/// Result of get_fault operation with full message types +struct FaultWithEnvResult { + bool success; + std::string error_message; + ros2_medkit_msgs::msg::Fault fault; + ros2_medkit_msgs::msg::EnvironmentData environment_data; +}; + /// Manager for fault management operations /// Provides interface to the ros2_medkit_fault_manager services class FaultManager { @@ -62,14 +73,22 @@ class FaultManager { /// @param include_muted Include muted faults (correlation symptoms) in response /// @param include_clusters Include cluster info in response /// @return FaultResult with array of faults (and optionally muted_faults and clusters) - FaultResult get_faults(const std::string & source_id = "", bool include_prefailed = true, - bool include_confirmed = true, bool include_cleared = false, bool include_muted = false, - bool include_clusters = false); + FaultResult list_faults(const std::string & source_id = "", bool include_prefailed = true, + bool include_confirmed = true, bool include_cleared = false, bool include_muted = false, + bool include_clusters = false); - /// Get a specific fault by code + /// Get a specific fault by code with environment data + /// @param fault_code Fault identifier + /// @param source_id Optional component identifier to verify fault belongs to component + /// @return FaultWithEnvResult with fault and environment_data, or error if not found + FaultWithEnvResult get_fault_with_env(const std::string & fault_code, const std::string & source_id = ""); + + /// Get a specific fault by code (JSON result - legacy) /// @param fault_code Fault identifier /// @param source_id Optional component identifier to verify fault belongs to component /// @return FaultResult with fault data or error if not found + /// @note Thread-safe: delegates to get_fault_with_env() which acquires get_mutex_. + /// Do NOT call this method while holding get_mutex_. FaultResult get_fault(const std::string & fault_code, const std::string & source_id = ""); /// Clear a fault @@ -88,6 +107,11 @@ class FaultManager { /// @return FaultResult with rosbag file path and metadata FaultResult get_rosbag(const std::string & fault_code); + /// Get all rosbag files for an entity (batch operation) + /// @param entity_fqn Entity fully qualified name for prefix matching + /// @return FaultResult with arrays of rosbag metadata + FaultResult list_rosbags(const std::string & entity_fqn); + /// Check if fault manager service is available /// @return true if services are available bool is_available() const; @@ -103,16 +127,26 @@ class FaultManager { /// Service clients rclcpp::Client::SharedPtr report_fault_client_; - rclcpp::Client::SharedPtr get_faults_client_; + rclcpp::Client::SharedPtr get_fault_client_; + rclcpp::Client::SharedPtr list_faults_client_; rclcpp::Client::SharedPtr clear_fault_client_; rclcpp::Client::SharedPtr get_snapshots_client_; rclcpp::Client::SharedPtr get_rosbag_client_; + rclcpp::Client::SharedPtr list_rosbags_client_; /// Service timeout double service_timeout_sec_{5.0}; - /// Mutex for thread-safe service calls - mutable std::mutex service_mutex_; + /// Per-client mutexes for thread-safe service calls. + /// Split by service client so that read operations (list, get) are not blocked + /// by slow write operations (report_fault with snapshot capture). + mutable std::mutex report_mutex_; + mutable std::mutex list_mutex_; + mutable std::mutex get_mutex_; + mutable std::mutex clear_mutex_; + mutable std::mutex snapshots_mutex_; + mutable std::mutex rosbag_mutex_; + mutable std::mutex list_rosbags_mutex_; }; } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/entity_path_utils.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/entity_path_utils.hpp new file mode 100644 index 00000000..a44a0465 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/entity_path_utils.hpp @@ -0,0 +1,78 @@ +// Copyright 2026 bburda +// +// 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. + +#pragma once + +#include +#include + +#include "ros2_medkit_gateway/models/entity_types.hpp" + +namespace ros2_medkit_gateway { + +/** + * @brief Parsed entity path information from HTTP request URL + * + * Contains all extracted information from an entity-related HTTP request path, + * supporting both top-level entities (apps, components, areas, functions) and + * nested entities (subareas, subcomponents). + */ +struct EntityPathInfo { + SovdEntityType type; ///< APP, COMPONENT, AREA, FUNCTION, or UNKNOWN + std::string entity_id; ///< Entity identifier (e.g., "motor_controller") + std::string resource_path; ///< Remainder after entity (e.g., "/faults/MOTOR_ERR") + std::string entity_path; ///< Full entity path (e.g., "/apps/motor_controller") + std::string parent_id; ///< For nested entities: parent entity ID (e.g., "perception" for subarea) + bool is_nested{false}; ///< True if this is a nested entity (subarea/subcomponent) +}; + +/** + * @brief Parse entity path from HTTP request URL + * + * Extracts entity type, ID, and resource path from URLs like: + * - /api/v1/apps/{id}/... -> APP, id, ... + * - /api/v1/components/{id}/... -> COMPONENT, id, ... + * - /api/v1/areas/{id}/... -> AREA, id, ... + * - /api/v1/functions/{id}/... -> FUNCTION, id, ... + * - /api/v1/areas/{parent}/subareas/{id}/... -> AREA, id, parent + * - /api/v1/components/{parent}/subcomponents/{id}/... -> COMPONENT, id, parent + * + * @param request_path Full request path (e.g., "/api/v1/apps/motor/faults") + * @return Parsed EntityPathInfo, or std::nullopt if path doesn't match any pattern + */ +std::optional parse_entity_path(const std::string & request_path); + +/** + * @brief Extract category from bulk-data URL + * + * Examples: + * - "/bulk-data/rosbags/..." -> "rosbags" + * - "/bulk-data/snapshots/..." -> "snapshots" + * + * @param path URL path containing bulk-data segment + * @return Category string, or empty if not found + */ +std::string extract_bulk_data_category(const std::string & path); + +/** + * @brief Extract bulk-data ID (UUID) from URL + * + * Example: "/bulk-data/rosbags/550e8400-e29b-41d4-a716-446655440000" -> UUID + * + * @param path URL path containing bulk-data segment + * @return Bulk data ID (UUID), or empty if not found + */ +std::string extract_bulk_data_id(const std::string & path); + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/bulkdata_handlers.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/bulkdata_handlers.hpp new file mode 100644 index 00000000..9a59e278 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/bulkdata_handlers.hpp @@ -0,0 +1,114 @@ +// Copyright 2026 bburda +// +// 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. + +#pragma once + +#include + +#include + +#include "ros2_medkit_gateway/http/handlers/handler_context.hpp" + +namespace ros2_medkit_gateway { +namespace handlers { + +/** + * @brief HTTP handlers for SOVD bulk-data endpoints. + * + * Provides handlers for listing bulk-data categories, descriptors, + * and downloading bulk-data files (rosbags) for any entity type. + * + * Supports SOVD entity paths: + * - /apps/{id}/bulk-data[/{category}[/{id}]] + * - /components/{id}/bulk-data[/{category}[/{id}]] + * - /areas/{id}/bulk-data[/{category}[/{id}]] + * - /functions/{id}/bulk-data[/{category}[/{id}]] + * - Nested entities (subareas, subcomponents) + */ +class BulkDataHandlers { + public: + /** + * @brief Construct BulkDataHandlers. + * @param ctx Handler context for sending responses and accessing FaultManager + */ + explicit BulkDataHandlers(HandlerContext & ctx); + + /** + * @brief GET {entity-path}/bulk-data - List bulk-data categories. + * + * Returns available bulk-data categories for an entity. + * Currently only "rosbags" category is supported. + * + * @param req HTTP request + * @param res HTTP response + */ + void handle_list_categories(const httplib::Request & req, httplib::Response & res); + + /** + * @brief GET {entity-path}/bulk-data/{category} - List bulk-data descriptors. + * + * Returns BulkDataDescriptor array for the specified category. + * For "rosbags" category, returns descriptors for all rosbags + * associated with faults from this entity. + * + * @param req HTTP request + * @param res HTTP response + */ + void handle_list_descriptors(const httplib::Request & req, httplib::Response & res); + + /** + * @brief GET {entity-path}/bulk-data/{category}/{id} - Download bulk-data file. + * + * Downloads the bulk-data file (rosbag) identified by the ID. + * Validates that the rosbag belongs to the specified entity. + * + * @param req HTTP request + * @param res HTTP response + */ + void handle_download(const httplib::Request & req, httplib::Response & res); + + /** + * @brief Get MIME type for rosbag format. + * @param format Storage format ("mcap", "sqlite3", "db3") + * @return MIME type string + */ + static std::string get_rosbag_mimetype(const std::string & format); + + private: + HandlerContext & ctx_; + + /** + * @brief Stream file contents to HTTP response. + * @param res HTTP response to write to + * @param file_path Path to file to stream (can be file or rosbag directory) + * @param content_type MIME type for Content-Type header + * @return true if successful, false if file could not be read + */ + bool stream_file_to_response(httplib::Response & res, const std::string & file_path, + const std::string & content_type); + + /** + * @brief Resolve rosbag file path from storage path. + * + * Rosbag2 creates a directory containing the actual db3/mcap file. + * This function resolves the directory to the actual file path. + * + * @param path Path to rosbag (can be file or directory) + * @return Resolved file path, or empty string if not found + */ + static std::string resolve_rosbag_file_path(const std::string & path); +}; + +} // namespace handlers +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp index 6260b0e7..7881db9b 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp @@ -15,7 +15,11 @@ #ifndef ROS2_MEDKIT_GATEWAY__HTTP__HANDLERS__FAULT_HANDLERS_HPP_ #define ROS2_MEDKIT_GATEWAY__HTTP__HANDLERS__FAULT_HANDLERS_HPP_ +#include + #include "ros2_medkit_gateway/http/handlers/handler_context.hpp" +#include "ros2_medkit_msgs/msg/environment_data.hpp" +#include "ros2_medkit_msgs/msg/fault.hpp" namespace ros2_medkit_gateway { namespace handlers { @@ -25,11 +29,12 @@ namespace handlers { * * Provides handlers for: * - GET /faults - List all faults - * - GET /faults/{fault_code}/snapshots - Get snapshots for a fault * - GET /components/{component_id}/faults - List faults for a component * - GET /components/{component_id}/faults/{fault_code} - Get specific fault - * - GET /components/{component_id}/faults/{fault_code}/snapshots - Get snapshots for a fault * - DELETE /components/{component_id}/faults/{fault_code} - Clear fault + * + * Note: Snapshot data is inline in fault responses (environment_data). + * Rosbag downloads use the bulk-data endpoint pattern. */ class FaultHandlers { public: @@ -66,19 +71,21 @@ class FaultHandlers { void handle_clear_all_faults(const httplib::Request & req, httplib::Response & res); /** - * @brief Handle GET /faults/{fault_code}/snapshots - get snapshots for a fault (system-wide). - */ - void handle_get_snapshots(const httplib::Request & req, httplib::Response & res); - - /** - * @brief Handle GET /components/{component_id}/faults/{fault_code}/snapshots - get snapshots for a fault. - */ - void handle_get_component_snapshots(const httplib::Request & req, httplib::Response & res); - - /** - * @brief Handle GET /faults/{fault_code}/snapshots/bag - download rosbag file for a fault. + * @brief Build SOVD-compliant fault response with environment data. + * + * Creates a response containing: + * - "item" with fault details and SOVD status object + * - "environment_data" with extended_data_records and snapshots + * - "x-medkit" extensions with occurrence_count, severity_label, etc. + * + * @param fault The fault message from fault_manager + * @param env_data Environment data with snapshots + * @param entity_path Entity path for bulk_data_uri generation (e.g., "/apps/motor") + * @return SOVD-compliant JSON response */ - void handle_get_rosbag(const httplib::Request & req, httplib::Response & res); + static nlohmann::json build_sovd_fault_response(const ros2_medkit_msgs::msg::Fault & fault, + const ros2_medkit_msgs::msg::EnvironmentData & env_data, + const std::string & entity_path); private: HandlerContext & ctx_; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handlers.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handlers.hpp index b7d4f8eb..93614ee3 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handlers.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handlers.hpp @@ -30,6 +30,7 @@ // Other handlers #include "ros2_medkit_gateway/http/handlers/auth_handlers.hpp" +#include "ros2_medkit_gateway/http/handlers/bulkdata_handlers.hpp" #include "ros2_medkit_gateway/http/handlers/config_handlers.hpp" #include "ros2_medkit_gateway/http/handlers/fault_handlers.hpp" #include "ros2_medkit_gateway/http/handlers/handler_context.hpp" diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/http_utils.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/http_utils.hpp index a556e938..ac052895 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/http_utils.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/http_utils.hpp @@ -16,6 +16,9 @@ #include +#include +#include +#include #include #include "ros2_medkit_gateway/models/entity_types.hpp" @@ -123,4 +126,35 @@ inline FaultStatusFilter parse_fault_status_param(const httplib::Request & req) return filter; } +/** + * @brief Convert nanoseconds since epoch to ISO 8601 string with milliseconds. + * + * Shared utility for formatting timestamps in SOVD-compliant responses. + * Used by fault_handlers and bulkdata_handlers. + * + * @param ns Nanoseconds since epoch + * @return ISO 8601 formatted string (e.g., "2025-01-15T10:30:00.123Z"), + * or "1970-01-01T00:00:00.000Z" on conversion failure + */ +inline std::string format_timestamp_ns(int64_t ns) { + auto seconds = ns / 1'000'000'000; + auto nanos = ns % 1'000'000'000; + + std::time_t time = static_cast(seconds); + std::tm tm_buf{}; + std::tm * tm = gmtime_r(&time, &tm_buf); + if (!tm) { + return "1970-01-01T00:00:00.000Z"; // fallback for invalid timestamps + } + + char buf[64]; + std::strftime(buf, sizeof(buf), "%Y-%m-%dT%H:%M:%S", tm); + + // Add milliseconds + char result[80]; + std::snprintf(result, sizeof(result), "%s.%03dZ", buf, static_cast(nanos / 1'000'000)); + + return result; +} + } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/rest_server.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/rest_server.hpp index f6620f87..8655a6e0 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/rest_server.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/rest_server.hpp @@ -92,6 +92,7 @@ class RESTServer { std::unique_ptr fault_handlers_; std::unique_ptr auth_handlers_; std::unique_ptr sse_fault_handler_; + std::unique_ptr bulkdata_handlers_; }; } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/fault_manager.cpp b/src/ros2_medkit_gateway/src/fault_manager.cpp index 6430efc6..91180b90 100644 --- a/src/ros2_medkit_gateway/src/fault_manager.cpp +++ b/src/ros2_medkit_gateway/src/fault_manager.cpp @@ -15,9 +15,8 @@ #include "ros2_medkit_gateway/fault_manager.hpp" #include -#include - #include +#include using namespace std::chrono_literals; @@ -26,10 +25,12 @@ namespace ros2_medkit_gateway { FaultManager::FaultManager(rclcpp::Node * node) : node_(node) { // Create service clients for fault_manager services report_fault_client_ = node_->create_client("/fault_manager/report_fault"); - get_faults_client_ = node_->create_client("/fault_manager/get_faults"); + get_fault_client_ = node_->create_client("/fault_manager/get_fault"); + list_faults_client_ = node_->create_client("/fault_manager/list_faults"); clear_fault_client_ = node_->create_client("/fault_manager/clear_fault"); get_snapshots_client_ = node_->create_client("/fault_manager/get_snapshots"); get_rosbag_client_ = node_->create_client("/fault_manager/get_rosbag"); + list_rosbags_client_ = node_->create_client("/fault_manager/list_rosbags"); // Get configurable timeout service_timeout_sec_ = node_->declare_parameter("fault_service_timeout_sec", 5.0); @@ -38,13 +39,13 @@ FaultManager::FaultManager(rclcpp::Node * node) : node_(node) { } bool FaultManager::wait_for_services(std::chrono::duration timeout) { - return report_fault_client_->wait_for_service(timeout) && get_faults_client_->wait_for_service(timeout) && - clear_fault_client_->wait_for_service(timeout); + return report_fault_client_->wait_for_service(timeout) && get_fault_client_->wait_for_service(timeout) && + list_faults_client_->wait_for_service(timeout) && clear_fault_client_->wait_for_service(timeout); } bool FaultManager::is_available() const { - return report_fault_client_->service_is_ready() && get_faults_client_->service_is_ready() && - clear_fault_client_->service_is_ready(); + return report_fault_client_->service_is_ready() && get_fault_client_->service_is_ready() && + list_faults_client_->service_is_ready() && clear_fault_client_->service_is_ready(); } /// Convert a ROS 2 Fault message to JSON for REST API responses. @@ -90,7 +91,7 @@ json FaultManager::fault_to_json(const ros2_medkit_msgs::msg::Fault & fault) { FaultResult FaultManager::report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, const std::string & source_id) { - std::lock_guard lock(service_mutex_); + std::lock_guard lock(report_mutex_); FaultResult result; auto timeout = std::chrono::duration(service_timeout_sec_); @@ -125,19 +126,19 @@ FaultResult FaultManager::report_fault(const std::string & fault_code, uint8_t s return result; } -FaultResult FaultManager::get_faults(const std::string & source_id, bool include_prefailed, bool include_confirmed, - bool include_cleared, bool include_muted, bool include_clusters) { - std::lock_guard lock(service_mutex_); +FaultResult FaultManager::list_faults(const std::string & source_id, bool include_prefailed, bool include_confirmed, + bool include_cleared, bool include_muted, bool include_clusters) { + std::lock_guard lock(list_mutex_); FaultResult result; auto timeout = std::chrono::duration(service_timeout_sec_); - if (!get_faults_client_->wait_for_service(timeout)) { + if (!list_faults_client_->wait_for_service(timeout)) { result.success = false; - result.error_message = "GetFaults service not available"; + result.error_message = "ListFaults service not available"; return result; } - auto request = std::make_shared(); + auto request = std::make_shared(); request->filter_by_severity = false; request->severity = 0; @@ -156,11 +157,11 @@ FaultResult FaultManager::get_faults(const std::string & source_id, bool include request->include_muted = include_muted; request->include_clusters = include_clusters; - auto future = get_faults_client_->async_send_request(request); + auto future = list_faults_client_->async_send_request(request); if (future.wait_for(timeout) != std::future_status::ready) { result.success = false; - result.error_message = "GetFaults service call timed out"; + result.error_message = "ListFaults service call timed out"; return result; } @@ -230,32 +231,75 @@ FaultResult FaultManager::get_faults(const std::string & source_id, bool include return result; } -FaultResult FaultManager::get_fault(const std::string & fault_code, const std::string & source_id) { - // Get all faults and filter by fault_code - auto all_faults = get_faults(source_id, true, true, true); +FaultWithEnvResult FaultManager::get_fault_with_env(const std::string & fault_code, const std::string & source_id) { + std::lock_guard lock(get_mutex_); + FaultWithEnvResult result; - if (!all_faults.success) { - return all_faults; + auto timeout = std::chrono::duration(service_timeout_sec_); + if (!get_fault_client_->wait_for_service(timeout)) { + result.success = false; + result.error_message = "GetFault service not available"; + return result; } - FaultResult result; + auto request = std::make_shared(); + request->fault_code = fault_code; - // Find the specific fault - for (const auto & fault : all_faults.data["faults"]) { - if (fault["fault_code"] == fault_code) { - result.success = true; - result.data = fault; - return result; + auto future = get_fault_client_->async_send_request(request); + + if (future.wait_for(timeout) != std::future_status::ready) { + result.success = false; + result.error_message = "GetFault service call timed out"; + return result; + } + + auto response = future.get(); + result.success = response->success; + + if (response->success) { + result.fault = response->fault; + result.environment_data = response->environment_data; + + // Verify source_id if provided + if (!source_id.empty()) { + bool matches = false; + for (const auto & src : result.fault.reporting_sources) { + if (src.rfind(source_id, 0) == 0) { + matches = true; + break; + } + } + if (!matches) { + result.success = false; + result.error_message = "Fault not found for source: " + source_id; + result.fault = ros2_medkit_msgs::msg::Fault(); + result.environment_data = ros2_medkit_msgs::msg::EnvironmentData(); + } } + } else { + result.error_message = response->error_message; + } + + return result; +} + +FaultResult FaultManager::get_fault(const std::string & fault_code, const std::string & source_id) { + // Use get_fault_with_env and convert to JSON + auto env_result = get_fault_with_env(fault_code, source_id); + + FaultResult result; + result.success = env_result.success; + result.error_message = env_result.error_message; + + if (env_result.success) { + result.data = fault_to_json(env_result.fault); } - result.success = false; - result.error_message = "Fault not found: " + fault_code; return result; } FaultResult FaultManager::clear_fault(const std::string & fault_code) { - std::lock_guard lock(service_mutex_); + std::lock_guard lock(clear_mutex_); FaultResult result; auto timeout = std::chrono::duration(service_timeout_sec_); @@ -292,7 +336,7 @@ FaultResult FaultManager::clear_fault(const std::string & fault_code) { } FaultResult FaultManager::get_snapshots(const std::string & fault_code, const std::string & topic) { - std::lock_guard lock(service_mutex_); + std::lock_guard lock(snapshots_mutex_); FaultResult result; auto timeout = std::chrono::duration(service_timeout_sec_); @@ -332,7 +376,7 @@ FaultResult FaultManager::get_snapshots(const std::string & fault_code, const st } FaultResult FaultManager::get_rosbag(const std::string & fault_code) { - std::lock_guard lock(service_mutex_); + std::lock_guard lock(rosbag_mutex_); FaultResult result; auto timeout = std::chrono::duration(service_timeout_sec_); @@ -368,4 +412,46 @@ FaultResult FaultManager::get_rosbag(const std::string & fault_code) { return result; } +FaultResult FaultManager::list_rosbags(const std::string & entity_fqn) { + std::lock_guard lock(list_rosbags_mutex_); + FaultResult result; + + auto timeout = std::chrono::duration(service_timeout_sec_); + if (!list_rosbags_client_->wait_for_service(timeout)) { + result.success = false; + result.error_message = "ListRosbags service not available"; + return result; + } + + auto request = std::make_shared(); + request->entity_fqn = entity_fqn; + + auto future = list_rosbags_client_->async_send_request(request); + + if (future.wait_for(timeout) != std::future_status::ready) { + result.success = false; + result.error_message = "ListRosbags service call timed out"; + return result; + } + + auto response = future.get(); + result.success = response->success; + + if (response->success) { + json rosbags = json::array(); + for (size_t i = 0; i < response->fault_codes.size(); ++i) { + rosbags.push_back({{"fault_code", response->fault_codes[i]}, + {"file_path", response->file_paths[i]}, + {"format", response->formats[i]}, + {"duration_sec", response->durations_sec[i]}, + {"size_bytes", response->sizes_bytes[i]}}); + } + result.data = {{"rosbags", rosbags}}; + } else { + result.error_message = response->error_message; + } + + return result; +} + } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp b/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp new file mode 100644 index 00000000..8076accb --- /dev/null +++ b/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp @@ -0,0 +1,144 @@ +// Copyright 2026 bburda +// +// 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_gateway/http/entity_path_utils.hpp" + +#include +#include + +namespace ros2_medkit_gateway { + +namespace { + +// Pattern definition for entity path matching +struct PatternDef { + std::regex pattern; + SovdEntityType type; + bool is_nested; +}; + +// Get static patterns - order matters (nested patterns must come first) +const std::vector & get_patterns() { + // clang-format off + static const std::vector patterns = { + // Nested entities (must be checked first due to more specific patterns) + {std::regex(R"(/api/v1/areas/([^/]+)/subareas/([^/]+)(/.*)?$)"), SovdEntityType::AREA, true}, + {std::regex(R"(/api/v1/components/([^/]+)/subcomponents/([^/]+)(/.*)?$)"), SovdEntityType::COMPONENT, true}, + // Top-level entities + {std::regex(R"(/api/v1/apps/([^/]+)(/.*)?$)"), SovdEntityType::APP, false}, + {std::regex(R"(/api/v1/components/([^/]+)(/.*)?$)"), SovdEntityType::COMPONENT, false}, + {std::regex(R"(/api/v1/areas/([^/]+)(/.*)?$)"), SovdEntityType::AREA, false}, + {std::regex(R"(/api/v1/functions/([^/]+)(/.*)?$)"), SovdEntityType::FUNCTION, false}, + }; + // clang-format on + return patterns; +} + +// Get entity type path segment +std::string get_type_segment(SovdEntityType type, bool is_nested) { + if (is_nested) { + switch (type) { + case SovdEntityType::AREA: + return "subareas"; + case SovdEntityType::COMPONENT: + return "subcomponents"; + default: + break; + } + } + switch (type) { + case SovdEntityType::APP: + return "apps"; + case SovdEntityType::COMPONENT: + return "components"; + case SovdEntityType::AREA: + return "areas"; + case SovdEntityType::FUNCTION: + return "functions"; + default: + return ""; + } +} + +} // namespace + +std::optional parse_entity_path(const std::string & request_path) { + if (request_path.empty()) { + return std::nullopt; + } + + for (const auto & def : get_patterns()) { + std::smatch match; + if (std::regex_match(request_path, match, def.pattern)) { + EntityPathInfo info; + info.type = def.type; + info.is_nested = def.is_nested; + + if (def.is_nested) { + // For nested: match[1] = parent_id, match[2] = entity_id, match[3] = resource_path + info.parent_id = match[1].str(); + info.entity_id = match[2].str(); + info.resource_path = match[3].matched ? match[3].str() : ""; + + // Build entity path: /{parent_type}/{parent_id}/{sub_type}/{entity_id} + std::string parent_segment = (def.type == SovdEntityType::AREA) ? "areas" : "components"; + std::string sub_segment = get_type_segment(def.type, true); + info.entity_path.reserve(parent_segment.size() + info.parent_id.size() + sub_segment.size() + + info.entity_id.size() + 4); + info.entity_path = "/"; + info.entity_path.append(parent_segment) + .append("/") + .append(info.parent_id) + .append("/") + .append(sub_segment) + .append("/") + .append(info.entity_id); + } else { + // For top-level: match[1] = entity_id, match[2] = resource_path + info.entity_id = match[1].str(); + info.resource_path = match[2].matched ? match[2].str() : ""; + + // Build entity path: /{type}/{entity_id} + std::string type_segment = get_type_segment(def.type, false); + info.entity_path.reserve(type_segment.size() + info.entity_id.size() + 2); + info.entity_path = "/"; + info.entity_path.append(type_segment).append("/").append(info.entity_id); + } + + return info; + } + } + + return std::nullopt; +} + +std::string extract_bulk_data_category(const std::string & path) { + static const std::regex pattern(R"(/bulk-data/([^/]+))"); + std::smatch match; + if (std::regex_search(path, match, pattern)) { + return match[1].str(); + } + return ""; +} + +std::string extract_bulk_data_id(const std::string & path) { + static const std::regex pattern(R"(/bulk-data/[^/]+/([^/]+))"); + std::smatch match; + if (std::regex_search(path, match, pattern)) { + return match[1].str(); + } + return ""; +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp new file mode 100644 index 00000000..2cde0010 --- /dev/null +++ b/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp @@ -0,0 +1,291 @@ +// Copyright 2026 bburda +// +// 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_gateway/http/handlers/bulkdata_handlers.hpp" + +#include +#include +#include +#include + +#include "ros2_medkit_gateway/gateway_node.hpp" +#include "ros2_medkit_gateway/http/entity_path_utils.hpp" +#include "ros2_medkit_gateway/http/error_codes.hpp" +#include "ros2_medkit_gateway/http/http_utils.hpp" + +namespace ros2_medkit_gateway { +namespace handlers { + +BulkDataHandlers::BulkDataHandlers(HandlerContext & ctx) : ctx_(ctx) { +} + +void BulkDataHandlers::handle_list_categories(const httplib::Request & req, httplib::Response & res) { + // Parse entity path from request URL + auto entity_info = parse_entity_path(req.path); + if (!entity_info) { + HandlerContext::send_error(res, httplib::StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Invalid entity path"); + return; + } + + // Validate entity exists and matches the route type (e.g., /components/ only accepts components) + auto entity_opt = ctx_.validate_entity_for_route(req, res, entity_info->entity_id); + if (!entity_opt) { + return; // Error response already sent + } + + // Currently only "rosbags" category is supported + nlohmann::json response = {{"items", nlohmann::json::array({"rosbags"})}}; + + HandlerContext::send_json(res, response); +} + +void BulkDataHandlers::handle_list_descriptors(const httplib::Request & req, httplib::Response & res) { + // Parse entity path from request URL + auto entity_info = parse_entity_path(req.path); + if (!entity_info) { + HandlerContext::send_error(res, httplib::StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Invalid entity path"); + return; + } + + // Validate entity exists and matches the route type + auto entity_opt = ctx_.validate_entity_for_route(req, res, entity_info->entity_id); + if (!entity_opt) { + return; // Error response already sent + } + auto entity = *entity_opt; + + // Extract and validate category from path + auto category = extract_bulk_data_category(req.path); + if (category.empty()) { + HandlerContext::send_error(res, httplib::StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Missing category"); + return; + } + + if (category != "rosbags") { + HandlerContext::send_error(res, httplib::StatusCode::NotFound_404, ERR_RESOURCE_NOT_FOUND, + "Unknown category: " + category); + return; + } + + // Get FaultManager from node + auto fault_mgr = ctx_.node()->get_fault_manager(); + + // Get all faults for this entity (filter by FQN/namespace path) + // Use the entity's FQN or namespace_path as the source_id filter + std::string source_filter = entity.fqn.empty() ? entity.namespace_path : entity.fqn; + auto faults_result = fault_mgr->list_faults(source_filter); + + // Build a map of fault_code -> fault_json for quick lookup + std::unordered_map fault_map; + if (faults_result.success && faults_result.data.contains("faults")) { + for (const auto & fault_json : faults_result.data["faults"]) { + if (fault_json.contains("fault_code")) { + std::string fc = fault_json["fault_code"].get(); + fault_map[fc] = fault_json; + } + } + } + + // Use batch rosbag retrieval (single service call) instead of N+1 individual calls + auto rosbags_result = fault_mgr->list_rosbags(source_filter); + + nlohmann::json items = nlohmann::json::array(); + + if (rosbags_result.success && rosbags_result.data.contains("rosbags")) { + for (const auto & rosbag : rosbags_result.data["rosbags"]) { + std::string fault_code = rosbag.value("fault_code", ""); + std::string format = rosbag.value("format", "mcap"); + uint64_t size_bytes = rosbag.value("size_bytes", 0); + double duration_sec = rosbag.value("duration_sec", 0.0); + + // Use fault_code as bulk_data_id + std::string bulk_data_id = fault_code; + + // Get timestamp from fault if available + int64_t created_at_ns = 0; + auto it = fault_map.find(fault_code); + if (it != fault_map.end()) { + double first_occurred = it->second.value("first_occurred", 0.0); + created_at_ns = static_cast(first_occurred * 1'000'000'000); + } + + nlohmann::json descriptor = { + {"id", bulk_data_id}, + {"name", fault_code + " recording " + format_timestamp_ns(created_at_ns)}, + {"mimetype", get_rosbag_mimetype(format)}, + {"size", size_bytes}, + {"creation_date", format_timestamp_ns(created_at_ns)}, + {"x-medkit", {{"fault_code", fault_code}, {"duration_sec", duration_sec}, {"format", format}}}}; + items.push_back(descriptor); + } + } + + nlohmann::json response = {{"items", items}}; + HandlerContext::send_json(res, response); +} + +void BulkDataHandlers::handle_download(const httplib::Request & req, httplib::Response & res) { + // Parse entity path from request URL + auto entity_info = parse_entity_path(req.path); + if (!entity_info) { + HandlerContext::send_error(res, httplib::StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Invalid entity path"); + return; + } + + // Validate entity exists and matches the route type + auto entity_opt = ctx_.validate_entity_for_route(req, res, entity_info->entity_id); + if (!entity_opt) { + return; // Error response already sent + } + auto entity = *entity_opt; + + // Extract category and bulk_data_id from path + auto category = extract_bulk_data_category(req.path); + auto bulk_data_id = extract_bulk_data_id(req.path); + + if (category != "rosbags") { + HandlerContext::send_error(res, httplib::StatusCode::NotFound_404, ERR_RESOURCE_NOT_FOUND, + "Unknown category: " + category); + return; + } + + if (bulk_data_id.empty()) { + HandlerContext::send_error(res, httplib::StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Missing bulk-data ID"); + return; + } + + // Get FaultManager from node + auto fault_mgr = ctx_.node()->get_fault_manager(); + + // bulk_data_id is the fault_code + std::string fault_code = bulk_data_id; + + // Get rosbag info + auto rosbag_result = fault_mgr->get_rosbag(fault_code); + if (!rosbag_result.success || !rosbag_result.data.contains("file_path")) { + HandlerContext::send_error(res, httplib::StatusCode::NotFound_404, ERR_RESOURCE_NOT_FOUND, "Bulk-data not found", + {{"bulk_data_id", bulk_data_id}}); + return; + } + + // Security check: verify rosbag belongs to this entity + // Use targeted get_fault lookup instead of loading the entire fault list + std::string source_filter = entity.fqn.empty() ? entity.namespace_path : entity.fqn; + auto fault_result = fault_mgr->get_fault(fault_code, source_filter); + + if (!fault_result.success) { + HandlerContext::send_error(res, httplib::StatusCode::NotFound_404, ERR_RESOURCE_NOT_FOUND, + "Bulk-data not found for this entity", {{"entity_id", entity_info->entity_id}}); + return; + } + + // Get file path and stream the file + std::string file_path = rosbag_result.data["file_path"].get(); + std::string format = rosbag_result.data.value("format", "mcap"); + auto mimetype = get_rosbag_mimetype(format); + std::string filename = fault_code + "." + format; + + // Set response headers for file download + res.set_header("Content-Disposition", "attachment; filename=\"" + filename + "\""); + + if (!stream_file_to_response(res, file_path, mimetype)) { + HandlerContext::send_error(res, httplib::StatusCode::InternalServerError_500, ERR_INTERNAL_ERROR, + "Failed to read rosbag file"); + } +} + +bool BulkDataHandlers::stream_file_to_response(httplib::Response & res, const std::string & file_path, + const std::string & content_type) { + // Resolve the actual file path - rosbag2 creates a directory with the db3/mcap file inside + std::string actual_path = resolve_rosbag_file_path(file_path); + if (actual_path.empty()) { + return false; + } + + // Get file size without reading entire file into memory + std::error_code ec; + auto file_size = std::filesystem::file_size(actual_path, ec); + if (ec) { + return false; + } + + // Use chunked streaming via content provider to avoid loading large rosbag files into memory. + // Rosbag files can be hundreds of MB to multiple GB. + static constexpr size_t kChunkSize = 64 * 1024; // 64 KB chunks + + res.set_content_provider(static_cast(file_size), content_type, + [actual_path](size_t offset, size_t length, httplib::DataSink & sink) -> bool { + std::ifstream file(actual_path, std::ios::binary); + if (!file.is_open()) { + return false; + } + + file.seekg(static_cast(offset)); + if (!file.good()) { + return false; + } + + size_t remaining = length; + std::vector buf(std::min(remaining, kChunkSize)); + + while (remaining > 0 && file.good()) { + size_t to_read = std::min(remaining, kChunkSize); + file.read(buf.data(), static_cast(to_read)); + auto bytes_read = static_cast(file.gcount()); + if (bytes_read == 0) { + break; + } + sink.write(buf.data(), bytes_read); + remaining -= bytes_read; + } + + return remaining == 0; + }); + + return true; +} + +std::string BulkDataHandlers::resolve_rosbag_file_path(const std::string & path) { + // If it's a regular file, return as-is + if (std::filesystem::is_regular_file(path)) { + return path; + } + + // If it's a directory (rosbag2 directory structure), find the db3/mcap file inside + if (std::filesystem::is_directory(path)) { + for (const auto & entry : std::filesystem::directory_iterator(path)) { + if (entry.is_regular_file()) { + auto ext = entry.path().extension().string(); + // Look for db3 (sqlite3 format) or mcap files + if (ext == ".db3" || ext == ".mcap") { + return entry.path().string(); + } + } + } + } + + return ""; // File not found +} + +std::string BulkDataHandlers::get_rosbag_mimetype(const std::string & format) { + if (format == "mcap") { + return "application/x-mcap"; + } else if (format == "sqlite3" || format == "db3") { + return "application/x-sqlite3"; + } + return "application/octet-stream"; +} + +} // namespace handlers +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp index 6884fde4..e73f265a 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp @@ -18,14 +18,13 @@ #include #include #include -#include -#include -#include #include #include +#include #include #include "ros2_medkit_gateway/gateway_node.hpp" +#include "ros2_medkit_gateway/http/entity_path_utils.hpp" #include "ros2_medkit_gateway/http/error_codes.hpp" #include "ros2_medkit_gateway/http/http_utils.hpp" #include "ros2_medkit_gateway/http/x_medkit.hpp" @@ -38,58 +37,6 @@ namespace handlers { namespace { -/// Sanitize a string for use in HTTP Content-Disposition filename -/// Removes/replaces characters that could cause header injection or filesystem issues -std::string sanitize_filename(const std::string & input) { - std::string result; - result.reserve(input.size()); - for (char c : input) { - // Allow only alphanumeric, underscore, hyphen, dot - if (std::isalnum(static_cast(c)) || c == '_' || c == '-' || c == '.') { - result += c; - } else { - result += '_'; // Replace unsafe characters - } - } - return result; -} - -/// Generate a timestamp string in YYYYMMDD_HHMMSS format for filenames -std::string generate_timestamp() { - auto now = std::chrono::system_clock::now(); - std::time_t now_time = std::chrono::system_clock::to_time_t(now); - std::tm now_tm; - gmtime_r(&now_time, &now_tm); // Use UTC for consistency - std::ostringstream oss; - oss << std::put_time(&now_tm, "%Y%m%d_%H%M%S"); - return oss.str(); -} - -/// Maximum allowed length for fault_code -constexpr size_t kMaxFaultCodeLength = 128; - -/// Validate fault_code format (same rules as FaultManagerNode) -/// @param fault_code The fault code to validate -/// @return Empty string if valid, error message if invalid -std::string validate_fault_code(const std::string & fault_code) { - if (fault_code.empty()) { - return "fault_code cannot be empty"; - } - if (fault_code.length() > kMaxFaultCodeLength) { - return "fault_code exceeds maximum length of " + std::to_string(kMaxFaultCodeLength) + " characters"; - } - for (char c : fault_code) { - if (!std::isalnum(static_cast(c)) && c != '_' && c != '-' && c != '.') { - return "fault_code contains invalid character '" + std::string(1, c) + - "'. Only alphanumeric, underscore, hyphen, and dot are allowed"; - } - } - if (fault_code.find("..") != std::string::npos) { - return "fault_code cannot contain '..' (path traversal not allowed)"; - } - return ""; -} - /// Helper to filter faults JSON array by a set of namespace prefixes /// Keeps faults where any reporting_source starts with any of the given prefixes json filter_faults_by_sources(const json & faults_array, const std::set & source_prefixes) { @@ -119,8 +66,167 @@ json filter_faults_by_sources(const json & faults_array, const std::set primary_fields = { + {"std_msgs/msg/Float64", "data"}, + {"std_msgs/msg/Float32", "data"}, + {"std_msgs/msg/Int32", "data"}, + {"std_msgs/msg/Int64", "data"}, + {"std_msgs/msg/Bool", "data"}, + {"std_msgs/msg/String", "data"}, + {"sensor_msgs/msg/Temperature", "temperature"}, + {"sensor_msgs/msg/BatteryState", "percentage"}, + {"sensor_msgs/msg/FluidPressure", "fluid_pressure"}, + {"sensor_msgs/msg/Range", "range"}, + {"geometry_msgs/msg/Twist", "linear"}, // Returns nested object + {"nav_msgs/msg/Odometry", "pose"}, // Returns nested object + }; + + auto it = primary_fields.find(message_type); + if (it != primary_fields.end() && full_data.contains(it->second)) { + return full_data[it->second]; + } + + // Fallback: return full data + return full_data; +} + } // namespace +// Static method: Build SOVD-compliant fault response with environment data +json FaultHandlers::build_sovd_fault_response(const ros2_medkit_msgs::msg::Fault & fault, + const ros2_medkit_msgs::msg::EnvironmentData & env_data, + const std::string & entity_path) { + json response; + + // === SOVD "item" structure === + response["item"] = {{"code", fault.fault_code}, + {"fault_name", fault.description}, + {"severity", fault.severity}, + {"status", build_status_object(fault.status)}}; + + // === SOVD "environment_data" === + json snapshots = json::array(); + + for (const auto & s : env_data.snapshots) { + json snap; + snap["type"] = s.type; + snap["name"] = s.name; + + if (s.type == "freeze_frame") { + // Parse JSON data and extract primary value + try { + json full_data = json::parse(s.data); + snap["data"] = extract_primary_value(s.message_type, full_data); + snap["x-medkit"] = {{"topic", s.topic}, + {"message_type", s.message_type}, + {"full_data", full_data}, + {"captured_at", to_iso8601_ns(s.captured_at_ns)}}; + } catch (const json::exception & e) { + // Invalid JSON - include raw data + snap["data"] = s.data; + snap["x-medkit"] = {{"topic", s.topic}, {"message_type", s.message_type}, {"parse_error", e.what()}}; + } + } else if (s.type == "rosbag") { + // Build absolute URI using entity path + fault_code as the bulk-data ID. + // This must match the download handler which looks up rosbags by fault_code, + // and handle_list_descriptors which also uses fault_code as the descriptor ID. + snap["bulk_data_uri"] = entity_path + "/bulk-data/rosbags/" + fault.fault_code; + snap["size_bytes"] = s.size_bytes; + snap["duration_sec"] = s.duration_sec; + snap["format"] = s.format; + } + + snapshots.push_back(snap); + } + + response["environment_data"] = { + {"extended_data_records", + {{"first_occurrence", to_iso8601_ns(env_data.extended_data_records.first_occurrence_ns)}, + {"last_occurrence", to_iso8601_ns(env_data.extended_data_records.last_occurrence_ns)}}}, + {"snapshots", snapshots}}; + + // === x-medkit extensions === + json reporting_sources = json::array(); + for (const auto & src : fault.reporting_sources) { + reporting_sources.push_back(src); + } + + response["x-medkit"] = {{"occurrence_count", fault.occurrence_count}, + {"reporting_sources", reporting_sources}, + {"severity_label", severity_to_label(fault.severity)}, + {"status_raw", fault.status}}; + + return response; +} + void FaultHandlers::handle_list_all_faults(const httplib::Request & req, httplib::Response & res) { try { auto filter = parse_fault_status_param(req); @@ -139,8 +245,8 @@ void FaultHandlers::handle_list_all_faults(const httplib::Request & req, httplib auto fault_mgr = ctx_.node()->get_fault_manager(); // Empty source_id = no filtering, return all faults - auto result = fault_mgr->get_faults("", filter.include_pending, filter.include_confirmed, filter.include_cleared, - include_muted, include_clusters); + auto result = fault_mgr->list_faults("", filter.include_pending, filter.include_confirmed, filter.include_cleared, + include_muted, include_clusters); if (result.success) { // Format: items array at top level @@ -215,8 +321,8 @@ void FaultHandlers::handle_list_faults(const httplib::Request & req, httplib::Re // Functions don't have a single namespace_path - they host apps from potentially different namespaces if (entity_info.type == EntityType::FUNCTION) { // Get all faults (no namespace filter) - auto result = fault_mgr->get_faults("", filter.include_pending, filter.include_confirmed, filter.include_cleared, - include_muted, include_clusters); + auto result = fault_mgr->list_faults("", filter.include_pending, filter.include_confirmed, filter.include_cleared, + include_muted, include_clusters); if (!result.success) { HandlerContext::send_error(res, StatusCode::ServiceUnavailable_503, ERR_SERVICE_UNAVAILABLE, @@ -256,8 +362,8 @@ void FaultHandlers::handle_list_faults(const httplib::Request & req, httplib::Re // Components group Apps, so we filter by the apps' FQNs rather than namespace (which is too broad) if (entity_info.type == EntityType::COMPONENT) { // Get all faults (no namespace filter) - auto result = fault_mgr->get_faults("", filter.include_pending, filter.include_confirmed, filter.include_cleared, - include_muted, include_clusters); + auto result = fault_mgr->list_faults("", filter.include_pending, filter.include_confirmed, filter.include_cleared, + include_muted, include_clusters); if (!result.success) { HandlerContext::send_error(res, StatusCode::ServiceUnavailable_503, ERR_SERVICE_UNAVAILABLE, @@ -296,8 +402,8 @@ void FaultHandlers::handle_list_faults(const httplib::Request & req, httplib::Re // For other entity types (App, Area), use namespace_path filtering std::string namespace_path = entity_info.namespace_path; - auto result = fault_mgr->get_faults(namespace_path, filter.include_pending, filter.include_confirmed, - filter.include_cleared, include_muted, include_clusters); + auto result = fault_mgr->list_faults(namespace_path, filter.include_pending, filter.include_confirmed, + filter.include_cleared, include_muted, include_clusters); if (result.success) { // Format: items array at top level @@ -346,6 +452,13 @@ void FaultHandlers::handle_get_fault(const httplib::Request & req, httplib::Resp entity_id = req.matches[1]; fault_code = req.matches[2]; + // Parse entity path from URL to get entity_path for bulk_data_uri + auto entity_path_info = parse_entity_path(req.path); + if (!entity_path_info) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Invalid entity path"); + return; + } + // Validate entity ID and type for this route auto entity_opt = ctx_.validate_entity_for_route(req, res, entity_id); if (!entity_opt) { @@ -363,16 +476,13 @@ void FaultHandlers::handle_get_fault(const httplib::Request & req, httplib::Resp std::string namespace_path = entity_info.namespace_path; auto fault_mgr = ctx_.node()->get_fault_manager(); - auto result = fault_mgr->get_fault(fault_code, namespace_path); - if (result.success) { - // Format: single item wrapped - json response = {{"item", result.data}}; + // Use get_fault_with_env to get fault with environment data + auto result = fault_mgr->get_fault_with_env(fault_code, namespace_path); - // x-medkit extension for entity context - XMedkit ext; - ext.entity_id(entity_id); - response["x-medkit"] = ext.build(); + if (result.success) { + // Build SOVD-compliant response with environment data + auto response = build_sovd_fault_response(result.fault, result.environment_data, entity_path_info->entity_path); HandlerContext::send_json(res, response); } else { @@ -468,7 +578,7 @@ void FaultHandlers::handle_clear_all_faults(const httplib::Request & req, httpli // Get all faults for this entity auto fault_mgr = ctx_.node()->get_fault_manager(); - auto faults_result = fault_mgr->get_faults(entity_info.namespace_path, "", ""); + auto faults_result = fault_mgr->list_faults(entity_info.namespace_path, "", ""); if (!faults_result.success) { HandlerContext::send_error(res, StatusCode::ServiceUnavailable_503, ERR_SERVICE_UNAVAILABLE, @@ -502,250 +612,5 @@ void FaultHandlers::handle_clear_all_faults(const httplib::Request & req, httpli } } -void FaultHandlers::handle_get_snapshots(const httplib::Request & req, httplib::Response & res) { - std::string fault_code; - try { - if (req.matches.size() < 2) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Invalid request"); - return; - } - - fault_code = req.matches[1]; - - // Validate fault code - if (fault_code.empty() || fault_code.length() > 256) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_PARAMETER, "Invalid fault code", - {{"details", "Fault code must be between 1 and 256 characters"}}); - return; - } - - // Optional topic filter from query parameter - std::string topic_filter = req.get_param_value("topic"); - - auto fault_mgr = ctx_.node()->get_fault_manager(); - auto result = fault_mgr->get_snapshots(fault_code, topic_filter); - - if (result.success) { - HandlerContext::send_json(res, result.data); - } else { - // Check if it's a "not found" error - if (result.error_message.find("not found") != std::string::npos || - result.error_message.find("Fault not found") != std::string::npos) { - HandlerContext::send_error(res, StatusCode::NotFound_404, ERR_RESOURCE_NOT_FOUND, "Fault not found", - {{"details", result.error_message}, {"fault_code", fault_code}}); - } else { - HandlerContext::send_error(res, StatusCode::ServiceUnavailable_503, ERR_SERVICE_UNAVAILABLE, - "Failed to get snapshots", - {{"details", result.error_message}, {"fault_code", fault_code}}); - } - } - } catch (const std::exception & e) { - HandlerContext::send_error(res, StatusCode::InternalServerError_500, ERR_INTERNAL_ERROR, "Failed to get snapshots", - {{"details", e.what()}, {"fault_code", fault_code}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_snapshots for fault '%s': %s", fault_code.c_str(), - e.what()); - } -} - -void FaultHandlers::handle_get_component_snapshots(const httplib::Request & req, httplib::Response & res) { - std::string entity_id; - std::string fault_code; - try { - if (req.matches.size() < 3) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Invalid request"); - return; - } - - entity_id = req.matches[1]; - fault_code = req.matches[2]; - - // Validate entity ID and type for this route - auto entity_opt = ctx_.validate_entity_for_route(req, res, entity_id); - if (!entity_opt) { - return; // Error response already sent - } - auto entity_info = *entity_opt; - - // Validate fault code - if (fault_code.empty() || fault_code.length() > 256) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_PARAMETER, "Invalid fault code", - {{"details", "Fault code must be between 1 and 256 characters"}}); - return; - } - - // Optional topic filter from query parameter - std::string topic_filter = req.get_param_value("topic"); - - auto fault_mgr = ctx_.node()->get_fault_manager(); - auto result = fault_mgr->get_snapshots(fault_code, topic_filter); - - if (result.success) { - // Add entity context to response - json response = result.data; - response[entity_info.id_field] = entity_id; - HandlerContext::send_json(res, response); - } else { - // Check if it's a "not found" error - if (result.error_message.find("not found") != std::string::npos || - result.error_message.find("Fault not found") != std::string::npos) { - HandlerContext::send_error( - res, StatusCode::NotFound_404, ERR_RESOURCE_NOT_FOUND, "Fault not found", - {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); - } else { - HandlerContext::send_error( - res, StatusCode::ServiceUnavailable_503, ERR_SERVICE_UNAVAILABLE, "Failed to get snapshots", - {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); - } - } - } catch (const std::exception & e) { - HandlerContext::send_error(res, StatusCode::InternalServerError_500, ERR_INTERNAL_ERROR, "Failed to get snapshots", - {{"details", e.what()}, {"entity_id", entity_id}, {"fault_code", fault_code}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_component_snapshots for entity '%s', fault '%s': %s", - entity_id.c_str(), fault_code.c_str(), e.what()); - } -} - -void FaultHandlers::handle_get_rosbag(const httplib::Request & req, httplib::Response & res) { - std::string fault_code; - try { - if (req.matches.size() < 2) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_REQUEST, "Invalid request", {}); - return; - } - - fault_code = req.matches[1]; - - // Validate fault code format (prevents path traversal and injection attacks) - std::string validation_error = validate_fault_code(fault_code); - if (!validation_error.empty()) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_PARAMETER, "Invalid fault code", - {{"details", validation_error}, {"fault_code", fault_code}}); - return; - } - - auto fault_mgr = ctx_.node()->get_fault_manager(); - auto result = fault_mgr->get_rosbag(fault_code); - - if (!result.success) { - // Check if it's a "not found" error - if (result.error_message.find("not found") != std::string::npos || - result.error_message.find("No rosbag") != std::string::npos) { - HandlerContext::send_error(res, StatusCode::NotFound_404, ERR_RESOURCE_NOT_FOUND, "Rosbag not found", - {{"details", result.error_message}, {"fault_code", fault_code}}); - } else if (result.error_message.find("invalid") != std::string::npos) { - // Validation error from service - HandlerContext::send_error(res, StatusCode::BadRequest_400, ERR_INVALID_PARAMETER, "Invalid fault code", - {{"details", result.error_message}, {"fault_code", fault_code}}); - } else { - HandlerContext::send_error(res, StatusCode::ServiceUnavailable_503, ERR_SERVICE_UNAVAILABLE, - "Failed to get rosbag", - {{"details", result.error_message}, {"fault_code", fault_code}}); - } - return; - } - - // Get file path from result - std::string file_path = result.data["file_path"].get(); - std::string format = result.data["format"].get(); - - // Check if path is a directory (rosbag2 creates directories) - std::filesystem::path bag_path(file_path); - bool is_directory = std::filesystem::is_directory(bag_path); - std::string archive_path; // Will be set if we create a temp archive - - // Determine content type and filename based on what we're sending - std::string content_type; - std::string timestamp = generate_timestamp(); - std::string filename; - - if (is_directory) { - // Create tar.gz archive of the entire bag directory (includes all segments + metadata) - archive_path = std::filesystem::temp_directory_path().string() + "/rosbag_download_" + - std::to_string(std::chrono::steady_clock::now().time_since_epoch().count()) + ".tar.gz"; - - std::string tar_cmd = "tar -czf " + archive_path + " -C " + bag_path.parent_path().string() + " " + - bag_path.filename().string() + " 2>/dev/null"; - - int tar_result = std::system(tar_cmd.c_str()); - if (tar_result != 0) { - HandlerContext::send_error(res, StatusCode::InternalServerError_500, ERR_INTERNAL_ERROR, - "Failed to create rosbag archive", - {{"fault_code", fault_code}, {"path", file_path}}); - return; - } - - file_path = archive_path; - content_type = "application/gzip"; - filename = "fault_" + sanitize_filename(fault_code) + "_" + timestamp + ".tar.gz"; - } else { - // Single file - determine type from format - if (format == "mcap") { - content_type = "application/x-mcap"; - filename = "fault_" + sanitize_filename(fault_code) + "_" + timestamp + ".mcap"; - } else { - content_type = "application/octet-stream"; - filename = "fault_" + sanitize_filename(fault_code) + "_" + timestamp + ".db3"; - } - } - - // Check file exists and get size - std::error_code ec; - auto file_size = std::filesystem::file_size(file_path, ec); - if (ec) { - if (!archive_path.empty()) { - std::filesystem::remove(archive_path, ec); - } - HandlerContext::send_error(res, StatusCode::InternalServerError_500, ERR_INTERNAL_ERROR, - "Failed to read rosbag file", {{"fault_code", fault_code}, {"path", file_path}}); - return; - } - - res.set_header("Content-Disposition", "attachment; filename=\"" + filename + "\""); - res.set_header("Content-Type", content_type); - res.status = StatusCode::OK_200; - - // Use streaming response for large files to avoid loading entire bag into memory - std::string path_copy = file_path; // Capture for content provider lambda - std::string archive_copy = archive_path; // Capture for cleanup lambda - res.set_content_provider( - file_size, content_type, - [path_copy](size_t offset, size_t length, httplib::DataSink & sink) { - std::ifstream file(path_copy, std::ios::binary); - if (!file) { - return false; - } - file.seekg(static_cast(offset)); - constexpr size_t kChunkSize = 65536; // 64KB chunks - std::vector buffer(std::min(length, kChunkSize)); - size_t remaining = length; - while (remaining > 0 && file) { - size_t to_read = std::min(remaining, kChunkSize); - file.read(buffer.data(), static_cast(to_read)); - auto bytes_read = static_cast(file.gcount()); - if (bytes_read == 0) { - break; - } - sink.write(buffer.data(), bytes_read); - remaining -= bytes_read; - } - return true; - }, - [archive_copy](bool /*success*/) { - // Resource releaser callback - clean up temp archive if we created one - if (!archive_copy.empty()) { - std::error_code cleanup_ec; - std::filesystem::remove(archive_copy, cleanup_ec); - // Ignore errors - temp file cleanup is best-effort - } - }); - - } catch (const std::exception & e) { - HandlerContext::send_error(res, StatusCode::InternalServerError_500, ERR_INTERNAL_ERROR, - "Failed to download rosbag", {{"details", e.what()}, {"fault_code", fault_code}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_rosbag for fault '%s': %s", fault_code.c_str(), - e.what()); - } -} - } // namespace handlers } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp b/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp index 3267a817..c437164b 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp @@ -241,6 +241,9 @@ void HandlerContext::set_cors_headers(httplib::Response & res, const std::string res.set_header("Access-Control-Allow-Headers", cors_config_.headers_header); } + // Expose headers that JavaScript needs access to (e.g., for file downloads) + res.set_header("Access-Control-Expose-Headers", "Content-Disposition, Content-Length"); + // Set credentials header if enabled if (cors_config_.allow_credentials) { res.set_header("Access-Control-Allow-Credentials", "true"); diff --git a/src/ros2_medkit_gateway/src/http/rest_server.cpp b/src/ros2_medkit_gateway/src/http/rest_server.cpp index 5e9c5226..7bf73dc5 100644 --- a/src/ros2_medkit_gateway/src/http/rest_server.cpp +++ b/src/ros2_medkit_gateway/src/http/rest_server.cpp @@ -60,6 +60,7 @@ RESTServer::RESTServer(GatewayNode * node, const std::string & host, int port, c fault_handlers_ = std::make_unique(*handler_ctx_); auth_handlers_ = std::make_unique(*handler_ctx_); sse_fault_handler_ = std::make_unique(*handler_ctx_); + bulkdata_handlers_ = std::make_unique(*handler_ctx_); // Set up global error handlers for SOVD GenericError compliance setup_global_error_handlers(); @@ -434,11 +435,6 @@ void RESTServer::setup_routes() { fault_handlers_->handle_clear_all_faults(req, res); }); - srv->Get((api_path("/functions") + R"(/([^/]+)/faults/([^/]+)/snapshots$)"), - [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_component_snapshots(req, res); - }); - // Single function (capabilities) - must be after more specific routes srv->Get((api_path("/functions") + R"(/([^/]+)$)"), [this](const httplib::Request & req, httplib::Response & res) { discovery_handlers_->handle_get_function(req, res); @@ -568,11 +564,6 @@ void RESTServer::setup_routes() { fault_handlers_->handle_clear_all_faults(req, res); }); - srv->Get((api_path("/areas") + R"(/([^/]+)/faults/([^/]+)/snapshots$)"), - [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_component_snapshots(req, res); - }); - // Single area (capabilities) - must be after more specific routes srv->Get((api_path("/areas") + R"(/([^/]+)$)"), [this](const httplib::Request & req, httplib::Response & res) { discovery_handlers_->handle_get_area(req, res); @@ -754,29 +745,87 @@ void RESTServer::setup_routes() { fault_handlers_->handle_clear_all_faults(req, res); }); - // Snapshot endpoints for fault debugging - // GET /faults/{fault_code}/snapshots - system-wide snapshot access - srv->Get((api_path("/faults") + R"(/([^/]+)/snapshots$)"), + // === Bulk Data Routes (REQ_INTEROP_071-073) === + // List bulk-data categories + srv->Get((api_path("/apps") + R"(/([^/]+)/bulk-data$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_categories(req, res); + }); + srv->Get((api_path("/components") + R"(/([^/]+)/bulk-data$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_categories(req, res); + }); + srv->Get((api_path("/areas") + R"(/([^/]+)/bulk-data$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_categories(req, res); + }); + srv->Get((api_path("/functions") + R"(/([^/]+)/bulk-data$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_categories(req, res); + }); + + // List bulk-data descriptors (by category) + srv->Get((api_path("/apps") + R"(/([^/]+)/bulk-data/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_descriptors(req, res); + }); + srv->Get((api_path("/components") + R"(/([^/]+)/bulk-data/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_descriptors(req, res); + }); + srv->Get((api_path("/areas") + R"(/([^/]+)/bulk-data/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_descriptors(req, res); + }); + srv->Get((api_path("/functions") + R"(/([^/]+)/bulk-data/([^/]+)$)"), [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_snapshots(req, res); + bulkdata_handlers_->handle_list_descriptors(req, res); }); - // GET /faults/{fault_code}/snapshots/bag - download rosbag file for fault - srv->Get((api_path("/faults") + R"(/([^/]+)/snapshots/bag$)"), + // Download bulk-data file + srv->Get((api_path("/apps") + R"(/([^/]+)/bulk-data/([^/]+)/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_download(req, res); + }); + srv->Get((api_path("/components") + R"(/([^/]+)/bulk-data/([^/]+)/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_download(req, res); + }); + srv->Get((api_path("/areas") + R"(/([^/]+)/bulk-data/([^/]+)/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_download(req, res); + }); + srv->Get((api_path("/functions") + R"(/([^/]+)/bulk-data/([^/]+)/([^/]+)$)"), [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_rosbag(req, res); + bulkdata_handlers_->handle_download(req, res); }); - // GET /components/{component_id}/faults/{fault_code}/snapshots - component-scoped snapshot access - srv->Get((api_path("/components") + R"(/([^/]+)/faults/([^/]+)/snapshots$)"), + // Nested entities - subareas + srv->Get((api_path("/areas") + R"(/([^/]+)/subareas/([^/]+)/bulk-data$)"), [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_component_snapshots(req, res); + bulkdata_handlers_->handle_list_categories(req, res); + }); + srv->Get((api_path("/areas") + R"(/([^/]+)/subareas/([^/]+)/bulk-data/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_descriptors(req, res); + }); + srv->Get((api_path("/areas") + R"(/([^/]+)/subareas/([^/]+)/bulk-data/([^/]+)/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_download(req, res); }); - // GET /apps/{app_id}/faults/{fault_code}/snapshots - app-scoped snapshot access - srv->Get((api_path("/apps") + R"(/([^/]+)/faults/([^/]+)/snapshots$)"), + // Nested entities - subcomponents + srv->Get((api_path("/components") + R"(/([^/]+)/subcomponents/([^/]+)/bulk-data$)"), [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_component_snapshots(req, res); + bulkdata_handlers_->handle_list_categories(req, res); + }); + srv->Get((api_path("/components") + R"(/([^/]+)/subcomponents/([^/]+)/bulk-data/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_list_descriptors(req, res); + }); + srv->Get((api_path("/components") + R"(/([^/]+)/subcomponents/([^/]+)/bulk-data/([^/]+)/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + bulkdata_handlers_->handle_download(req, res); }); // Authentication endpoints (REQ_INTEROP_086, REQ_INTEROP_087) @@ -817,6 +866,9 @@ void RESTServer::set_cors_headers(httplib::Response & res, const std::string & o res.set_header("Access-Control-Allow-Headers", cors_config_.headers_header); } + // Expose headers that JavaScript needs access to (e.g., for file downloads) + res.set_header("Access-Control-Expose-Headers", "Content-Disposition, Content-Length"); + // Set credentials header if enabled if (cors_config_.allow_credentials) { res.set_header("Access-Control-Allow-Credentials", "true"); diff --git a/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp b/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp new file mode 100644 index 00000000..a5841e2a --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp @@ -0,0 +1,96 @@ +// Copyright 2026 bburda +// +// 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 "ros2_medkit_gateway/http/handlers/bulkdata_handlers.hpp" +#include "ros2_medkit_gateway/http/http_utils.hpp" + +using ros2_medkit_gateway::handlers::BulkDataHandlers; + +class BulkDataHandlersTest : public ::testing::Test { + protected: + void SetUp() override { + } + void TearDown() override { + } +}; + +// === MIME type tests === + +// @verifies REQ_INTEROP_071 +TEST_F(BulkDataHandlersTest, GetRosbagMimetypeMcap) { + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("mcap"), "application/x-mcap"); +} + +// @verifies REQ_INTEROP_071 +TEST_F(BulkDataHandlersTest, GetRosbagMimetypeSqlite3) { + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("sqlite3"), "application/x-sqlite3"); +} + +// @verifies REQ_INTEROP_071 +TEST_F(BulkDataHandlersTest, GetRosbagMimetypeDb3) { + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("db3"), "application/x-sqlite3"); +} + +// @verifies REQ_INTEROP_071 +TEST_F(BulkDataHandlersTest, GetRosbagMimetypeUnknown) { + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("unknown"), "application/octet-stream"); +} + +// @verifies REQ_INTEROP_071 +TEST_F(BulkDataHandlersTest, GetRosbagMimetypeEmpty) { + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype(""), "application/octet-stream"); +} + +// @verifies REQ_INTEROP_071 +TEST_F(BulkDataHandlersTest, GetRosbagMimetypeCasesSensitive) { + // MCAP should not match mcap (case sensitive) + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("MCAP"), "application/octet-stream"); + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("Mcap"), "application/octet-stream"); +} + +// === Shared timestamp utility tests === + +// @verifies REQ_INTEROP_013 +TEST_F(BulkDataHandlersTest, FormatTimestampNsValidTimestamp) { + // 2026-02-08T00:00:00.000Z + int64_t ns = 1770458400000000000; + auto result = ros2_medkit_gateway::format_timestamp_ns(ns); + EXPECT_TRUE(result.find("2026") != std::string::npos); + EXPECT_TRUE(result.find("T") != std::string::npos); + EXPECT_TRUE(result.find("Z") != std::string::npos); +} + +// @verifies REQ_INTEROP_013 +TEST_F(BulkDataHandlersTest, FormatTimestampNsEpoch) { + auto result = ros2_medkit_gateway::format_timestamp_ns(0); + EXPECT_EQ(result, "1970-01-01T00:00:00.000Z"); +} + +// @verifies REQ_INTEROP_013 +TEST_F(BulkDataHandlersTest, FormatTimestampNsWithMilliseconds) { + // 1 second + 123 ms + int64_t ns = 1'000'000'000 + 123'000'000; + auto result = ros2_medkit_gateway::format_timestamp_ns(ns); + EXPECT_TRUE(result.find(".123Z") != std::string::npos); +} + +// @verifies REQ_INTEROP_013 +TEST_F(BulkDataHandlersTest, FormatTimestampNsNegativeFallback) { + // Negative timestamps should return fallback + auto result = ros2_medkit_gateway::format_timestamp_ns(-1); + EXPECT_FALSE(result.empty()); + EXPECT_TRUE(result.find("Z") != std::string::npos); +} diff --git a/src/ros2_medkit_gateway/test/test_entity_path_utils.cpp b/src/ros2_medkit_gateway/test/test_entity_path_utils.cpp new file mode 100644 index 00000000..027dc566 --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_entity_path_utils.cpp @@ -0,0 +1,185 @@ +// Copyright 2026 bburda +// +// 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 "ros2_medkit_gateway/http/entity_path_utils.hpp" + +using namespace ros2_medkit_gateway; + +// ==================== parse_entity_path tests ==================== + +TEST(EntityPathUtilsTest, ParseTopLevelApp) { + auto info = parse_entity_path("/api/v1/apps/motor_controller/bulk-data/rosbags"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::APP); + EXPECT_EQ(info->entity_id, "motor_controller"); + EXPECT_EQ(info->resource_path, "/bulk-data/rosbags"); + EXPECT_EQ(info->entity_path, "/apps/motor_controller"); + EXPECT_TRUE(info->parent_id.empty()); + EXPECT_FALSE(info->is_nested); +} + +TEST(EntityPathUtilsTest, ParseTopLevelComponent) { + auto info = parse_entity_path("/api/v1/components/perception_lidar/faults/ERROR"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::COMPONENT); + EXPECT_EQ(info->entity_id, "perception_lidar"); + EXPECT_EQ(info->resource_path, "/faults/ERROR"); + EXPECT_EQ(info->entity_path, "/components/perception_lidar"); + EXPECT_TRUE(info->parent_id.empty()); + EXPECT_FALSE(info->is_nested); +} + +TEST(EntityPathUtilsTest, ParseTopLevelArea) { + auto info = parse_entity_path("/api/v1/areas/powertrain/data"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::AREA); + EXPECT_EQ(info->entity_id, "powertrain"); + EXPECT_EQ(info->resource_path, "/data"); + EXPECT_EQ(info->entity_path, "/areas/powertrain"); + EXPECT_TRUE(info->parent_id.empty()); + EXPECT_FALSE(info->is_nested); +} + +TEST(EntityPathUtilsTest, ParseTopLevelFunction) { + auto info = parse_entity_path("/api/v1/functions/motor_control/operations"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::FUNCTION); + EXPECT_EQ(info->entity_id, "motor_control"); + EXPECT_EQ(info->resource_path, "/operations"); + EXPECT_EQ(info->entity_path, "/functions/motor_control"); + EXPECT_TRUE(info->parent_id.empty()); + EXPECT_FALSE(info->is_nested); +} + +TEST(EntityPathUtilsTest, ParseNestedSubarea) { + auto info = parse_entity_path("/api/v1/areas/perception/subareas/lidar/faults/LIDAR_ERROR"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::AREA); + EXPECT_EQ(info->entity_id, "lidar"); + EXPECT_EQ(info->resource_path, "/faults/LIDAR_ERROR"); + EXPECT_EQ(info->entity_path, "/areas/perception/subareas/lidar"); + EXPECT_EQ(info->parent_id, "perception"); + EXPECT_TRUE(info->is_nested); +} + +TEST(EntityPathUtilsTest, ParseNestedSubcomponent) { + auto info = parse_entity_path("/api/v1/components/ecu/subcomponents/sensor_hub/data"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::COMPONENT); + EXPECT_EQ(info->entity_id, "sensor_hub"); + EXPECT_EQ(info->resource_path, "/data"); + EXPECT_EQ(info->entity_path, "/components/ecu/subcomponents/sensor_hub"); + EXPECT_EQ(info->parent_id, "ecu"); + EXPECT_TRUE(info->is_nested); +} + +TEST(EntityPathUtilsTest, ParseEntityWithNoResourcePath) { + auto info = parse_entity_path("/api/v1/apps/motor_controller"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::APP); + EXPECT_EQ(info->entity_id, "motor_controller"); + EXPECT_TRUE(info->resource_path.empty()); + EXPECT_EQ(info->entity_path, "/apps/motor_controller"); +} + +TEST(EntityPathUtilsTest, ParseInvalidPath) { + auto info = parse_entity_path("/api/v1/invalid/path"); + EXPECT_FALSE(info.has_value()); +} + +TEST(EntityPathUtilsTest, ParseEmptyPath) { + auto info = parse_entity_path(""); + EXPECT_FALSE(info.has_value()); +} + +TEST(EntityPathUtilsTest, ParsePathWithoutApiPrefix) { + // Should not match without /api/v1 prefix + auto info = parse_entity_path("/apps/motor_controller/faults"); + EXPECT_FALSE(info.has_value()); +} + +TEST(EntityPathUtilsTest, ParsePathWithTrailingSlash) { + auto info = parse_entity_path("/api/v1/apps/motor_controller/"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::APP); + EXPECT_EQ(info->entity_id, "motor_controller"); + EXPECT_EQ(info->resource_path, "/"); +} + +// ==================== extract_bulk_data_category tests ==================== + +TEST(EntityPathUtilsTest, ExtractCategoryFromBulkDataPath) { + EXPECT_EQ(extract_bulk_data_category("/bulk-data/rosbags/abc-123"), "rosbags"); + EXPECT_EQ(extract_bulk_data_category("/bulk-data/snapshots/xyz"), "snapshots"); +} + +TEST(EntityPathUtilsTest, ExtractCategoryFromFullPath) { + EXPECT_EQ(extract_bulk_data_category("/api/v1/apps/motor/bulk-data/rosbags/uuid"), "rosbags"); +} + +TEST(EntityPathUtilsTest, ExtractCategoryEmpty) { + EXPECT_EQ(extract_bulk_data_category("/api/v1/apps/motor/faults"), ""); + EXPECT_EQ(extract_bulk_data_category(""), ""); +} + +// ==================== extract_bulk_data_id tests ==================== + +TEST(EntityPathUtilsTest, ExtractBulkDataIdFromPath) { + EXPECT_EQ(extract_bulk_data_id("/bulk-data/rosbags/550e8400-e29b-41d4-a716-446655440000"), + "550e8400-e29b-41d4-a716-446655440000"); +} + +TEST(EntityPathUtilsTest, ExtractBulkDataIdFromFullPath) { + EXPECT_EQ(extract_bulk_data_id("/api/v1/apps/motor/bulk-data/snapshots/my-snapshot-id"), "my-snapshot-id"); +} + +TEST(EntityPathUtilsTest, ExtractBulkDataIdEmpty) { + EXPECT_EQ(extract_bulk_data_id("/bulk-data/rosbags"), ""); // No ID after category + EXPECT_EQ(extract_bulk_data_id("/api/v1/apps/motor"), ""); // No bulk-data segment + EXPECT_EQ(extract_bulk_data_id(""), ""); +} + +// ==================== Complex path scenarios ==================== + +TEST(EntityPathUtilsTest, ParseAppWithMultipleResourceSegments) { + auto info = parse_entity_path("/api/v1/apps/motor_controller/faults/MOTOR_ERR/snapshots"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::APP); + EXPECT_EQ(info->entity_id, "motor_controller"); + EXPECT_EQ(info->resource_path, "/faults/MOTOR_ERR/snapshots"); +} + +TEST(EntityPathUtilsTest, ParseComponentWithBulkData) { + auto info = parse_entity_path("/api/v1/components/sensor_hub/bulk-data/rosbags/uuid-123"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::COMPONENT); + EXPECT_EQ(info->entity_id, "sensor_hub"); + EXPECT_EQ(info->resource_path, "/bulk-data/rosbags/uuid-123"); + + // Verify bulk-data extraction still works on resource_path + EXPECT_EQ(extract_bulk_data_category(info->resource_path), "rosbags"); + EXPECT_EQ(extract_bulk_data_id(info->resource_path), "uuid-123"); +} + +TEST(EntityPathUtilsTest, ParseNestedSubareaWithBulkData) { + auto info = parse_entity_path("/api/v1/areas/perception/subareas/camera/bulk-data/snapshots/snap-001"); + ASSERT_TRUE(info.has_value()); + EXPECT_EQ(info->type, SovdEntityType::AREA); + EXPECT_EQ(info->entity_id, "camera"); + EXPECT_EQ(info->parent_id, "perception"); + EXPECT_TRUE(info->is_nested); + EXPECT_EQ(info->resource_path, "/bulk-data/snapshots/snap-001"); +} diff --git a/src/ros2_medkit_gateway/test/test_fault_handlers.cpp b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp new file mode 100644 index 00000000..f01ea6b9 --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp @@ -0,0 +1,351 @@ +// Copyright 2026 bburda +// +// 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 "ros2_medkit_gateway/http/handlers/fault_handlers.hpp" +#include "ros2_medkit_msgs/msg/environment_data.hpp" +#include "ros2_medkit_msgs/msg/extended_data_records.hpp" +#include "ros2_medkit_msgs/msg/fault.hpp" +#include "ros2_medkit_msgs/msg/snapshot.hpp" + +using json = nlohmann::json; +using ros2_medkit_gateway::handlers::FaultHandlers; + +class FaultHandlersTest : public ::testing::Test { + protected: + void SetUp() override { + } + void TearDown() override { + } +}; + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseBasicFields) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "TEST_FAULT"; + fault.description = "Test fault description"; + fault.severity = 3; // ERROR + fault.status = "CONFIRMED"; // Active/confirmed fault + fault.occurrence_count = 5; + fault.reporting_sources = {"/powertrain/motor_controller"}; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + env_data.extended_data_records.first_occurrence_ns = 1707044400000000000; + env_data.extended_data_records.last_occurrence_ns = 1707044460000000000; + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/motor_controller"); + + // Verify item structure + EXPECT_EQ(response["item"]["code"], "TEST_FAULT"); + EXPECT_EQ(response["item"]["fault_name"], "Test fault description"); + EXPECT_EQ(response["item"]["severity"], 3); + + // Verify status object + auto status = response["item"]["status"]; + EXPECT_EQ(status["aggregatedStatus"], "active"); + EXPECT_EQ(status["testFailed"], "1"); + EXPECT_EQ(status["confirmedDTC"], "1"); + EXPECT_EQ(status["pendingDTC"], "0"); + + // Verify x-medkit + EXPECT_EQ(response["x-medkit"]["occurrence_count"], 5); + EXPECT_EQ(response["x-medkit"]["severity_label"], "ERROR"); + EXPECT_EQ(response["x-medkit"]["status_raw"], + "CONFIRMED"); // Raw status in x-medkit, not in status object +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseWithFreezeFrame) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "TEMP_FAULT"; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + + ros2_medkit_msgs::msg::Snapshot freeze_frame; + freeze_frame.type = "freeze_frame"; + freeze_frame.name = "temperature"; + freeze_frame.data = R"({"temperature": 85.5, "variance": 0.1})"; + freeze_frame.topic = "/motor/temperature"; + freeze_frame.message_type = "sensor_msgs/msg/Temperature"; + freeze_frame.captured_at_ns = 1707044400000000000; + env_data.snapshots.push_back(freeze_frame); + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/motor"); + + auto & snap = response["environment_data"]["snapshots"][0]; + EXPECT_EQ(snap["type"], "freeze_frame"); + EXPECT_EQ(snap["name"], "temperature"); + EXPECT_DOUBLE_EQ(snap["data"].get(), 85.5); // Primary value extracted + EXPECT_EQ(snap["x-medkit"]["topic"], "/motor/temperature"); + EXPECT_EQ(snap["x-medkit"]["message_type"], "sensor_msgs/msg/Temperature"); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseWithRosbag) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "ROSBAG_FAULT"; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + + ros2_medkit_msgs::msg::Snapshot rosbag; + rosbag.type = "rosbag"; + rosbag.name = "fault_recording"; + rosbag.size_bytes = 1234567; + rosbag.duration_sec = 6.0; + rosbag.format = "mcap"; + env_data.snapshots.push_back(rosbag); + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/motor_controller"); + + auto & snap = response["environment_data"]["snapshots"][0]; + EXPECT_EQ(snap["type"], "rosbag"); + EXPECT_EQ(snap["bulk_data_uri"], "/apps/motor_controller/bulk-data/rosbags/ROSBAG_FAULT"); + EXPECT_EQ(snap["size_bytes"], 1234567); + EXPECT_DOUBLE_EQ(snap["duration_sec"], 6.0); + EXPECT_EQ(snap["format"], "mcap"); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseNestedEntityPath) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "NESTED_FAULT"; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + ros2_medkit_msgs::msg::Snapshot rosbag; + rosbag.type = "rosbag"; + rosbag.bulk_data_id = "NESTED_FAULT"; + env_data.snapshots.push_back(rosbag); + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/areas/perception/subareas/lidar"); + + auto & snap = response["environment_data"]["snapshots"][0]; + EXPECT_EQ(snap["bulk_data_uri"], "/areas/perception/subareas/lidar/bulk-data/rosbags/NESTED_FAULT"); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseStatusCleared) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "CLEARED_FAULT"; + fault.status = "CLEARED"; // Cleared status + + ros2_medkit_msgs::msg::EnvironmentData env_data; + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + + auto status = response["item"]["status"]; + EXPECT_EQ(status["aggregatedStatus"], "cleared"); + EXPECT_EQ(status["testFailed"], "0"); + EXPECT_EQ(status["confirmedDTC"], "0"); + EXPECT_EQ(status["pendingDTC"], "0"); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseStatusPassive) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "PASSIVE_FAULT"; + fault.status = "PREFAILED"; // Pending/passive status + + ros2_medkit_msgs::msg::EnvironmentData env_data; + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + + auto status = response["item"]["status"]; + EXPECT_EQ(status["aggregatedStatus"], "passive"); + EXPECT_EQ(status["pendingDTC"], "1"); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseSeverityLabels) { + ros2_medkit_msgs::msg::EnvironmentData env_data; + + // Test DEBUG (0) + { + ros2_medkit_msgs::msg::Fault fault; + fault.severity = 0; + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + EXPECT_EQ(response["x-medkit"]["severity_label"], "DEBUG"); + } + // Test INFO (1) + { + ros2_medkit_msgs::msg::Fault fault; + fault.severity = 1; + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + EXPECT_EQ(response["x-medkit"]["severity_label"], "INFO"); + } + // Test WARN (2) + { + ros2_medkit_msgs::msg::Fault fault; + fault.severity = 2; + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + EXPECT_EQ(response["x-medkit"]["severity_label"], "WARN"); + } + // Test ERROR (3) + { + ros2_medkit_msgs::msg::Fault fault; + fault.severity = 3; + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + EXPECT_EQ(response["x-medkit"]["severity_label"], "ERROR"); + } + // Test FATAL (4) + { + ros2_medkit_msgs::msg::Fault fault; + fault.severity = 4; + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + EXPECT_EQ(response["x-medkit"]["severity_label"], "FATAL"); + } + // Test UNKNOWN (255) + { + ros2_medkit_msgs::msg::Fault fault; + fault.severity = 255; + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + EXPECT_EQ(response["x-medkit"]["severity_label"], "UNKNOWN"); + } +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseWithInvalidJson) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "INVALID_JSON_FAULT"; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + ros2_medkit_msgs::msg::Snapshot freeze_frame; + freeze_frame.type = "freeze_frame"; + freeze_frame.name = "invalid_data"; + freeze_frame.data = "not valid json {"; + freeze_frame.topic = "/test"; + freeze_frame.message_type = "std_msgs/msg/String"; + env_data.snapshots.push_back(freeze_frame); + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + + auto & snap = response["environment_data"]["snapshots"][0]; + EXPECT_EQ(snap["data"], "not valid json {"); // Raw data returned + EXPECT_TRUE(snap["x-medkit"].contains("parse_error")); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseExtendedDataRecords) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "TEST_FAULT"; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + env_data.extended_data_records.first_occurrence_ns = 1770458400000000000; // 2026-02-08 + env_data.extended_data_records.last_occurrence_ns = 1770458460000000000; + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + + auto & edr = response["environment_data"]["extended_data_records"]; + std::string first = edr["first_occurrence"].get(); + std::string last = edr["last_occurrence"].get(); + + // Verify ISO 8601 format with milliseconds and Z suffix + EXPECT_TRUE(first.find("2026") != std::string::npos); + EXPECT_TRUE(first.find("Z") != std::string::npos); + EXPECT_TRUE(first.find("T") != std::string::npos); + EXPECT_TRUE(last.find("Z") != std::string::npos); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponsePrimaryValueExtraction) { + ros2_medkit_msgs::msg::EnvironmentData env_data; + + // Test std_msgs/msg/Float64 - should extract "data" field + { + ros2_medkit_msgs::msg::Fault fault; + ros2_medkit_msgs::msg::Snapshot snap; + snap.type = "freeze_frame"; + snap.data = R"({"data": 42.5})"; + snap.message_type = "std_msgs/msg/Float64"; + env_data.snapshots.clear(); + env_data.snapshots.push_back(snap); + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + EXPECT_DOUBLE_EQ(response["environment_data"]["snapshots"][0]["data"].get(), 42.5); + } + + // Test unknown message type - should return full data + { + ros2_medkit_msgs::msg::Fault fault; + ros2_medkit_msgs::msg::Snapshot snap; + snap.type = "freeze_frame"; + snap.data = R"({"foo": "bar", "baz": 123})"; + snap.message_type = "custom_msgs/msg/Unknown"; + env_data.snapshots.clear(); + env_data.snapshots.push_back(snap); + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto data = response["environment_data"]["snapshots"][0]["data"]; + EXPECT_EQ(data["foo"], "bar"); + EXPECT_EQ(data["baz"], 123); + } +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseMultipleSources) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "MULTI_SOURCE_FAULT"; + fault.reporting_sources = {"/perception/lidar", "/perception/camera", "/control/motor"}; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + + auto sources = response["x-medkit"]["reporting_sources"]; + ASSERT_EQ(sources.size(), 3); + EXPECT_EQ(sources[0], "/perception/lidar"); + EXPECT_EQ(sources[1], "/perception/camera"); + EXPECT_EQ(sources[2], "/control/motor"); +} + +// @verifies REQ_INTEROP_013 +TEST_F(FaultHandlersTest, BuildSovdFaultResponseMixedSnapshots) { + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = "MIXED_FAULT"; + + ros2_medkit_msgs::msg::EnvironmentData env_data; + + // Add freeze frame + ros2_medkit_msgs::msg::Snapshot freeze_frame; + freeze_frame.type = "freeze_frame"; + freeze_frame.name = "temperature"; + freeze_frame.data = R"({"temperature": 75.0})"; + freeze_frame.message_type = "sensor_msgs/msg/Temperature"; + env_data.snapshots.push_back(freeze_frame); + + // Add rosbag + ros2_medkit_msgs::msg::Snapshot rosbag; + rosbag.type = "rosbag"; + rosbag.name = "recording"; + rosbag.bulk_data_id = "MIXED_FAULT"; + rosbag.size_bytes = 1000; + rosbag.format = "mcap"; + env_data.snapshots.push_back(rosbag); + + auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/components/motor"); + + ASSERT_EQ(response["environment_data"]["snapshots"].size(), 2); + + // Verify freeze frame + auto & snap0 = response["environment_data"]["snapshots"][0]; + EXPECT_EQ(snap0["type"], "freeze_frame"); + EXPECT_DOUBLE_EQ(snap0["data"].get(), 75.0); + + // Verify rosbag + auto & snap1 = response["environment_data"]["snapshots"][1]; + EXPECT_EQ(snap1["type"], "rosbag"); + EXPECT_EQ(snap1["bulk_data_uri"], "/components/motor/bulk-data/rosbags/MIXED_FAULT"); +} diff --git a/src/ros2_medkit_gateway/test/test_handler_context.cpp b/src/ros2_medkit_gateway/test/test_handler_context.cpp index fa3370d7..a823a5e0 100644 --- a/src/ros2_medkit_gateway/test/test_handler_context.cpp +++ b/src/ros2_medkit_gateway/test/test_handler_context.cpp @@ -171,6 +171,7 @@ TEST_F(HandlerContextCorsTest, SetCorsHeadersSetsAllHeaders) { EXPECT_EQ(res.get_header_value("Access-Control-Allow-Methods"), "GET, POST, PUT, DELETE"); EXPECT_EQ(res.get_header_value("Access-Control-Allow-Headers"), "Content-Type, Authorization"); EXPECT_EQ(res.get_header_value("Access-Control-Allow-Credentials"), "true"); + EXPECT_EQ(res.get_header_value("Access-Control-Expose-Headers"), "Content-Disposition, Content-Length"); } TEST_F(HandlerContextCorsTest, SetCorsHeadersWithoutCredentials) { diff --git a/src/ros2_medkit_gateway/test/test_integration.test.py b/src/ros2_medkit_gateway/test/test_integration.test.py index 239594d0..023f3169 100644 --- a/src/ros2_medkit_gateway/test/test_integration.test.py +++ b/src/ros2_medkit_gateway/test/test_integration.test.py @@ -2486,74 +2486,58 @@ def test_65_root_endpoint_includes_snapshots(self): def test_66_get_snapshots_nonexistent_fault(self): """ - Test GET /faults/{fault_code}/snapshots returns 404 for unknown fault. + Test GET /faults/{fault_code}/snapshots returns 404 (endpoint removed). - @verifies REQ_INTEROP_088 + Legacy snapshot endpoints have been removed in favor of: + - Inline snapshots in fault response (environment_data.snapshots) + - Bulk-data endpoint for rosbag downloads """ response = requests.get( f'{self.BASE_URL}/faults/NONEXISTENT_FAULT_CODE/snapshots', timeout=10 ) + # Endpoint was removed, should return 404 self.assertEqual(response.status_code, 404) - data = response.json() - self.assertIn('error_code', data) - self.assertIn('parameters', data) - self.assertIn('fault_code', data['parameters']) - self.assertEqual(data['parameters'].get('fault_code'), 'NONEXISTENT_FAULT_CODE') - - print('✓ Get snapshots nonexistent fault test passed') + print('✓ Legacy snapshots endpoint returns 404 (removed)') def test_67_get_component_snapshots_nonexistent_fault(self): """ - Test GET /apps/{id}/faults/{code}/snapshots returns 404 for unknown fault. + Test GET /apps/{id}/faults/{code}/snapshots returns 404 (endpoint removed). - @verifies REQ_INTEROP_088 + Legacy snapshot endpoints have been removed. """ response = requests.get( f'{self.BASE_URL}/apps/temp_sensor/faults/NONEXISTENT_FAULT/snapshots', timeout=10 ) + # Endpoint was removed, should return 404 self.assertEqual(response.status_code, 404) - data = response.json() - self.assertIn('error_code', data) - self.assertIn('parameters', data) - self.assertIn('app_id', data['parameters']) - self.assertEqual(data['parameters'].get('app_id'), 'temp_sensor') - self.assertIn('fault_code', data['parameters']) - self.assertEqual(data['parameters'].get('fault_code'), 'NONEXISTENT_FAULT') - - print('✓ Get app snapshots nonexistent fault test passed') + print('✓ Legacy app snapshots endpoint returns 404 (removed)') def test_68_get_snapshots_nonexistent_component(self): """ - Test GET /components/{id}/faults/{code}/snapshots returns 404 for unknown entity. + Test GET /components/{id}/faults/{code}/snapshots returns 404 (endpoint removed). - @verifies REQ_INTEROP_088 + Legacy snapshot endpoints have been removed. """ response = requests.get( f'{self.BASE_URL}/components/nonexistent_component/faults/ANY_FAULT/snapshots', timeout=10 ) + # Endpoint was removed, should return 404 self.assertEqual(response.status_code, 404) - data = response.json() - self.assertIn('error_code', data) - self.assertEqual(data['message'], 'Entity not found') - self.assertIn('parameters', data) - self.assertIn('entity_id', data['parameters']) - self.assertEqual(data['parameters'].get('entity_id'), 'nonexistent_component') - - print('✓ Get snapshots nonexistent entity test passed') + print('✓ Legacy component snapshots endpoint returns 404 (removed)') def test_69_get_snapshots_invalid_component_id(self): """ - Test GET /components/{id}/faults/{code}/snapshots returns 400 for invalid entity ID. + Test GET /components/{id}/faults/{code}/snapshots returns 404 (endpoint removed). - @verifies REQ_INTEROP_088 + Legacy snapshot endpoints have been removed. """ - # Note: hyphens are now allowed in IDs (for manifest entity IDs like 'engine-ecu') + # Note: endpoint removed, so any ID should return 404 invalid_ids = [ 'component;drop', 'component