From 5a7f51b1bdbf319d904c7c54f35dfe2159a594c3 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 07:19:42 +0000 Subject: [PATCH 01/27] feat(msgs): add EnvironmentData, Snapshot, GetFault, and ListFaultsForEntity definitions Add new ROS2 message definitions for SOVD-compliant fault environment data: - ExtendedDataRecords.msg: first/last occurrence timestamps - Snapshot.msg: unified freeze frame and rosbag snapshot type - EnvironmentData.msg: container for snapshots in fault response - GetFault.srv: NEW service for single fault lookup with environment_data - ListFaultsForEntity.srv: service to query faults by entity --- src/ros2_medkit_msgs/CMakeLists.txt | 5 ++ src/ros2_medkit_msgs/msg/EnvironmentData.msg | 24 +++++++ .../msg/ExtendedDataRecords.msg | 24 +++++++ src/ros2_medkit_msgs/msg/Snapshot.msg | 63 +++++++++++++++++++ src/ros2_medkit_msgs/srv/GetFault.srv | 38 +++++++++++ .../srv/ListFaultsForEntity.srv | 34 ++++++++++ 6 files changed, 188 insertions(+) create mode 100644 src/ros2_medkit_msgs/msg/EnvironmentData.msg create mode 100644 src/ros2_medkit_msgs/msg/ExtendedDataRecords.msg create mode 100644 src/ros2_medkit_msgs/msg/Snapshot.msg create mode 100644 src/ros2_medkit_msgs/srv/GetFault.srv create mode 100644 src/ros2_medkit_msgs/srv/ListFaultsForEntity.srv diff --git a/src/ros2_medkit_msgs/CMakeLists.txt b/src/ros2_medkit_msgs/CMakeLists.txt index d2a042f2..e0985c2a 100644 --- a/src/ros2_medkit_msgs/CMakeLists.txt +++ b/src/ros2_medkit_msgs/CMakeLists.txt @@ -28,11 +28,16 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/FaultEvent.msg" "msg/MutedFaultInfo.msg" "msg/ClusterInfo.msg" + "msg/ExtendedDataRecords.msg" + "msg/Snapshot.msg" + "msg/EnvironmentData.msg" "srv/ReportFault.srv" "srv/GetFaults.srv" + "srv/GetFault.srv" "srv/ClearFault.srv" "srv/GetSnapshots.srv" "srv/GetRosbag.srv" + "srv/ListFaultsForEntity.srv" DEPENDENCIES builtin_interfaces ) diff --git a/src/ros2_medkit_msgs/msg/EnvironmentData.msg b/src/ros2_medkit_msgs/msg/EnvironmentData.msg new file mode 100644 index 00000000..fa588846 --- /dev/null +++ b/src/ros2_medkit_msgs/msg/EnvironmentData.msg @@ -0,0 +1,24 @@ +# Copyright 2025 mfaferek93 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# EnvironmentData.msg - SOVD-compliant environment data for faults. +# +# Contains extended data records and snapshots (freeze frames + rosbag metadata) +# for fault diagnostic responses. Used in GetFault service response. + +# Extended data records with occurrence timestamps +ExtendedDataRecords extended_data_records + +# Array of snapshots (freeze frames and/or rosbag metadata) +Snapshot[] snapshots diff --git a/src/ros2_medkit_msgs/msg/ExtendedDataRecords.msg b/src/ros2_medkit_msgs/msg/ExtendedDataRecords.msg new file mode 100644 index 00000000..1b35b1ae --- /dev/null +++ b/src/ros2_medkit_msgs/msg/ExtendedDataRecords.msg @@ -0,0 +1,24 @@ +# Copyright 2025 mfaferek93 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# ExtendedDataRecords.msg - Extended data records for fault diagnostics. +# +# Contains timestamps of first and last occurrence for SOVD-compliant +# fault environment data responses. + +# Timestamp of first occurrence (nanoseconds since epoch) +int64 first_occurence_ns + +# Timestamp of last occurrence (nanoseconds since epoch) +int64 last_occurence_ns diff --git a/src/ros2_medkit_msgs/msg/Snapshot.msg b/src/ros2_medkit_msgs/msg/Snapshot.msg new file mode 100644 index 00000000..141a13d4 --- /dev/null +++ b/src/ros2_medkit_msgs/msg/Snapshot.msg @@ -0,0 +1,63 @@ +# Copyright 2025 mfaferek93 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Snapshot.msg - Unified snapshot type for freeze frames and rosbag recordings. +# +# Used in EnvironmentData to represent captured diagnostic data. Supports two types: +# - "freeze_frame": Single message capture with JSON-serialized data +# - "rosbag": Recording file metadata with bulk-data download reference + +# Snapshot type: "freeze_frame" or "rosbag" +string type + +# Human-readable name for this snapshot +string name + +# --- Freeze frame fields (empty for rosbag type) --- + +# JSON-serialized message data +string data + +# ROS topic name (e.g., "/motor/temperature") +string topic + +# Message type (e.g., "sensor_msgs/msg/Temperature") +string message_type + +# --- Rosbag fields (empty for freeze_frame type) --- + +# UUID - globally unique identifier for bulk-data download +string bulk_data_id + +# File size in bytes +uint64 size_bytes + +# Recording duration in seconds +float64 duration_sec + +# Recording format: "sqlite3" or "mcap" +string format + +# --- Common metadata --- + +# Capture timestamp (nanoseconds since epoch) +int64 captured_at_ns + +# Snapshot type constants +string TYPE_FREEZE_FRAME = "freeze_frame" +string TYPE_ROSBAG = "rosbag" + +# Rosbag format constants +string FORMAT_SQLITE3 = "sqlite3" +string FORMAT_MCAP = "mcap" diff --git a/src/ros2_medkit_msgs/srv/GetFault.srv b/src/ros2_medkit_msgs/srv/GetFault.srv new file mode 100644 index 00000000..d8700580 --- /dev/null +++ b/src/ros2_medkit_msgs/srv/GetFault.srv @@ -0,0 +1,38 @@ +# Copyright 2025 mfaferek93 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# GetFault.srv - Get single fault by code with environment data. +# +# Unlike GetFaults.srv (plural) which returns a list of faults for filtering, +# this service returns a single fault with full environment data including +# snapshots and extended data records for SOVD-compliant fault responses. + +# Request fields + +# Fault code to retrieve (e.g., "MOTOR_OVERHEAT") +string fault_code +--- +# Response fields + +# Whether the request was successful +bool success + +# Error message if success is false +string error_message + +# Fault details (empty if not found) +Fault fault + +# Environment data with snapshots and extended records +EnvironmentData environment_data diff --git a/src/ros2_medkit_msgs/srv/ListFaultsForEntity.srv b/src/ros2_medkit_msgs/srv/ListFaultsForEntity.srv new file mode 100644 index 00000000..c58bdcc1 --- /dev/null +++ b/src/ros2_medkit_msgs/srv/ListFaultsForEntity.srv @@ -0,0 +1,34 @@ +# Copyright 2025 mfaferek93 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# ListFaultsForEntity.srv - Request list of faults for a specific entity. +# +# Filters faults by checking if entity FQN appears in fault.reporting_sources[]. +# Used by the gateway to serve entity-specific fault lists. + +# Request fields + +# Entity ID (app, component, area, etc.) +string entity_id +--- +# Response fields + +# Whether the request was successful +bool success + +# Error message if success is false +string error_message + +# Faults where reporting_sources contains entity FQN +Fault[] faults From 9fb17cbd2e3d1b73a2451da1454b1fa21915dd95 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 07:26:34 +0000 Subject: [PATCH 02/27] feat(fault-manager): add UUID generation and rosbag index Add UUID generation for rosbag files and index for UUID-based lookup: - FaultStorage::generate_uuid() for UUID v4 generation - RosbagFileInfo::bulk_data_id field for globally unique identifier - FaultStorage::get_rosbag_by_id() for lookup by UUID - FaultStorage::get_rosbag_path() for file path retrieval - FaultStorage::get_rosbags_for_entity() for entity-filtered queries - rosbag_by_id_ index for O(1) UUID lookups in InMemoryFaultStorage - SQLite schema migration for existing databases Required for SOVD bulk-data download pattern. @verifies REQ_INTEROP_073 --- .../fault_storage.hpp | 26 ++- .../sqlite_fault_storage.hpp | 3 + .../src/fault_storage.cpp | 84 +++++++- .../src/rosbag_capture.cpp | 2 + .../src/sqlite_fault_storage.cpp | 156 ++++++++++++--- .../test/test_sqlite_storage.cpp | 182 ++++++++++++++++++ 6 files changed, 426 insertions(+), 27 deletions(-) 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..b7093711 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 @@ -84,6 +84,7 @@ struct SnapshotData { /// Rosbag file metadata for time-window recording struct RosbagFileInfo { + std::string bulk_data_id; ///< UUID - globally unique identifier for bulk-data download std::string fault_code; std::string file_path; std::string format; ///< "sqlite3" or "mcap" @@ -178,6 +179,25 @@ class FaultStorage { /// @return Vector of rosbag file info virtual std::vector get_all_rosbag_files() const = 0; + /// Get rosbag by bulk_data_id (UUID) + /// @param bulk_data_id The UUID to look up + /// @return Rosbag file info if exists, nullopt otherwise + virtual std::optional get_rosbag_by_id(const std::string & bulk_data_id) const = 0; + + /// Get file path for a rosbag by bulk_data_id + /// @param bulk_data_id The UUID to look up + /// @return File path if exists, empty string otherwise + virtual std::string get_rosbag_path(const std::string & bulk_data_id) 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 get_rosbags_for_entity(const std::string & entity_fqn) const = 0; + + /// Generate a UUID v4 string + /// @return A new UUID in format "xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx" + static std::string generate_uuid(); + protected: FaultStorage() = default; FaultStorage(const FaultStorage &) = default; @@ -220,6 +240,9 @@ 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::optional get_rosbag_by_id(const std::string & bulk_data_id) const override; + std::string get_rosbag_path(const std::string & bulk_data_id) const override; + std::vector get_rosbags_for_entity(const std::string & entity_fqn) const override; private: /// Update fault status based on debounce counter @@ -228,7 +251,8 @@ 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 + std::map rosbag_by_id_; ///< bulk_data_id -> fault_code (index) 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..471336ef 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 @@ -70,6 +70,9 @@ 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::optional get_rosbag_by_id(const std::string & bulk_data_id) const override; + std::string get_rosbag_path(const std::string & bulk_data_id) const override; + std::vector get_rosbags_for_entity(const std::string & entity_fqn) const override; /// Get the database path const std::string & db_path() const { diff --git a/src/ros2_medkit_fault_manager/src/fault_storage.cpp b/src/ros2_medkit_fault_manager/src/fault_storage.cpp index dce066b0..2a47638f 100644 --- a/src/ros2_medkit_fault_manager/src/fault_storage.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_storage.cpp @@ -16,9 +16,31 @@ #include #include +#include +#include +#include namespace ros2_medkit_fault_manager { +std::string FaultStorage::generate_uuid() { + static std::random_device rd; + static std::mt19937 gen(rd()); + static std::uniform_int_distribution dis; + + std::stringstream ss; + ss << std::hex << std::setfill('0'); + + // xxxxxxxx-xxxx-4xxx-Nxxx-xxxxxxxxxxxx (UUID v4 format) + ss << std::setw(8) << dis(gen) << '-'; + ss << std::setw(4) << (dis(gen) & 0xFFFF) << '-'; + ss << std::setw(4) << ((dis(gen) & 0x0FFF) | 0x4000) << '-'; // Version 4 + ss << std::setw(4) << ((dis(gen) & 0x3FFF) | 0x8000) << '-'; // Variant 1 + ss << std::setw(8) << dis(gen); + ss << std::setw(4) << (dis(gen) & 0xFFFF); + + return ss.str(); +} + ros2_medkit_msgs::msg::Fault FaultState::to_msg() const { ros2_medkit_msgs::msg::Fault msg; msg.fault_code = fault_code; @@ -315,13 +337,21 @@ 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()) { + // Remove old entry from UUID index + rosbag_by_id_.erase(it->second.bulk_data_id); + 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; + // Update UUID index + if (!info.bulk_data_id.empty()) { + rosbag_by_id_[info.bulk_data_id] = info.fault_code; + } } std::optional InMemoryFaultStorage::get_rosbag_file(const std::string & fault_code) const { @@ -342,6 +372,9 @@ bool InMemoryFaultStorage::delete_rosbag_file(const std::string & fault_code) { return false; } + // Remove from UUID index + rosbag_by_id_.erase(it->second.bulk_data_id); + // Try to delete the actual file std::error_code ec; std::filesystem::remove_all(it->second.file_path, ec); @@ -378,4 +411,47 @@ std::vector InMemoryFaultStorage::get_all_rosbag_files() const { return result; } +std::optional InMemoryFaultStorage::get_rosbag_by_id(const std::string & bulk_data_id) const { + std::lock_guard lock(mutex_); + + auto it = rosbag_by_id_.find(bulk_data_id); + if (it == rosbag_by_id_.end()) { + return std::nullopt; + } + + auto rosbag_it = rosbag_files_.find(it->second); + if (rosbag_it == rosbag_files_.end()) { + return std::nullopt; + } + + return rosbag_it->second; +} + +std::string InMemoryFaultStorage::get_rosbag_path(const std::string & bulk_data_id) const { + auto rosbag = get_rosbag_by_id(bulk_data_id); + if (rosbag) { + return rosbag->file_path; + } + return ""; +} + +std::vector InMemoryFaultStorage::get_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; +} + } // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp b/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp index 5f980168..305f44ab 100644 --- a/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp +++ b/src/ros2_medkit_fault_manager/src/rosbag_capture.cpp @@ -209,6 +209,7 @@ void RosbagCapture::on_fault_confirmed(const std::string & fault_code) { size_t bag_size = calculate_bag_size(bag_path); RosbagFileInfo info; + info.bulk_data_id = FaultStorage::generate_uuid(); info.fault_code = fault_code; info.file_path = bag_path; info.format = config_.format; @@ -649,6 +650,7 @@ void RosbagCapture::post_fault_timer_callback() { size_t bag_size = calculate_bag_size(current_bag_path_); RosbagFileInfo info; + info.bulk_data_id = FaultStorage::generate_uuid(); info.fault_code = current_fault_code_; info.file_path = current_bag_path_; info.format = config_.format; 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..401ee46a 100644 --- a/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp +++ b/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp @@ -192,6 +192,7 @@ void SqliteFaultStorage::initialize_schema() { const char * create_rosbag_files_table_sql = R"( CREATE TABLE IF NOT EXISTS rosbag_files ( id INTEGER PRIMARY KEY AUTOINCREMENT, + bulk_data_id TEXT NOT NULL UNIQUE, fault_code TEXT NOT NULL UNIQUE, file_path TEXT NOT NULL, format TEXT NOT NULL, @@ -200,6 +201,7 @@ void SqliteFaultStorage::initialize_schema() { created_at_ns INTEGER NOT NULL ); CREATE INDEX IF NOT EXISTS idx_rosbag_files_fault_code ON rosbag_files(fault_code); + CREATE INDEX IF NOT EXISTS idx_rosbag_files_bulk_data_id ON rosbag_files(bulk_data_id); CREATE INDEX IF NOT EXISTS idx_rosbag_files_created_at ON rosbag_files(created_at_ns); )"; @@ -208,6 +210,48 @@ void SqliteFaultStorage::initialize_schema() { sqlite3_free(err_msg); throw std::runtime_error("Failed to create rosbag_files table: " + error); } + + // Migration: Add bulk_data_id column if it doesn't exist (for existing databases) + // Check if bulk_data_id column exists + bool has_bulk_data_id = false; + SqliteStatement check_stmt(db_, "PRAGMA table_info(rosbag_files)"); + while (check_stmt.step() == SQLITE_ROW) { + std::string col_name = check_stmt.column_text(1); + if (col_name == "bulk_data_id") { + has_bulk_data_id = true; + break; + } + } + + if (!has_bulk_data_id) { + // Add column and populate with UUIDs + const char * add_column_sql = "ALTER TABLE rosbag_files ADD COLUMN bulk_data_id TEXT"; + if (sqlite3_exec(db_, add_column_sql, nullptr, nullptr, &err_msg) != SQLITE_OK) { + std::string error = err_msg ? err_msg : "Unknown error"; + sqlite3_free(err_msg); + RCUTILS_LOG_WARN_NAMED("SqliteFaultStorage", "Failed to add bulk_data_id column: %s", error.c_str()); + } else { + // Generate UUIDs for existing rows + SqliteStatement select_stmt(db_, "SELECT fault_code FROM rosbag_files WHERE bulk_data_id IS NULL"); + std::vector fault_codes; + while (select_stmt.step() == SQLITE_ROW) { + fault_codes.push_back(select_stmt.column_text(0)); + } + + for (const auto & fc : fault_codes) { + std::string uuid = FaultStorage::generate_uuid(); + SqliteStatement update_stmt(db_, "UPDATE rosbag_files SET bulk_data_id = ? WHERE fault_code = ?"); + update_stmt.bind_text(1, uuid); + update_stmt.bind_text(2, fc); + update_stmt.step(); + } + + // Create index for bulk_data_id + const char * create_idx_sql = + "CREATE INDEX IF NOT EXISTS idx_rosbag_files_bulk_data_id ON rosbag_files(bulk_data_id)"; + sqlite3_exec(db_, create_idx_sql, nullptr, nullptr, nullptr); + } + } } std::vector SqliteFaultStorage::parse_json_array(const std::string & json_str) { @@ -757,18 +801,19 @@ void SqliteFaultStorage::store_rosbag_file(const RosbagFileInfo & info) { // Use INSERT OR REPLACE to handle updates (fault_code is UNIQUE) SqliteStatement stmt(db_, "INSERT OR REPLACE INTO rosbag_files " - "(fault_code, file_path, format, duration_sec, size_bytes, created_at_ns) " - "VALUES (?, ?, ?, ?, ?, ?)"); + "(bulk_data_id, fault_code, file_path, format, duration_sec, size_bytes, created_at_ns) " + "VALUES (?, ?, ?, ?, ?, ?, ?)"); - stmt.bind_text(1, info.fault_code); - stmt.bind_text(2, info.file_path); - stmt.bind_text(3, info.format); + stmt.bind_text(1, info.bulk_data_id); + stmt.bind_text(2, info.fault_code); + stmt.bind_text(3, info.file_path); + stmt.bind_text(4, info.format); // Bind duration_sec as a double using sqlite3_bind_double directly - if (sqlite3_bind_double(stmt.get(), 4, info.duration_sec) != SQLITE_OK) { + if (sqlite3_bind_double(stmt.get(), 5, info.duration_sec) != SQLITE_OK) { throw std::runtime_error(std::string("Failed to bind duration_sec: ") + sqlite3_errmsg(db_)); } - stmt.bind_int64(5, static_cast(info.size_bytes)); - stmt.bind_int64(6, info.created_at_ns); + stmt.bind_int64(6, static_cast(info.size_bytes)); + stmt.bind_int64(7, info.created_at_ns); if (stmt.step() != SQLITE_DONE) { throw std::runtime_error(std::string("Failed to store rosbag file: ") + sqlite3_errmsg(db_)); @@ -779,7 +824,7 @@ std::optional SqliteFaultStorage::get_rosbag_file(const std::str std::lock_guard lock(mutex_); SqliteStatement stmt(db_, - "SELECT fault_code, file_path, format, duration_sec, size_bytes, created_at_ns " + "SELECT bulk_data_id, fault_code, file_path, format, duration_sec, size_bytes, created_at_ns " "FROM rosbag_files WHERE fault_code = ?"); stmt.bind_text(1, fault_code); @@ -788,12 +833,13 @@ std::optional SqliteFaultStorage::get_rosbag_file(const std::str } 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); + info.bulk_data_id = stmt.column_text(0); + info.fault_code = stmt.column_text(1); + info.file_path = stmt.column_text(2); + info.format = stmt.column_text(3); + info.duration_sec = sqlite3_column_double(stmt.get(), 4); + info.size_bytes = static_cast(stmt.column_int64(5)); + info.created_at_ns = stmt.column_int64(6); return info; } @@ -842,17 +888,83 @@ std::vector SqliteFaultStorage::get_all_rosbag_files() const { std::vector result; SqliteStatement stmt(db_, - "SELECT fault_code, file_path, format, duration_sec, size_bytes, created_at_ns " + "SELECT bulk_data_id, fault_code, file_path, format, duration_sec, size_bytes, created_at_ns " "FROM rosbag_files ORDER BY created_at_ns ASC"); 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); + info.bulk_data_id = stmt.column_text(0); + info.fault_code = stmt.column_text(1); + info.file_path = stmt.column_text(2); + info.format = stmt.column_text(3); + info.duration_sec = sqlite3_column_double(stmt.get(), 4); + info.size_bytes = static_cast(stmt.column_int64(5)); + info.created_at_ns = stmt.column_int64(6); + result.push_back(info); + } + + return result; +} + +std::optional SqliteFaultStorage::get_rosbag_by_id(const std::string & bulk_data_id) const { + std::lock_guard lock(mutex_); + + SqliteStatement stmt(db_, + "SELECT bulk_data_id, fault_code, file_path, format, duration_sec, size_bytes, created_at_ns " + "FROM rosbag_files WHERE bulk_data_id = ?"); + stmt.bind_text(1, bulk_data_id); + + if (stmt.step() != SQLITE_ROW) { + return std::nullopt; + } + + RosbagFileInfo info; + info.bulk_data_id = stmt.column_text(0); + info.fault_code = stmt.column_text(1); + info.file_path = stmt.column_text(2); + info.format = stmt.column_text(3); + info.duration_sec = sqlite3_column_double(stmt.get(), 4); + info.size_bytes = static_cast(stmt.column_int64(5)); + info.created_at_ns = stmt.column_int64(6); + + return info; +} + +std::string SqliteFaultStorage::get_rosbag_path(const std::string & bulk_data_id) const { + auto rosbag = get_rosbag_by_id(bulk_data_id); + if (rosbag) { + return rosbag->file_path; + } + return ""; +} + +std::vector SqliteFaultStorage::get_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 + // reporting_sources is stored as JSON array + SqliteStatement stmt( + db_, + "SELECT r.bulk_data_id, 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 " + "WHERE f.reporting_sources LIKE ?"); + + // Use LIKE with wildcards for JSON array search + std::string pattern = "%\"" + entity_fqn + "\"%"; + stmt.bind_text(1, pattern); + + while (stmt.step() == SQLITE_ROW) { + RosbagFileInfo info; + info.bulk_data_id = stmt.column_text(0); + info.fault_code = stmt.column_text(1); + info.file_path = stmt.column_text(2); + info.format = stmt.column_text(3); + info.duration_sec = sqlite3_column_double(stmt.get(), 4); + info.size_bytes = static_cast(stmt.column_int64(5)); + info.created_at_ns = stmt.column_int64(6); result.push_back(info); } 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..620098d2 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" @@ -708,6 +709,187 @@ TEST_F(SqliteFaultStorageTest, ClearFaultDeletesAssociatedSnapshots) { EXPECT_TRUE(snapshots_after.empty()); } +// UUID generation tests + +// @verifies REQ_INTEROP_073 +TEST(UUIDGenerationTest, GeneratesValidFormat) { + auto uuid = ros2_medkit_fault_manager::FaultStorage::generate_uuid(); + + // UUID v4 format: xxxxxxxx-xxxx-4xxx-Nxxx-xxxxxxxxxxxx (36 chars with hyphens) + EXPECT_EQ(uuid.length(), 36u); + EXPECT_EQ(uuid[8], '-'); + EXPECT_EQ(uuid[13], '-'); + EXPECT_EQ(uuid[18], '-'); + EXPECT_EQ(uuid[23], '-'); + + // Version 4 indicator + EXPECT_EQ(uuid[14], '4'); +} + +// @verifies REQ_INTEROP_073 +TEST(UUIDGenerationTest, GeneratesUniqueValues) { + std::set uuids; + for (int i = 0; i < 1000; ++i) { + uuids.insert(ros2_medkit_fault_manager::FaultStorage::generate_uuid()); + } + EXPECT_EQ(uuids.size(), 1000u); // All should be unique +} + +// Rosbag UUID-based lookup tests + +// @verifies REQ_INTEROP_073 +TEST_F(SqliteFaultStorageTest, StoreRosbagWithUUID) { + using ros2_medkit_fault_manager::RosbagFileInfo; + + RosbagFileInfo info; + info.bulk_data_id = "550e8400-e29b-41d4-a716-446655440000"; + info.fault_code = "TEST_FAULT"; + info.file_path = "/tmp/test.mcap"; + info.format = "mcap"; + info.duration_sec = 5.0; + info.size_bytes = 1024; + info.created_at_ns = 1234567890; + + storage_->store_rosbag_file(info); + + auto result = storage_->get_rosbag_file("TEST_FAULT"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->bulk_data_id, "550e8400-e29b-41d4-a716-446655440000"); +} + +// @verifies REQ_INTEROP_073 +TEST_F(SqliteFaultStorageTest, GetRosbagByIdSuccess) { + using ros2_medkit_fault_manager::RosbagFileInfo; + + RosbagFileInfo info; + info.bulk_data_id = "test-uuid-1234-5678-9012"; + info.fault_code = "MY_FAULT"; + info.file_path = "/tmp/my.mcap"; + info.format = "mcap"; + info.duration_sec = 10.0; + info.size_bytes = 2048; + info.created_at_ns = 9876543210; + + storage_->store_rosbag_file(info); + + auto result = storage_->get_rosbag_by_id("test-uuid-1234-5678-9012"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->fault_code, "MY_FAULT"); + EXPECT_EQ(result->file_path, "/tmp/my.mcap"); +} + +// @verifies REQ_INTEROP_073 +TEST_F(SqliteFaultStorageTest, GetRosbagByIdNotFound) { + auto result = storage_->get_rosbag_by_id("nonexistent-uuid"); + EXPECT_FALSE(result.has_value()); +} + +// @verifies REQ_INTEROP_073 +TEST_F(SqliteFaultStorageTest, GetRosbagPathSuccess) { + using ros2_medkit_fault_manager::RosbagFileInfo; + + RosbagFileInfo info; + info.bulk_data_id = "path-test-uuid"; + info.fault_code = "PATH_TEST_FAULT"; + info.file_path = "/data/recordings/fault.mcap"; + info.format = "mcap"; + info.duration_sec = 3.0; + info.size_bytes = 512; + info.created_at_ns = 1111111111; + + storage_->store_rosbag_file(info); + + std::string path = storage_->get_rosbag_path("path-test-uuid"); + EXPECT_EQ(path, "/data/recordings/fault.mcap"); +} + +// @verifies REQ_INTEROP_073 +TEST_F(SqliteFaultStorageTest, GetRosbagPathNotFound) { + std::string path = storage_->get_rosbag_path("nonexistent-path-uuid"); + EXPECT_TRUE(path.empty()); +} + +// @verifies REQ_INTEROP_071 +TEST_F(SqliteFaultStorageTest, GetRosbagsForEntityFiltersCorrectly) { + 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.bulk_data_id = "entity-uuid-1"; + 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.bulk_data_id = "entity-uuid-2"; + 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_->get_rosbags_for_entity("/powertrain/motor"); + ASSERT_EQ(rosbags.size(), 1u); + EXPECT_EQ(rosbags[0].bulk_data_id, "entity-uuid-1"); + + // Get rosbags for brake entity + auto brake_rosbags = storage_->get_rosbags_for_entity("/chassis/brake"); + ASSERT_EQ(brake_rosbags.size(), 1u); + EXPECT_EQ(brake_rosbags[0].bulk_data_id, "entity-uuid-2"); + + // Get rosbags for unknown entity + auto unknown_rosbags = storage_->get_rosbags_for_entity("/unknown/entity"); + EXPECT_TRUE(unknown_rosbags.empty()); +} + +// @verifies REQ_INTEROP_073 +TEST_F(SqliteFaultStorageTest, GetAllRosbagFilesIncludesBulkDataId) { + using ros2_medkit_fault_manager::RosbagFileInfo; + + RosbagFileInfo info1; + info1.bulk_data_id = "bulk-uuid-1"; + 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.bulk_data_id = "bulk-uuid-2"; + 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].bulk_data_id, "bulk-uuid-1"); + EXPECT_EQ(all_rosbags[1].bulk_data_id, "bulk-uuid-2"); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); From 94824ffbdaaea63636b112da229047f9c95f08d2 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 07:36:11 +0000 Subject: [PATCH 03/27] feat(fault-manager): add GetFault service with environment data Implement GetFault service handler that returns: - Fault data with basic info and status - ExtendedDataRecords with first/last occurrence timestamps - Snapshots including freeze frames and rosbag metadata Add unit tests for GetFault service responses. --- .gitignore | 2 +- .../fault_manager_node.hpp | 9 +++ .../fault_storage.hpp | 4 +- .../src/fault_manager_node.cpp | 79 +++++++++++++++++++ .../test/test_fault_manager.cpp | 77 ++++++++++++++++++ 5 files changed, 168 insertions(+), 3 deletions(-) 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/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..202f28ae 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,6 +24,7 @@ #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_fault.hpp" #include "ros2_medkit_msgs/srv/get_faults.hpp" #include "ros2_medkit_msgs/srv/get_rosbag.hpp" #include "ros2_medkit_msgs/srv/get_snapshots.hpp" @@ -66,6 +67,10 @@ class FaultManagerNode : public rclcpp::Node { void handle_get_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, const std::shared_ptr & response); @@ -100,6 +105,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}; @@ -110,6 +118,7 @@ class FaultManagerNode : public rclcpp::Node { rclcpp::Service::SharedPtr report_fault_srv_; rclcpp::Service::SharedPtr get_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_; 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 b7093711..c53f402b 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 @@ -251,8 +251,8 @@ class InMemoryFaultStorage : public FaultStorage { mutable std::mutex mutex_; std::map faults_; std::vector snapshots_; - std::map rosbag_files_; ///< fault_code -> rosbag info - std::map rosbag_by_id_; ///< bulk_data_id -> fault_code (index) + std::map rosbag_files_; ///< fault_code -> rosbag info + std::map rosbag_by_id_; ///< bulk_data_id -> fault_code (index) DebounceConfig config_; }; 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..d36c0960 100644 --- a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp @@ -26,7 +26,10 @@ #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 { @@ -124,6 +127,12 @@ FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node(" handle_get_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( "~/clear_fault", [this](const std::shared_ptr & request, const std::shared_ptr & response) { @@ -467,6 +476,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_occurence_ns = rclcpp::Time(fault->first_occurred).nanoseconds(); + extended_records.last_occurence_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 = rosbag_info->bulk_data_id; + 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; 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..83c98124 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,7 @@ #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/report_fault.hpp" using ros2_medkit_fault_manager::DebounceConfig; @@ -33,6 +35,7 @@ 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::ReportFault; class FaultStorageTest : public ::testing::Test { @@ -704,16 +707,19 @@ 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"); // 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))); } void TearDown() override { event_subscription_.reset(); report_fault_client_.reset(); clear_fault_client_.reset(); + get_fault_client_.reset(); test_node_.reset(); fault_manager_.reset(); received_events_.clear(); @@ -756,11 +762,24 @@ 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::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_; std::vector received_events_; }; @@ -887,6 +906,64 @@ 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_occurence_ns, 0); + EXPECT_NE(edr.last_occurence_ns, 0); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); From 8ff17d11367de7fc6541993de05689cf8ea5aef6 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 07:45:05 +0000 Subject: [PATCH 04/27] feat(fault-manager): add ListFaultsForEntity service Implement ListFaultsForEntity service for querying faults by entity: - Add get_all_faults() method to FaultStorage interface and implementations - Add matches_entity() helper for FQN suffix matching - Add list_faults_for_entity service to FaultManagerNode Add unit tests for new functionality including: - GetAllFaults storage tests - ListFaultsForEntity service tests - matches_entity helper tests --- .../fault_manager_node.hpp | 13 ++ .../fault_storage.hpp | 5 + .../sqlite_fault_storage.hpp | 1 + .../src/fault_manager_node.cpp | 52 ++++++++ .../src/fault_storage.cpp | 13 ++ .../src/sqlite_fault_storage.cpp | 29 +++++ .../test/test_fault_manager.cpp | 119 ++++++++++++++++++ 7 files changed, 232 insertions(+) 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 202f28ae..3f1c845a 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 @@ -28,6 +28,7 @@ #include "ros2_medkit_msgs/srv/get_faults.hpp" #include "ros2_medkit_msgs/srv/get_rosbag.hpp" #include "ros2_medkit_msgs/srv/get_snapshots.hpp" +#include "ros2_medkit_msgs/srv/list_faults_for_entity.hpp" #include "ros2_medkit_msgs/srv/report_fault.hpp" namespace ros2_medkit_fault_manager { @@ -55,6 +56,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(); @@ -83,6 +90,11 @@ class FaultManagerNode : public rclcpp::Node { void handle_get_rosbag(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(); @@ -122,6 +134,7 @@ class FaultManagerNode : public rclcpp::Node { rclcpp::Service::SharedPtr clear_fault_srv_; rclcpp::Service::SharedPtr get_snapshots_srv_; rclcpp::Service::SharedPtr get_rosbag_srv_; + rclcpp::Service::SharedPtr list_faults_for_entity_srv_; rclcpp::TimerBase::SharedPtr auto_confirm_timer_; /// Timer for periodic cleanup of expired correlation data 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 c53f402b..ccc7833e 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 @@ -194,6 +194,10 @@ class FaultStorage { /// @return Vector of rosbag file info for faults reported by this entity virtual std::vector get_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; + /// Generate a UUID v4 string /// @return A new UUID in format "xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx" static std::string generate_uuid(); @@ -243,6 +247,7 @@ class InMemoryFaultStorage : public FaultStorage { std::optional get_rosbag_by_id(const std::string & bulk_data_id) const override; std::string get_rosbag_path(const std::string & bulk_data_id) const override; std::vector get_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 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 471336ef..04bc07a9 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 @@ -73,6 +73,7 @@ class SqliteFaultStorage : public FaultStorage { std::optional get_rosbag_by_id(const std::string & bulk_data_id) const override; std::string get_rosbag_path(const std::string & bulk_data_id) const override; std::vector get_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 d36c0960..42f888df 100644 --- a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp @@ -151,6 +151,13 @@ FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node(" handle_get_rosbag(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) { @@ -875,4 +882,49 @@ void FaultManagerNode::handle_get_rosbag(const std::shared_ptrfault_code.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 2a47638f..239cd0df 100644 --- a/src/ros2_medkit_fault_manager/src/fault_storage.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_storage.cpp @@ -454,4 +454,17 @@ std::vector InMemoryFaultStorage::get_rosbags_for_entity(const s 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/sqlite_fault_storage.cpp b/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp index 401ee46a..f00a557c 100644 --- a/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp +++ b/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp @@ -971,4 +971,33 @@ std::vector SqliteFaultStorage::get_rosbags_for_entity(const std 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 83c98124..32d57e96 100644 --- a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp +++ b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp @@ -27,6 +27,7 @@ #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; @@ -36,6 +37,7 @@ 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 { @@ -603,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: @@ -708,11 +731,14 @@ class FaultEventPublishingTest : public ::testing::Test { 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 { @@ -720,6 +746,7 @@ class FaultEventPublishingTest : public ::testing::Test { 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(); @@ -774,12 +801,25 @@ class FaultEventPublishingTest : public ::testing::Test { 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_; }; @@ -964,6 +1004,85 @@ TEST_F(FaultEventPublishingTest, GetFaultReturnsExtendedDataRecords) { EXPECT_NE(edr.last_occurence_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); From e9be59b2eb38217e48fc7f2e88fe62f583deb514 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 07:55:09 +0000 Subject: [PATCH 05/27] feat(gateway): add EntityPathInfo utility for entity path parsing Add utility functions for parsing entity paths from HTTP request URLs: - EntityPathInfo struct with type, entity_id, resource_path, parent_id - parse_entity_path() extracts entity info from /api/v1/{type}/{id}/... URLs - extract_bulk_data_category() gets bulk-data category from path - extract_bulk_data_id() gets bulk-data item ID from path Supports nested entities (subareas, subcomponents) with parent tracking. Includes 20 unit tests covering all parsing scenarios. --- src/ros2_medkit_gateway/CMakeLists.txt | 7 + .../http/entity_path_utils.hpp | 78 ++++++++ .../src/http/entity_path_utils.cpp | 133 +++++++++++++ .../test/test_entity_path_utils.cpp | 185 ++++++++++++++++++ 4 files changed, 403 insertions(+) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/entity_path_utils.hpp create mode 100644 src/ros2_medkit_gateway/src/http/entity_path_utils.cpp create mode 100644 src/ros2_medkit_gateway/test/test_entity_path_utils.cpp diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index c08474e8..aa97e0fa 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -146,6 +146,7 @@ add_library(gateway_lib STATIC 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 @@ -318,6 +319,10 @@ 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) + # Apply coverage flags to test targets if(ENABLE_COVERAGE) target_compile_options(test_gateway_node PRIVATE --coverage -O0 -g) @@ -350,6 +355,8 @@ 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) endif() # Integration testing 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/src/http/entity_path_utils.cpp b/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp new file mode 100644 index 00000000..eaecf5ee --- /dev/null +++ b/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp @@ -0,0 +1,133 @@ +// 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 = "/" + parent_segment + "/" + info.parent_id + "/" + sub_segment + "/" + 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 = "/" + type_segment + "/" + 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/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"); +} From 1b476b7f3e149b99f2d852e7729452230f64e0c2 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 08:06:06 +0000 Subject: [PATCH 06/27] feat(gateway): add SOVD-compliant fault response builder Add build_sovd_fault_response() for SOVD-compliant responses: - 'item' wrapper with fault details and status object - 'environment_data' with extended_data_records and snapshots - 'x-medkit' extensions with occurrence_count, severity_label - Rosbag snapshots include bulk_data_uri (absolute path) - Primary value extraction for common ROS message types Add FaultManager::get_fault_with_env() for retrieving fault with environment data via GetFault service. Update handle_get_fault to use entity path from URL for bulk_data_uri generation. Includes 12 unit tests covering SOVD response building. @verifies REQ_INTEROP_013 --- src/ros2_medkit_gateway/CMakeLists.txt | 4 + .../ros2_medkit_gateway/fault_manager.hpp | 19 +- .../http/handlers/fault_handlers.hpp | 21 ++ src/ros2_medkit_gateway/src/fault_manager.cpp | 80 +++- .../src/http/handlers/fault_handlers.cpp | 192 +++++++++- .../test/test_fault_handlers.cpp | 350 ++++++++++++++++++ 6 files changed, 639 insertions(+), 27 deletions(-) create mode 100644 src/ros2_medkit_gateway/test/test_fault_handlers.cpp diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index aa97e0fa..08246880 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -323,6 +323,10 @@ if(BUILD_TESTING) 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) + # Apply coverage flags to test targets if(ENABLE_COVERAGE) target_compile_options(test_gateway_node PRIVATE --coverage -O0 -g) 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..6a445204 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,8 +21,10 @@ #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_fault.hpp" #include "ros2_medkit_msgs/srv/get_faults.hpp" #include "ros2_medkit_msgs/srv/get_rosbag.hpp" #include "ros2_medkit_msgs/srv/get_snapshots.hpp" @@ -39,6 +41,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 { @@ -66,7 +76,13 @@ class FaultManager { 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 @@ -103,6 +119,7 @@ class FaultManager { /// Service clients rclcpp::Client::SharedPtr report_fault_client_; + rclcpp::Client::SharedPtr get_fault_client_; rclcpp::Client::SharedPtr get_faults_client_; rclcpp::Client::SharedPtr clear_fault_client_; rclcpp::Client::SharedPtr get_snapshots_client_; 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..64d4c52b 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 { @@ -80,6 +84,23 @@ class FaultHandlers { */ void handle_get_rosbag(const httplib::Request & req, httplib::Response & res); + /** + * @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 + */ + 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/src/fault_manager.cpp b/src/ros2_medkit_gateway/src/fault_manager.cpp index 6430efc6..fb1dcc0e 100644 --- a/src/ros2_medkit_gateway/src/fault_manager.cpp +++ b/src/ros2_medkit_gateway/src/fault_manager.cpp @@ -26,6 +26,7 @@ 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_fault_client_ = node_->create_client("/fault_manager/get_fault"); get_faults_client_ = node_->create_client("/fault_manager/get_faults"); clear_fault_client_ = node_->create_client("/fault_manager/clear_fault"); get_snapshots_client_ = node_->create_client("/fault_manager/get_snapshots"); @@ -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) && + get_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() && + get_faults_client_->service_is_ready() && clear_fault_client_->service_is_ready(); } /// Convert a ROS 2 Fault message to JSON for REST API responses. @@ -230,27 +231,70 @@ 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(service_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; + + auto future = get_fault_client_->async_send_request(request); - // 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; + 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; } 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..24fd9766 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp @@ -23,9 +23,11 @@ #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" @@ -119,8 +121,178 @@ json filter_faults_by_sources(const json & faults_array, const std::set(seconds); + std::tm tm_buf; + std::tm * tm = gmtime_r(&time, &tm_buf); + + 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; +} + +/// Map fault severity level to human-readable label +std::string severity_to_label(uint8_t severity) { + switch (severity) { + case 0: + return "DEBUG"; + case 1: + return "INFO"; + case 2: + return "WARN"; + case 3: + return "ERROR"; + case 4: + return "FATAL"; + default: + return "UNKNOWN"; + } +} + +/// Extract primary value from JSON data based on message type +/// For common ROS message types, extracts the main value field +json extract_primary_value(const std::string & message_type, const json & full_data) { + // Map of message type to primary field + static const std::unordered_map 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 + UUID + snap["bulk_data_uri"] = entity_path + "/bulk-data/rosbags/" + s.bulk_data_id; + 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_occurence", to_iso8601_ns(env_data.extended_data_records.first_occurence_ns)}, + {"last_occurence", to_iso8601_ns(env_data.extended_data_records.last_occurence_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)}}; + + return response; +} + void FaultHandlers::handle_list_all_faults(const httplib::Request & req, httplib::Response & res) { try { auto filter = parse_fault_status_param(req); @@ -346,6 +518,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 +542,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 { 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..0036e28e --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp @@ -0,0 +1,350 @@ +// 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_occurence_ns = 1707044400000000000; + env_data.extended_data_records.last_occurence_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"); +} + +// @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.bulk_data_id = "550e8400-e29b-41d4-a716-446655440000"; + 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/550e8400-e29b-41d4-a716-446655440000"); + 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 = "test-uuid"; + 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/test-uuid"); +} + +// @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_occurence_ns = 1770458400000000000; // 2026-02-08 + env_data.extended_data_records.last_occurence_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_occurence"].get(); + std::string last = edr["last_occurence"].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 = "uuid-123"; + 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/uuid-123"); +} From 071e1176cd14d1fe903d858ab1b9fa216e1abf33 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 08:22:15 +0000 Subject: [PATCH 07/27] feat(gateway): add SOVD bulk-data handlers for all entity types Add BulkDataHandlers class with: - handle_list_categories: GET {entity}/bulk-data - handle_list_descriptors: GET {entity}/bulk-data/{category} - handle_download: GET {entity}/bulk-data/{category}/{id} Supports all entity path patterns: - /apps, /components, /areas, /functions - /areas/{id}/subareas, /components/{id}/subcomponents Security: Validates rosbag belongs to requested entity. CORS: Exposes Content-Disposition header for frontend. @verifies REQ_INTEROP_071 @verifies REQ_INTEROP_072 @verifies REQ_INTEROP_073 --- src/ros2_medkit_gateway/CMakeLists.txt | 9 + .../http/handlers/bulkdata_handlers.hpp | 110 ++++++++ .../http/handlers/handlers.hpp | 1 + .../ros2_medkit_gateway/http/rest_server.hpp | 1 + .../src/http/handlers/bulkdata_handlers.cpp | 261 ++++++++++++++++++ .../src/http/rest_server.cpp | 84 ++++++ .../test/test_bulkdata_handlers.cpp | 61 ++++ 7 files changed, 527 insertions(+) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/bulkdata_handlers.hpp create mode 100644 src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp create mode 100644 src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 08246880..9ddd3964 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -142,6 +142,7 @@ 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 @@ -327,6 +328,10 @@ if(BUILD_TESTING) 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) @@ -361,6 +366,10 @@ if(BUILD_TESTING) 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 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..8fc27f73 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/bulkdata_handlers.hpp @@ -0,0 +1,110 @@ +// 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 + * @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 Format nanosecond timestamp to ISO 8601 string. + * @param ns Nanoseconds since epoch + * @return ISO 8601 formatted string (e.g., "2025-01-15T10:30:00.123Z") + */ + static std::string format_timestamp_ns(int64_t ns); +}; + +} // namespace handlers +} // namespace ros2_medkit_gateway 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/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/http/handlers/bulkdata_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp new file mode 100644 index 00000000..7c2163e0 --- /dev/null +++ b/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp @@ -0,0 +1,261 @@ +// 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" + +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; + } + + // Verify entity exists + auto entity = ctx_.get_entity_info(entity_info->entity_id); + if (entity.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, httplib::StatusCode::NotFound_404, ERR_ENTITY_NOT_FOUND, "Entity not found", + {{"entity_id", entity_info->entity_id}}); + return; + } + + // 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; + } + + // Get entity for FQN lookup + auto entity = ctx_.get_entity_info(entity_info->entity_id); + if (entity.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, httplib::StatusCode::NotFound_404, ERR_ENTITY_NOT_FOUND, "Entity not found", + {{"entity_id", entity_info->entity_id}}); + return; + } + + // 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->get_faults(source_filter); + + nlohmann::json items = nlohmann::json::array(); + + if (faults_result.success && faults_result.data.contains("faults")) { + for (const auto & fault_json : faults_result.data["faults"]) { + if (!fault_json.contains("fault_code")) { + continue; + } + + std::string fault_code = fault_json["fault_code"].get(); + + // Get rosbag info for this fault + auto rosbag_result = fault_mgr->get_rosbag(fault_code); + if (rosbag_result.success && rosbag_result.data.contains("file_path")) { + // Build BulkDataDescriptor + std::string format = rosbag_result.data.value("format", "mcap"); + uint64_t size_bytes = rosbag_result.data.value("size_bytes", 0); + double duration_sec = rosbag_result.data.value("duration_sec", 0.0); + + // Use fault_code as bulk_data_id (could be UUID if available) + std::string bulk_data_id = fault_code; + + // Get timestamp from fault if available + double first_occurred = fault_json.value("first_occurred", 0.0); + int64_t 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; + } + + // Get entity to verify it exists + auto entity = ctx_.get_entity_info(entity_info->entity_id); + if (entity.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, httplib::StatusCode::NotFound_404, ERR_ENTITY_NOT_FOUND, "Entity not found", + {{"entity_id", entity_info->entity_id}}); + return; + } + + // 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 + // Get faults for entity and check if fault_code is in the list + std::string source_filter = entity.fqn.empty() ? entity.namespace_path : entity.fqn; + auto faults_result = fault_mgr->get_faults(source_filter); + + bool belongs_to_entity = false; + if (faults_result.success && faults_result.data.contains("faults")) { + for (const auto & fault_json : faults_result.data["faults"]) { + if (fault_json.contains("fault_code") && fault_json["fault_code"].get() == fault_code) { + belongs_to_entity = true; + break; + } + } + } + + if (!belongs_to_entity) { + 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 + "\""); + res.set_header("Access-Control-Expose-Headers", "Content-Disposition"); + + 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) { + std::ifstream file(file_path, std::ios::binary | std::ios::ate); + if (!file.is_open()) { + return false; + } + + auto size = file.tellg(); + file.seekg(0, std::ios::beg); + + std::vector buffer(static_cast(size)); + if (!file.read(buffer.data(), size)) { + return false; + } + + res.set_content(std::string(buffer.begin(), buffer.end()), content_type); + return true; +} + +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/gzip"; + } + return "application/octet-stream"; +} + +std::string BulkDataHandlers::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); + + 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 handlers +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/http/rest_server.cpp b/src/ros2_medkit_gateway/src/http/rest_server.cpp index 5e9c5226..4a77b67c 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(); @@ -779,6 +780,89 @@ void RESTServer::setup_routes() { fault_handlers_->handle_get_component_snapshots(req, res); }); + // === 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) { + bulkdata_handlers_->handle_list_descriptors(req, res); + }); + + // 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) { + bulkdata_handlers_->handle_download(req, res); + }); + + // Nested entities - subareas + srv->Get((api_path("/areas") + R"(/([^/]+)/subareas/([^/]+)/bulk-data$)"), + [this](const httplib::Request & req, httplib::Response & 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); + }); + + // Nested entities - subcomponents + srv->Get((api_path("/components") + R"(/([^/]+)/subcomponents/([^/]+)/bulk-data$)"), + [this](const httplib::Request & req, httplib::Response & 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) // POST /auth/authorize - Authenticate and get tokens (client_credentials grant) srv->Post(api_path("/auth/authorize"), [this](const httplib::Request & req, httplib::Response & res) { 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..24c7f8fc --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp @@ -0,0 +1,61 @@ +// 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" + +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/gzip"); +} + +// @verifies REQ_INTEROP_071 +TEST_F(BulkDataHandlersTest, GetRosbagMimetypeDb3) { + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("db3"), "application/gzip"); +} + +// @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"); +} From a9345ce0686deefd8bc6cad6a467ce7ede523f7f Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 08:34:12 +0000 Subject: [PATCH 08/27] refactor: remove legacy snapshot endpoints REQ_INTEROP_072, REQ_INTEROP_073 - Remove /faults/{code}/snapshots (system-wide) - Remove /faults/{code}/snapshots/bag (rosbag download) - Remove entity-scoped snapshot endpoints for components, apps, areas, functions - Remove handler declarations from fault_handlers.hpp - Remove handler implementations from fault_handlers.cpp - Remove unused helper functions (sanitize_filename, generate_timestamp, validate_fault_code) - Remove unused includes (filesystem, fstream, iomanip) Snapshots are now inline in fault responses (environment_data). Rosbag downloads use bulk-data endpoint pattern (GET /{entity}/bulk-data/rosbags/{fault_code}). --- src/dynamic_message_introspection | 1 + .../http/handlers/fault_handlers.hpp | 20 +- .../src/http/handlers/fault_handlers.cpp | 300 ------------------ .../src/http/rest_server.cpp | 35 -- 4 files changed, 4 insertions(+), 352 deletions(-) create mode 160000 src/dynamic_message_introspection diff --git a/src/dynamic_message_introspection b/src/dynamic_message_introspection new file mode 160000 index 00000000..b38e36c2 --- /dev/null +++ b/src/dynamic_message_introspection @@ -0,0 +1 @@ +Subproject commit b38e36c2e2ff2a490d1b1e9db99c55ecb5dd5a07 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 64d4c52b..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 @@ -29,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: @@ -69,21 +70,6 @@ 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. - */ - void handle_get_rosbag(const httplib::Request & req, httplib::Response & res); - /** * @brief Build SOVD-compliant fault response with environment data. * 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 24fd9766..69c4edc6 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp @@ -18,9 +18,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -40,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) { @@ -678,250 +623,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/rest_server.cpp b/src/ros2_medkit_gateway/src/http/rest_server.cpp index 4a77b67c..beb3da6d 100644 --- a/src/ros2_medkit_gateway/src/http/rest_server.cpp +++ b/src/ros2_medkit_gateway/src/http/rest_server.cpp @@ -435,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); @@ -569,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); @@ -755,31 +745,6 @@ 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$)"), - [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_snapshots(req, res); - }); - - // GET /faults/{fault_code}/snapshots/bag - download rosbag file for fault - srv->Get((api_path("/faults") + R"(/([^/]+)/snapshots/bag$)"), - [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_rosbag(req, res); - }); - - // GET /components/{component_id}/faults/{fault_code}/snapshots - component-scoped snapshot access - srv->Get((api_path("/components") + R"(/([^/]+)/faults/([^/]+)/snapshots$)"), - [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_component_snapshots(req, res); - }); - - // GET /apps/{app_id}/faults/{fault_code}/snapshots - app-scoped snapshot access - srv->Get((api_path("/apps") + R"(/([^/]+)/faults/([^/]+)/snapshots$)"), - [this](const httplib::Request & req, httplib::Response & res) { - fault_handlers_->handle_get_component_snapshots(req, res); - }); - // === Bulk Data Routes (REQ_INTEROP_071-073) === // List bulk-data categories srv->Get((api_path("/apps") + R"(/([^/]+)/bulk-data$)"), From 7ec221f2e958789c3912a46658ccd87d39575be2 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 08:48:27 +0000 Subject: [PATCH 09/27] test(gateway): add bulk-data integration tests Add comprehensive integration tests: - Bulk-data category listing for all entity types - BulkDataDescriptor structure validation - Rosbag download with headers verification - Security check for cross-entity access - Nested entity path support - SOVD-compliant fault response structure - Legacy endpoints removal verification @verifies REQ_INTEROP_071 @verifies REQ_INTEROP_072 @verifies REQ_INTEROP_073 @verifies REQ_INTEROP_013 --- src/ros2_medkit_gateway/CMakeLists.txt | 2 +- .../src/http/entity_path_utils.cpp | 15 +- .../test/test_integration.test.py | 535 ++++++++++++++++++ 3 files changed, 549 insertions(+), 3 deletions(-) diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 9ddd3964..1814db66 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -385,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/src/http/entity_path_utils.cpp b/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp index eaecf5ee..8076accb 100644 --- a/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp +++ b/src/ros2_medkit_gateway/src/http/entity_path_utils.cpp @@ -94,7 +94,16 @@ std::optional parse_entity_path(const std::string & request_path // 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 = "/" + parent_segment + "/" + info.parent_id + "/" + sub_segment + "/" + info.entity_id; + 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(); @@ -102,7 +111,9 @@ std::optional parse_entity_path(const std::string & request_path // Build entity path: /{type}/{entity_id} std::string type_segment = get_type_segment(def.type, false); - info.entity_path = "/" + type_segment + "/" + info.entity_id; + 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; diff --git a/src/ros2_medkit_gateway/test/test_integration.test.py b/src/ros2_medkit_gateway/test/test_integration.test.py index 239594d0..8176850a 100644 --- a/src/ros2_medkit_gateway/test/test_integration.test.py +++ b/src/ros2_medkit_gateway/test/test_integration.test.py @@ -4485,3 +4485,538 @@ def test_120_app_routes_work_with_app_id(self): self.assertIn('items', data) print(f'✓ App routes correctly accept app ID: {app_id}') + + # ========== Bulk-Data Endpoints Tests (REQ_INTEROP_071-073) ========== + + def test_121_bulk_data_list_categories_success(self): + """ + Test GET /apps/{app}/bulk-data returns categories. + + @verifies REQ_INTEROP_071 + """ + response = requests.get( + f'{self.BASE_URL}/apps/lidar_sensor/bulk-data', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + self.assertIn('items', data) + self.assertIsInstance(data['items'], list) + # Should include rosbags category + self.assertIn('rosbags', data['items']) + + print(f'✓ Bulk-data categories test passed: {data["items"]}') + + def test_122_bulk_data_list_categories_all_entity_types(self): + """ + Test bulk-data endpoint works for all entity types. + + @verifies REQ_INTEROP_071 + """ + entity_endpoints = [ + '/apps/lidar_sensor/bulk-data', + '/components/perception/bulk-data', + '/areas/perception/bulk-data', + ] + + for endpoint in entity_endpoints: + response = requests.get(f'{self.BASE_URL}{endpoint}', timeout=10) + self.assertEqual( + response.status_code, 200, + f'Expected 200 for {endpoint}, got {response.status_code}' + ) + + data = response.json() + self.assertIn('items', data) + self.assertIsInstance(data['items'], list) + + print('✓ Bulk-data categories work for all entity types') + + def test_123_bulk_data_list_categories_entity_not_found(self): + """ + Test bulk-data returns 404 for nonexistent entity. + + @verifies REQ_INTEROP_071 + """ + response = requests.get( + f'{self.BASE_URL}/apps/nonexistent_app/bulk-data', + timeout=10 + ) + self.assertEqual(response.status_code, 404) + + data = response.json() + self.assertIn('error_code', data) + + print('✓ Bulk-data 404 for nonexistent entity test passed') + + def test_124_bulk_data_list_descriptors_structure(self): + """ + Test GET /apps/{app}/bulk-data/rosbags returns BulkDataDescriptor[]. + + @verifies REQ_INTEROP_072 + """ + # Wait for fault to be generated (lidar sensor has invalid params) + time.sleep(3) + + response = requests.get( + f'{self.BASE_URL}/apps/lidar_sensor/bulk-data/rosbags', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + self.assertIn('items', data) + self.assertIsInstance(data['items'], list) + + # If there are rosbags, verify structure + if len(data['items']) > 0: + descriptor = data['items'][0] + self.assertIn('id', descriptor) + self.assertIn('name', descriptor) + self.assertIn('size', descriptor) + self.assertIn('mime_type', descriptor) + self.assertIn('href', descriptor) + # Verify x-medkit extension + self.assertIn('x-medkit', descriptor) + x_medkit = descriptor['x-medkit'] + self.assertIn('fault_code', x_medkit) + + print(f'✓ Bulk-data descriptors test passed: {len(data["items"])} rosbags') + else: + print('✓ Bulk-data descriptors test passed: 0 rosbags (OK if no faults yet)') + + def test_125_bulk_data_list_descriptors_empty_result(self): + """ + Test bulk-data returns empty array for entity without rosbags. + + @verifies REQ_INTEROP_072 + """ + # temp_sensor doesn't generate faults, so should have no rosbags + response = requests.get( + f'{self.BASE_URL}/apps/temp_sensor/bulk-data/rosbags', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + self.assertIn('items', data) + self.assertIsInstance(data['items'], list) + # Should return empty array (not error) + self.assertEqual(len(data['items']), 0) + + print('✓ Bulk-data empty descriptors test passed') + + def test_126_bulk_data_unknown_category_returns_404(self): + """ + Test bulk-data returns 404 for unknown category. + + @verifies REQ_INTEROP_072 + """ + response = requests.get( + f'{self.BASE_URL}/apps/lidar_sensor/bulk-data/unknown_category', + timeout=10 + ) + self.assertEqual(response.status_code, 404) + + data = response.json() + self.assertIn('error_code', data) + + print('✓ Bulk-data unknown category 404 test passed') + + def _wait_for_fault_with_rosbag(self, entity_id: str, max_wait: float = 30.0): + """ + Wait for a fault with rosbag to be available for entity. + + Returns fault_code if found, None if timeout. + """ + start_time = time.time() + while time.time() - start_time < max_wait: + try: + response = requests.get( + f'{self.BASE_URL}/apps/{entity_id}/bulk-data/rosbags', + timeout=5 + ) + if response.status_code == 200: + data = response.json() + items = data.get('items', []) + if len(items) > 0: + return items[0].get('id') + except requests.exceptions.RequestException: + pass + time.sleep(1) + return None + + def test_127_bulk_data_download_success(self): + """ + Test GET /apps/{app}/bulk-data/rosbags/{id} downloads file successfully. + + This test verifies the complete rosbag download flow: + 1. Wait for fault with rosbag snapshot + 2. Download the rosbag via bulk-data endpoint + 3. Verify response headers (Content-Type, Content-Disposition) + 4. Verify response body is non-empty binary data + + @verifies REQ_INTEROP_073 + """ + # Wait for fault_manager to capture rosbag (lidar_sensor generates faults) + rosbag_id = self._wait_for_fault_with_rosbag('lidar_sensor', max_wait=30.0) + if rosbag_id is None: + self.skipTest('No rosbag available for download test (fault not yet generated)') + + # Download the rosbag + response = requests.get( + f'{self.BASE_URL}/apps/lidar_sensor/bulk-data/rosbags/{rosbag_id}', + timeout=30, + stream=True + ) + self.assertEqual(response.status_code, 200) + + # Verify Content-Type header + content_type = response.headers.get('Content-Type', '') + valid_types = ['application/gzip', 'application/x-mcap', 'application/octet-stream'] + self.assertTrue( + any(t in content_type for t in valid_types), + f'Expected valid rosbag Content-Type, got: {content_type}' + ) + + # Verify Content-Disposition header + content_disposition = response.headers.get('Content-Disposition', '') + self.assertIn('attachment', content_disposition) + self.assertIn('filename=', content_disposition) + + # Verify we got actual data + content = response.content + self.assertGreater(len(content), 0, 'Downloaded rosbag should have content') + + print(f'✓ Bulk-data download success: {len(content)} bytes, ' + f'Content-Type: {content_type}') + + def test_128_bulk_data_download_not_found(self): + """ + Test bulk-data download returns 404 for invalid UUID. + + @verifies REQ_INTEROP_073 + """ + response = requests.get( + f'{self.BASE_URL}/apps/lidar_sensor/bulk-data/rosbags/nonexistent-uuid', + timeout=10 + ) + self.assertEqual(response.status_code, 404) + + # Response should be JSON error + data = response.json() + self.assertIn('error_code', data) + + print('✓ Bulk-data download 404 for invalid UUID test passed') + + def test_129_bulk_data_download_wrong_entity_returns_404(self): + """ + Test bulk-data download returns 404 if rosbag exists but for different entity. + + Security check: rosbag belonging to one entity shouldn't be accessible + via another entity's bulk-data endpoint. + + @verifies REQ_INTEROP_073 + """ + # Get a rosbag ID from lidar_sensor + rosbag_id = self._wait_for_fault_with_rosbag('lidar_sensor', max_wait=15.0) + if rosbag_id is None: + self.skipTest('No rosbag available for cross-entity test') + + # Try to access it via temp_sensor (wrong entity) + response = requests.get( + f'{self.BASE_URL}/apps/temp_sensor/bulk-data/rosbags/{rosbag_id}', + timeout=10 + ) + # Should return 404 (not found for this entity) + self.assertEqual(response.status_code, 404) + + print('✓ Bulk-data cross-entity access denied test passed') + + def test_130_bulk_data_nested_entity_path(self): + """ + Test bulk-data endpoints work for nested entities (e.g., perception area). + + @verifies REQ_INTEROP_071 + """ + # Test nested area + response = requests.get( + f'{self.BASE_URL}/areas/perception/bulk-data', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + self.assertIn('items', data) + self.assertIn('rosbags', data['items']) + + print('✓ Bulk-data nested entity path test passed') + + # ========== SOVD-Compliant Fault Response Tests (REQ_INTEROP_013) ========== + + def _wait_for_lidar_fault(self, max_wait: float = 30.0): + """ + Wait for lidar_sensor fault to be available. + + Returns (fault_code, entity_id) tuple if found, (None, None) if timeout. + """ + start_time = time.time() + while time.time() - start_time < max_wait: + try: + response = requests.get( + f'{self.BASE_URL}/apps/lidar_sensor/faults', + timeout=5 + ) + if response.status_code == 200: + data = response.json() + items = data.get('items', []) + if len(items) > 0: + fault_code = items[0].get('faultCode') + if fault_code: + return (fault_code, 'lidar_sensor') + except requests.exceptions.RequestException: + pass + time.sleep(1) + return (None, None) + + def test_131_fault_response_structure(self): + """ + Test GET /{entity}/faults/{code} returns SOVD-compliant response. + + @verifies REQ_INTEROP_013 + """ + fault_code, entity_id = self._wait_for_lidar_fault(max_wait=30.0) + if fault_code is None: + self.skipTest('No fault available for response structure test') + + response = requests.get( + f'{self.BASE_URL}/apps/{entity_id}/faults/{fault_code}', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + # SOVD structure: { "item": {...}, "environment_data": {...} } + self.assertIn('item', data) + self.assertIn('environment_data', data) + + # Verify item structure + item = data['item'] + self.assertIn('faultCode', item) + self.assertIn('status', item) + + print(f'✓ Fault response structure test passed: {fault_code}') + + def test_132_fault_status_object_structure(self): + """ + Test fault item has SOVD-compliant status object. + + @verifies REQ_INTEROP_013 + """ + fault_code, entity_id = self._wait_for_lidar_fault(max_wait=30.0) + if fault_code is None: + self.skipTest('No fault available for status object test') + + response = requests.get( + f'{self.BASE_URL}/apps/{entity_id}/faults/{fault_code}', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + item = data['item'] + status = item['status'] + + # SOVD status object structure + self.assertIn('testFailed', status) + self.assertIn('presentStatus', status) + self.assertIn('confirmedStatus', status) + self.assertIsInstance(status['testFailed'], bool) + self.assertIsInstance(status['presentStatus'], bool) + self.assertIsInstance(status['confirmedStatus'], bool) + + print(f'✓ Fault status object test passed: {status}') + + def test_133_fault_environment_data_structure(self): + """ + Test fault response includes environment_data with snapshots. + + @verifies REQ_INTEROP_013 + """ + fault_code, entity_id = self._wait_for_lidar_fault(max_wait=30.0) + if fault_code is None: + self.skipTest('No fault available for environment_data test') + + response = requests.get( + f'{self.BASE_URL}/apps/{entity_id}/faults/{fault_code}', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + env_data = data['environment_data'] + + # SOVD environment_data structure + self.assertIn('extended_data_records', env_data) + self.assertIn('snapshots', env_data) + self.assertIsInstance(env_data['extended_data_records'], list) + self.assertIsInstance(env_data['snapshots'], list) + + print(f'✓ Fault environment_data structure test passed: ' + f'{len(env_data["snapshots"])} snapshots') + + def test_134_fault_snapshot_freeze_frame_structure(self): + """ + Test freeze_frame snapshot has correct structure. + + @verifies REQ_INTEROP_013 + """ + fault_code, entity_id = self._wait_for_lidar_fault(max_wait=30.0) + if fault_code is None: + self.skipTest('No fault available for freeze_frame test') + + response = requests.get( + f'{self.BASE_URL}/apps/{entity_id}/faults/{fault_code}', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + snapshots = data['environment_data']['snapshots'] + + # Find freeze_frame snapshot + freeze_frame = None + for snapshot in snapshots: + if snapshot.get('category') == 'freeze_frame': + freeze_frame = snapshot + break + + if freeze_frame is None: + self.skipTest('No freeze_frame snapshot in fault response') + + # Verify freeze_frame structure + self.assertIn('id', freeze_frame) + self.assertIn('category', freeze_frame) + self.assertIn('timestamp', freeze_frame) + self.assertEqual(freeze_frame['category'], 'freeze_frame') + # freeze_frame has inline data + self.assertIn('data', freeze_frame) + + print('✓ Fault freeze_frame snapshot test passed') + + def test_135_fault_snapshot_rosbag_has_bulk_data_uri(self): + """ + Test rosbag snapshot has correct bulk_data_uri format. + + @verifies REQ_INTEROP_013 + """ + fault_code, entity_id = self._wait_for_lidar_fault(max_wait=30.0) + if fault_code is None: + self.skipTest('No fault available for rosbag snapshot test') + + response = requests.get( + f'{self.BASE_URL}/apps/{entity_id}/faults/{fault_code}', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + snapshots = data['environment_data']['snapshots'] + + # Find rosbag snapshot + rosbag = None + for snapshot in snapshots: + if snapshot.get('category') == 'rosbag': + rosbag = snapshot + break + + if rosbag is None: + self.skipTest('No rosbag snapshot in fault response') + + # Verify rosbag structure + self.assertIn('id', rosbag) + self.assertIn('category', rosbag) + self.assertEqual(rosbag['category'], 'rosbag') + # rosbag has bulk_data_uri instead of inline data + self.assertIn('bulk_data_uri', rosbag) + self.assertIn('/bulk-data/rosbags/', rosbag['bulk_data_uri']) + + print(f'✓ Fault rosbag bulk_data_uri test passed: {rosbag["bulk_data_uri"]}') + + def test_136_fault_x_medkit_extensions(self): + """ + Test fault response includes x-medkit extensions. + + @verifies REQ_INTEROP_013 + """ + fault_code, entity_id = self._wait_for_lidar_fault(max_wait=30.0) + if fault_code is None: + self.skipTest('No fault available for x-medkit test') + + response = requests.get( + f'{self.BASE_URL}/apps/{entity_id}/faults/{fault_code}', + timeout=10 + ) + self.assertEqual(response.status_code, 200) + + data = response.json() + # x-medkit extensions + self.assertIn('x-medkit', data) + x_medkit = data['x-medkit'] + self.assertIn('occurrence_count', x_medkit) + self.assertIn('severity_label', x_medkit) + self.assertIn('reporting_sources', x_medkit) + + print(f'✓ Fault x-medkit extensions test passed: ' + f'severity={x_medkit["severity_label"]}, ' + f'occurrences={x_medkit["occurrence_count"]}') + + def test_137_bulk_data_download_verifies_complete_rosbag(self): + """ + Test downloading rosbag and verifying it's a valid archive. + + This test goes beyond basic download verification to ensure + the downloaded content is actually a valid gzip/tar archive. + + @verifies REQ_INTEROP_073 + """ + import gzip + import io + + rosbag_id = self._wait_for_fault_with_rosbag('lidar_sensor', max_wait=30.0) + if rosbag_id is None: + self.skipTest('No rosbag available for complete download test') + + # Download the rosbag + response = requests.get( + f'{self.BASE_URL}/apps/lidar_sensor/bulk-data/rosbags/{rosbag_id}', + timeout=30, + stream=True + ) + self.assertEqual(response.status_code, 200) + + content = response.content + content_type = response.headers.get('Content-Type', '') + + # If it's gzip, verify it's a valid gzip file + if 'gzip' in content_type: + try: + # Try to read gzip header + with gzip.GzipFile(fileobj=io.BytesIO(content)) as f: + # Just read a bit to verify it's valid + f.read(100) + print(f'✓ Downloaded rosbag is valid gzip archive ({len(content)} bytes)') + except gzip.BadGzipFile: + self.fail('Downloaded rosbag is not a valid gzip file') + elif 'mcap' in content_type: + # MCAP files start with magic bytes + self.assertTrue( + content[:4] == b'\x89MCX' or len(content) > 0, + 'MCAP file should have content' + ) + print(f'✓ Downloaded rosbag is MCAP format ({len(content)} bytes)') + else: + # For other formats, just verify we have content + self.assertGreater(len(content), 0) + print(f'✓ Downloaded rosbag has content ({len(content)} bytes)') From 48bd151b731db038d934ea5df525fa37c318646a Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Thu, 5 Feb 2026 17:23:04 +0000 Subject: [PATCH 10/27] fix(bulk-data): resolve rosbag directory path and correct MIME type - Add resolve_rosbag_file_path() to handle rosbag2 directory structure - Fix MIME type from application/gzip to application/x-sqlite3 for db3 files - Update unit tests for correct MIME type expectations - Update integration tests for SQLite format validation --- .../http/handlers/bulkdata_handlers.hpp | 13 +- .../src/http/handlers/bulkdata_handlers.cpp | 33 +++- .../test/test_bulkdata_handlers.cpp | 4 +- .../test/test_integration.test.py | 184 ++++++------------ 4 files changed, 103 insertions(+), 131 deletions(-) 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 index 8fc27f73..c170c8f0 100644 --- 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 @@ -91,13 +91,24 @@ class BulkDataHandlers { /** * @brief Stream file contents to HTTP response. * @param res HTTP response to write to - * @param file_path Path to file to stream + * @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); + /** * @brief Format nanosecond timestamp to ISO 8601 string. * @param ns Nanoseconds since epoch diff --git a/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp index 7c2163e0..e22ee73f 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/bulkdata_handlers.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "ros2_medkit_gateway/gateway_node.hpp" @@ -213,7 +214,13 @@ void BulkDataHandlers::handle_download(const httplib::Request & req, httplib::Re bool BulkDataHandlers::stream_file_to_response(httplib::Response & res, const std::string & file_path, const std::string & content_type) { - std::ifstream file(file_path, std::ios::binary | std::ios::ate); + // 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; + } + + std::ifstream file(actual_path, std::ios::binary | std::ios::ate); if (!file.is_open()) { return false; } @@ -230,11 +237,33 @@ bool BulkDataHandlers::stream_file_to_response(httplib::Response & res, const st 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/gzip"; + return "application/x-sqlite3"; } return "application/octet-stream"; } diff --git a/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp b/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp index 24c7f8fc..2c57af52 100644 --- a/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp +++ b/src/ros2_medkit_gateway/test/test_bulkdata_handlers.cpp @@ -35,12 +35,12 @@ TEST_F(BulkDataHandlersTest, GetRosbagMimetypeMcap) { // @verifies REQ_INTEROP_071 TEST_F(BulkDataHandlersTest, GetRosbagMimetypeSqlite3) { - EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("sqlite3"), "application/gzip"); + 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/gzip"); + EXPECT_EQ(BulkDataHandlers::get_rosbag_mimetype("db3"), "application/x-sqlite3"); } // @verifies REQ_INTEROP_071 diff --git a/src/ros2_medkit_gateway/test/test_integration.test.py b/src/ros2_medkit_gateway/test/test_integration.test.py index 8176850a..77f9b6dc 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