diff --git a/src/ros2_medkit_fault_manager/CMakeLists.txt b/src/ros2_medkit_fault_manager/CMakeLists.txt index 1150364..933e533 100644 --- a/src/ros2_medkit_fault_manager/CMakeLists.txt +++ b/src/ros2_medkit_fault_manager/CMakeLists.txt @@ -34,11 +34,13 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(ros2_medkit_msgs REQUIRED) +find_package(SQLite3 REQUIRED) # Library target (shared between executable and tests) add_library(fault_manager_lib STATIC src/fault_manager_node.cpp src/fault_storage.cpp + src/sqlite_fault_storage.cpp ) target_include_directories(fault_manager_lib PUBLIC @@ -46,11 +48,13 @@ target_include_directories(fault_manager_lib PUBLIC $ ) -ament_target_dependencies(fault_manager_lib +ament_target_dependencies(fault_manager_lib PUBLIC rclcpp ros2_medkit_msgs ) +target_link_libraries(fault_manager_lib PUBLIC SQLite::SQLite3) + if(ENABLE_COVERAGE) target_compile_options(fault_manager_lib PRIVATE --coverage -O0 -g) target_link_options(fault_manager_lib PRIVATE --coverage) @@ -95,10 +99,17 @@ if(BUILD_TESTING) target_link_libraries(test_fault_manager fault_manager_lib) ament_target_dependencies(test_fault_manager rclcpp ros2_medkit_msgs) - # Apply coverage flags to test target + # SQLite storage tests + ament_add_gtest(test_sqlite_storage test/test_sqlite_storage.cpp) + target_link_libraries(test_sqlite_storage fault_manager_lib) + ament_target_dependencies(test_sqlite_storage rclcpp ros2_medkit_msgs) + + # Apply coverage flags to test targets if(ENABLE_COVERAGE) target_compile_options(test_fault_manager PRIVATE --coverage -O0 -g) target_link_options(test_fault_manager PRIVATE --coverage) + target_compile_options(test_sqlite_storage PRIVATE --coverage -O0 -g) + target_link_options(test_sqlite_storage PRIVATE --coverage) endif() # Integration tests diff --git a/src/ros2_medkit_fault_manager/README.md b/src/ros2_medkit_fault_manager/README.md index 0775947..0067bb9 100644 --- a/src/ros2_medkit_fault_manager/README.md +++ b/src/ros2_medkit_fault_manager/README.md @@ -22,13 +22,36 @@ query and clearing interfaces. - **Occurrence tracking**: Counts total reports and tracks all reporting sources - **Severity escalation**: Fault severity is updated if a higher severity is reported - **Status lifecycle**: PENDING → CONFIRMED → CLEARED (automatic status transitions in Issue #6) +- **Persistent storage**: SQLite backend ensures faults survive node restarts + +## Parameters + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `storage_type` | string | `"sqlite"` | Storage backend: `"sqlite"` for persistent storage, `"memory"` for in-memory | +| `database_path` | string | `"/var/lib/ros2_medkit/faults.db"` | Path to SQLite database file. Use `":memory:"` for in-memory SQLite | + +### Storage Backends + +**SQLite (default)**: Faults are persisted to disk and survive node restarts. The database directory is created automatically if it doesn't exist. Uses WAL mode for optimal performance. + +**Memory**: Faults are stored in memory only. Useful for testing or when persistence is not required. ## Usage ### Launch ```bash +# Default (SQLite storage) ros2 launch ros2_medkit_fault_manager fault_manager.launch.py + +# With custom database path +ros2 run ros2_medkit_fault_manager fault_manager_node --ros-args \ + -p database_path:=/custom/path/faults.db + +# With in-memory storage (no persistence) +ros2 run ros2_medkit_fault_manager fault_manager_node --ros-args \ + -p storage_type:=memory ``` ### Manual Testing 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 390ab63..f685732 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 @@ -15,6 +15,7 @@ #pragma once #include +#include #include "rclcpp/rclcpp.hpp" #include "ros2_medkit_fault_manager/fault_storage.hpp" @@ -27,17 +28,30 @@ namespace ros2_medkit_fault_manager { /// Central fault manager node /// /// Provides service interfaces for fault reporting, querying, and clearing. -/// Stores faults in memory and aggregates reports by fault_code. +/// Supports configurable storage backends (memory or SQLite) via ROS parameters. +/// +/// Parameters: +/// - storage_type (string): "memory" or "sqlite" (default: "sqlite") +/// - database_path (string): Path to SQLite database file (default: "/var/lib/ros2_medkit/faults.db") +/// Use ":memory:" for in-memory SQLite database (useful for testing) class FaultManagerNode : public rclcpp::Node { public: explicit FaultManagerNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /// Get read-only access to fault storage (for testing) const FaultStorage & get_storage() const { - return storage_; + return *storage_; + } + + /// Get the storage type being used + const std::string & get_storage_type() const { + return storage_type_; } private: + /// Create storage backend based on configuration + std::unique_ptr create_storage(); + /// Handle ReportFault service request void handle_report_fault(const std::shared_ptr & request, const std::shared_ptr & response); @@ -53,7 +67,9 @@ class FaultManagerNode : public rclcpp::Node { /// Validate severity value static bool is_valid_severity(uint8_t severity); - InMemoryFaultStorage storage_; + std::string storage_type_; + std::string database_path_; + std::unique_ptr storage_; rclcpp::Service::SharedPtr report_fault_srv_; rclcpp::Service::SharedPtr get_faults_srv_; 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 new file mode 100644 index 0000000..53d3228 --- /dev/null +++ b/src/ros2_medkit_fault_manager/include/ros2_medkit_fault_manager/sqlite_fault_storage.hpp @@ -0,0 +1,78 @@ +// 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. + +#pragma once + +#include + +#include +#include + +#include "ros2_medkit_fault_manager/fault_storage.hpp" + +namespace ros2_medkit_fault_manager { + +/// SQLite-based fault storage implementation with persistence +/// Thread-safe implementation using mutex protection +class SqliteFaultStorage : public FaultStorage { + public: + /// Create SQLite fault storage + /// @param db_path Path to SQLite database file. Use ":memory:" for in-memory database. + /// @throws std::runtime_error if database cannot be opened or initialized + explicit SqliteFaultStorage(const std::string & db_path); + + /// Destructor - closes database connection + ~SqliteFaultStorage() override; + + // Non-copyable, non-movable (owns SQLite connection) + SqliteFaultStorage(const SqliteFaultStorage &) = delete; + SqliteFaultStorage & operator=(const SqliteFaultStorage &) = delete; + SqliteFaultStorage(SqliteFaultStorage &&) = delete; + SqliteFaultStorage & operator=(SqliteFaultStorage &&) = delete; + + bool report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, + const std::string & source_id, const rclcpp::Time & timestamp) override; + + std::vector get_faults(bool filter_by_severity, uint8_t severity, + const std::vector & statuses) const override; + + std::optional get_fault(const std::string & fault_code) const override; + + bool clear_fault(const std::string & fault_code) override; + + size_t size() const override; + + bool contains(const std::string & fault_code) const override; + + /// Get the database path + const std::string & db_path() const { + return db_path_; + } + + private: + /// Initialize database schema + void initialize_schema(); + + /// Parse JSON array string to vector of strings + static std::vector parse_json_array(const std::string & json_str); + + /// Serialize vector of strings to JSON array string + static std::string serialize_json_array(const std::vector & vec); + + std::string db_path_; + sqlite3 * db_{nullptr}; + mutable std::mutex mutex_; +}; + +} // namespace ros2_medkit_fault_manager diff --git a/src/ros2_medkit_fault_manager/package.xml b/src/ros2_medkit_fault_manager/package.xml index c9781b4..e0decf4 100644 --- a/src/ros2_medkit_fault_manager/package.xml +++ b/src/ros2_medkit_fault_manager/package.xml @@ -12,6 +12,7 @@ rclcpp ros2_medkit_msgs + sqlite3 ament_lint_auto ament_lint_common 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 23d1bb3..a636cb6 100644 --- a/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_manager_node.cpp @@ -14,9 +14,20 @@ #include "ros2_medkit_fault_manager/fault_manager_node.hpp" +#include + +#include "ros2_medkit_fault_manager/sqlite_fault_storage.hpp" + namespace ros2_medkit_fault_manager { FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node("fault_manager", options) { + // Declare and get parameters + storage_type_ = declare_parameter("storage_type", "sqlite"); + database_path_ = declare_parameter("database_path", "/var/lib/ros2_medkit/faults.db"); + + // Create storage backend + storage_ = create_storage(); + // Create service servers report_fault_srv_ = create_service( "~/report_fault", [this](const std::shared_ptr & request, @@ -36,7 +47,39 @@ FaultManagerNode::FaultManagerNode(const rclcpp::NodeOptions & options) : Node(" handle_clear_fault(request, response); }); - RCLCPP_INFO(get_logger(), "FaultManager node started"); + RCLCPP_INFO(get_logger(), "FaultManager node started (storage=%s)", storage_type_.c_str()); +} + +std::unique_ptr FaultManagerNode::create_storage() { + if (storage_type_ == "memory") { + RCLCPP_INFO(get_logger(), "Using in-memory fault storage"); + return std::make_unique(); + } + + if (storage_type_ == "sqlite") { + // Create parent directory if it doesn't exist (except for :memory:) + if (database_path_ != ":memory:") { + std::filesystem::path db_path(database_path_); + auto parent_dir = db_path.parent_path(); + std::string parent_dir_str = parent_dir.string(); + if (!parent_dir_str.empty() && !std::filesystem::exists(parent_dir)) { + try { + std::filesystem::create_directories(parent_dir); + RCLCPP_INFO(get_logger(), "Created database directory: %s", parent_dir_str.c_str()); + } catch (const std::filesystem::filesystem_error & e) { + RCLCPP_ERROR(get_logger(), "Failed to create database directory for fault manager storage at '%s': %s", + parent_dir_str.c_str(), e.what()); + throw; + } + } + } + + RCLCPP_INFO(get_logger(), "Using SQLite fault storage: %s", database_path_.c_str()); + return std::make_unique(database_path_); + } + + RCLCPP_ERROR(get_logger(), "Unknown storage_type '%s', falling back to in-memory", storage_type_.c_str()); + return std::make_unique(); } void FaultManagerNode::handle_report_fault( @@ -65,7 +108,7 @@ void FaultManagerNode::handle_report_fault( // Report the fault bool is_new = - storage_.report_fault(request->fault_code, request->severity, request->description, request->source_id, now()); + storage_->report_fault(request->fault_code, request->severity, request->description, request->source_id, now()); response->success = true; if (is_new) { @@ -81,7 +124,7 @@ void FaultManagerNode::handle_report_fault( void FaultManagerNode::handle_get_faults(const std::shared_ptr & request, const std::shared_ptr & response) { - response->faults = storage_.get_faults(request->filter_by_severity, request->severity, request->statuses); + response->faults = storage_->get_faults(request->filter_by_severity, request->severity, request->statuses); RCLCPP_DEBUG(get_logger(), "GetFaults returned %zu faults", response->faults.size()); } @@ -96,7 +139,7 @@ void FaultManagerNode::handle_clear_fault( return; } - bool cleared = storage_.clear_fault(request->fault_code); + bool cleared = storage_->clear_fault(request->fault_code); response->success = cleared; if (cleared) { diff --git a/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp b/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp new file mode 100644 index 0000000..2688591 --- /dev/null +++ b/src/ros2_medkit_fault_manager/src/sqlite_fault_storage.cpp @@ -0,0 +1,509 @@ +// 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. + +#include "ros2_medkit_fault_manager/sqlite_fault_storage.hpp" + +#include +#include +#include +#include + +#include "rcutils/logging_macros.h" +#include "ros2_medkit_msgs/msg/fault.hpp" + +namespace ros2_medkit_fault_manager { + +namespace { + +/// RAII wrapper for SQLite statements +class SqliteStatement { + public: + SqliteStatement(sqlite3 * db, const char * sql) : db_(db) { + if (sqlite3_prepare_v2(db, sql, -1, &stmt_, nullptr) != SQLITE_OK) { + throw std::runtime_error(std::string("Failed to prepare statement: ") + sqlite3_errmsg(db)); + } + } + + ~SqliteStatement() { + if (stmt_) { + sqlite3_finalize(stmt_); + } + } + + SqliteStatement(const SqliteStatement &) = delete; + SqliteStatement & operator=(const SqliteStatement &) = delete; + + sqlite3_stmt * get() const { + return stmt_; + } + + void bind_text(int index, const std::string & value) { + const auto size = value.size(); + if (size > static_cast(std::numeric_limits::max())) { + throw std::runtime_error("Failed to bind text: value size exceeds SQLite int length limit"); + } + const auto length = static_cast(size); + if (sqlite3_bind_text(stmt_, index, value.c_str(), length, SQLITE_TRANSIENT) != SQLITE_OK) { + throw std::runtime_error(std::string("Failed to bind text: ") + sqlite3_errmsg(db_)); + } + } + + void bind_int(int index, int value) { + if (sqlite3_bind_int(stmt_, index, value) != SQLITE_OK) { + throw std::runtime_error(std::string("Failed to bind int: ") + sqlite3_errmsg(db_)); + } + } + + void bind_int64(int index, int64_t value) { + if (sqlite3_bind_int64(stmt_, index, value) != SQLITE_OK) { + throw std::runtime_error(std::string("Failed to bind int64: ") + sqlite3_errmsg(db_)); + } + } + + int step() { + return sqlite3_step(stmt_); + } + + void reset() { + sqlite3_reset(stmt_); + sqlite3_clear_bindings(stmt_); + } + + std::string column_text(int index) { + const auto * text = reinterpret_cast(sqlite3_column_text(stmt_, index)); + return text ? std::string(text) : std::string(); + } + + int column_int(int index) { + return sqlite3_column_int(stmt_, index); + } + + int64_t column_int64(int index) { + return sqlite3_column_int64(stmt_, index); + } + + private: + sqlite3 * db_; + sqlite3_stmt * stmt_{nullptr}; +}; + +} // namespace + +SqliteFaultStorage::SqliteFaultStorage(const std::string & db_path) : db_path_(db_path) { + int flags = SQLITE_OPEN_READWRITE | SQLITE_OPEN_CREATE | SQLITE_OPEN_FULLMUTEX; + if (sqlite3_open_v2(db_path.c_str(), &db_, flags, nullptr) != SQLITE_OK) { + std::string error = db_ ? sqlite3_errmsg(db_) : "Unknown error"; + if (db_) { + sqlite3_close(db_); + db_ = nullptr; + } + throw std::runtime_error("Failed to open database '" + db_path + "': " + error); + } + + // Enable WAL mode for better concurrent performance + char * err_msg = nullptr; + if (sqlite3_exec(db_, "PRAGMA journal_mode=WAL;", nullptr, nullptr, &err_msg) != SQLITE_OK) { + std::string error = err_msg ? err_msg : "Unknown error"; + sqlite3_free(err_msg); + sqlite3_close(db_); + db_ = nullptr; + throw std::runtime_error("Failed to enable WAL mode: " + error); + } + + // Set busy timeout to handle concurrent access + sqlite3_busy_timeout(db_, 5000); + + initialize_schema(); +} + +SqliteFaultStorage::~SqliteFaultStorage() { + if (db_) { + sqlite3_close(db_); + } +} + +void SqliteFaultStorage::initialize_schema() { + const char * create_table_sql = R"( + CREATE TABLE IF NOT EXISTS faults ( + fault_code TEXT PRIMARY KEY, + severity INTEGER NOT NULL, + description TEXT NOT NULL, + first_occurred_ns INTEGER NOT NULL, + last_occurred_ns INTEGER NOT NULL, + occurrence_count INTEGER NOT NULL, + status TEXT NOT NULL, + reporting_sources TEXT NOT NULL + ); + )"; + + char * err_msg = nullptr; + if (sqlite3_exec(db_, create_table_sql, nullptr, nullptr, &err_msg) != SQLITE_OK) { + std::string error = err_msg ? err_msg : "Unknown error"; + sqlite3_free(err_msg); + throw std::runtime_error("Failed to create schema: " + error); + } +} + +std::vector SqliteFaultStorage::parse_json_array(const std::string & json_str) { + std::vector result; + + // Simple JSON array parser for ["a", "b", "c"] format + if (json_str.size() < 2 || json_str.front() != '[' || json_str.back() != ']') { + if (!json_str.empty()) { + RCUTILS_LOG_WARN_NAMED("sqlite_fault_storage", "Malformed JSON array in database: '%s'", json_str.c_str()); + } + return result; + } + + std::string content = json_str.substr(1, json_str.size() - 2); + if (content.empty()) { + return result; + } + + size_t pos = 0; + while (pos < content.size()) { + // Skip whitespace + while (pos < content.size() && std::isspace(static_cast(content[pos]))) { + ++pos; + } + if (pos >= content.size()) { + break; + } + + // Expect opening quote + if (content[pos] != '"') { + break; + } + ++pos; + + // Find closing quote (handle escape sequences) + std::string value; + while (pos < content.size() && content[pos] != '"') { + if (content[pos] == '\\' && pos + 1 < content.size()) { + ++pos; + char escaped = content[pos]; + switch (escaped) { + case '"': + value.push_back('"'); + break; + case '\\': + value.push_back('\\'); + break; + case '/': + value.push_back('/'); + break; + case 'b': + value.push_back('\b'); + break; + case 'f': + value.push_back('\f'); + break; + case 'n': + value.push_back('\n'); + break; + case 'r': + value.push_back('\r'); + break; + case 't': + value.push_back('\t'); + break; + default: + // Unknown escape sequence: preserve character as-is + value.push_back(escaped); + break; + } + ++pos; + continue; + } + value.push_back(content[pos]); + ++pos; + } + + if (pos < content.size()) { + ++pos; // Skip closing quote + } + + result.push_back(value); + + // Skip whitespace and comma + while (pos < content.size() && (std::isspace(static_cast(content[pos])) || content[pos] == ',')) { + ++pos; + } + } + + return result; +} + +std::string SqliteFaultStorage::serialize_json_array(const std::vector & vec) { + std::ostringstream oss; + oss << '['; + for (size_t i = 0; i < vec.size(); ++i) { + if (i > 0) { + oss << ','; + } + oss << '"'; + // Escape special characters per JSON specification + for (char c : vec[i]) { + switch (c) { + case '"': + oss << "\\\""; + break; + case '\\': + oss << "\\\\"; + break; + case '\b': + oss << "\\b"; + break; + case '\f': + oss << "\\f"; + break; + case '\n': + oss << "\\n"; + break; + case '\r': + oss << "\\r"; + break; + case '\t': + oss << "\\t"; + break; + default: + oss << c; + break; + } + } + oss << '"'; + } + oss << ']'; + return oss.str(); +} + +bool SqliteFaultStorage::report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, + const std::string & source_id, const rclcpp::Time & timestamp) { + std::lock_guard lock(mutex_); + + int64_t timestamp_ns = timestamp.nanoseconds(); + + // Check if fault exists + SqliteStatement check_stmt(db_, + "SELECT severity, occurrence_count, reporting_sources FROM faults WHERE fault_code = ?"); + check_stmt.bind_text(1, fault_code); + + if (check_stmt.step() == SQLITE_ROW) { + // Fault exists - update it + int existing_severity = check_stmt.column_int(0); + int64_t existing_count = check_stmt.column_int64(1); + std::string sources_json = check_stmt.column_text(2); + + // Parse existing sources and add new one + std::vector sources = parse_json_array(sources_json); + std::set sources_set(sources.begin(), sources.end()); + sources_set.insert(source_id); + sources.assign(sources_set.begin(), sources_set.end()); + + // Escalate severity if new severity is higher + int new_severity = std::max(existing_severity, static_cast(severity)); + + // Increment count with saturation + int64_t new_count = existing_count; + if (new_count < std::numeric_limits::max()) { + ++new_count; + } + + // Update with new description only if provided + const char * update_sql = description.empty() + ? "UPDATE faults SET severity = ?, last_occurred_ns = ?, occurrence_count = ?, " + "reporting_sources = ? WHERE fault_code = ?" + : "UPDATE faults SET severity = ?, description = ?, last_occurred_ns = ?, " + "occurrence_count = ?, reporting_sources = ? WHERE fault_code = ?"; + + SqliteStatement update_stmt(db_, update_sql); + + if (description.empty()) { + update_stmt.bind_int(1, new_severity); + update_stmt.bind_int64(2, timestamp_ns); + update_stmt.bind_int64(3, new_count); + update_stmt.bind_text(4, serialize_json_array(sources)); + update_stmt.bind_text(5, fault_code); + } else { + update_stmt.bind_int(1, new_severity); + update_stmt.bind_text(2, description); + update_stmt.bind_int64(3, timestamp_ns); + update_stmt.bind_int64(4, new_count); + update_stmt.bind_text(5, serialize_json_array(sources)); + update_stmt.bind_text(6, fault_code); + } + + if (update_stmt.step() != SQLITE_DONE) { + throw std::runtime_error(std::string("Failed to update fault: ") + sqlite3_errmsg(db_)); + } + + return false; // Existing fault updated + } + + // New fault - insert + SqliteStatement insert_stmt(db_, + "INSERT INTO faults (fault_code, severity, description, first_occurred_ns, " + "last_occurred_ns, occurrence_count, status, reporting_sources) " + "VALUES (?, ?, ?, ?, ?, ?, ?, ?)"); + + insert_stmt.bind_text(1, fault_code); + insert_stmt.bind_int(2, static_cast(severity)); + insert_stmt.bind_text(3, description); + insert_stmt.bind_int64(4, timestamp_ns); + insert_stmt.bind_int64(5, timestamp_ns); + insert_stmt.bind_int(6, 1); + insert_stmt.bind_text(7, ros2_medkit_msgs::msg::Fault::STATUS_PENDING); + insert_stmt.bind_text(8, serialize_json_array({source_id})); + + if (insert_stmt.step() != SQLITE_DONE) { + throw std::runtime_error(std::string("Failed to insert fault: ") + sqlite3_errmsg(db_)); + } + + return true; // New fault created +} + +std::vector +SqliteFaultStorage::get_faults(bool filter_by_severity, uint8_t severity, + const std::vector & statuses) const { + std::lock_guard lock(mutex_); + + // Determine which statuses to include + std::set status_filter; + if (statuses.empty()) { + status_filter.insert(ros2_medkit_msgs::msg::Fault::STATUS_CONFIRMED); + } else { + for (const auto & s : statuses) { + if (s == ros2_medkit_msgs::msg::Fault::STATUS_PENDING || s == ros2_medkit_msgs::msg::Fault::STATUS_CONFIRMED || + s == ros2_medkit_msgs::msg::Fault::STATUS_CLEARED) { + status_filter.insert(s); + } + } + if (status_filter.empty()) { + status_filter.insert(ros2_medkit_msgs::msg::Fault::STATUS_CONFIRMED); + } + } + + // Build query + std::string sql = + "SELECT fault_code, severity, description, first_occurred_ns, last_occurred_ns, " + "occurrence_count, status, reporting_sources FROM faults WHERE status IN ("; + for (size_t i = 0; i < status_filter.size(); ++i) { + if (i > 0) { + sql += ", "; + } + sql += "?"; + } + sql += ")"; + + if (filter_by_severity) { + sql += " AND severity = ?"; + } + + SqliteStatement stmt(db_, sql.c_str()); + + int param_index = 1; + for (const auto & s : status_filter) { + stmt.bind_text(param_index++, s); + } + if (filter_by_severity) { + stmt.bind_int(param_index, static_cast(severity)); + } + + 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; +} + +std::optional SqliteFaultStorage::get_fault(const std::string & fault_code) 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 WHERE fault_code = ?"); + stmt.bind_text(1, fault_code); + + if (stmt.step() != SQLITE_ROW) { + return std::nullopt; + } + + 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)); + + return fault; +} + +bool SqliteFaultStorage::clear_fault(const std::string & fault_code) { + std::lock_guard lock(mutex_); + + SqliteStatement stmt(db_, "UPDATE faults SET status = ? WHERE fault_code = ?"); + stmt.bind_text(1, ros2_medkit_msgs::msg::Fault::STATUS_CLEARED); + stmt.bind_text(2, fault_code); + + if (stmt.step() != SQLITE_DONE) { + throw std::runtime_error(std::string("Failed to clear fault: ") + sqlite3_errmsg(db_)); + } + + return sqlite3_changes(db_) > 0; +} + +size_t SqliteFaultStorage::size() const { + std::lock_guard lock(mutex_); + + SqliteStatement stmt(db_, "SELECT COUNT(*) FROM faults"); + + if (stmt.step() != SQLITE_ROW) { + return 0; + } + + return static_cast(stmt.column_int64(0)); +} + +bool SqliteFaultStorage::contains(const std::string & fault_code) const { + std::lock_guard lock(mutex_); + + SqliteStatement stmt(db_, "SELECT 1 FROM faults WHERE fault_code = ? LIMIT 1"); + stmt.bind_text(1, fault_code); + + return stmt.step() == SQLITE_ROW; +} + +} // 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 f20d58f..d1da427 100644 --- a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp +++ b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp @@ -147,7 +147,12 @@ TEST_F(FaultStorageTest, InvalidStatusDefaultsToConfirmed) { class FaultManagerNodeTest : public ::testing::Test { protected: void SetUp() override { - node_ = std::make_shared(); + // Use in-memory storage for tests to avoid permission issues + rclcpp::NodeOptions options; + options.parameter_overrides({ + {"storage_type", "memory"}, + }); + node_ = std::make_shared(options); } void TearDown() override { @@ -160,6 +165,7 @@ class FaultManagerNodeTest : public ::testing::Test { TEST_F(FaultManagerNodeTest, NodeCreation) { EXPECT_STREQ(node_->get_name(), "fault_manager"); EXPECT_EQ(node_->get_storage().size(), 0u); + EXPECT_EQ(node_->get_storage_type(), "memory"); } int main(int argc, char ** argv) { diff --git a/src/ros2_medkit_fault_manager/test/test_integration.test.py b/src/ros2_medkit_fault_manager/test/test_integration.test.py index 532cab9..71c3c37 100644 --- a/src/ros2_medkit_fault_manager/test/test_integration.test.py +++ b/src/ros2_medkit_fault_manager/test/test_integration.test.py @@ -73,6 +73,9 @@ def generate_test_description(): name='fault_manager', output='screen', additional_env=get_coverage_env(), + parameters=[{ + 'storage_type': 'memory', # Use in-memory storage for integration tests + }], ) return ( diff --git a/src/ros2_medkit_fault_manager/test/test_sqlite_storage.cpp b/src/ros2_medkit_fault_manager/test/test_sqlite_storage.cpp new file mode 100644 index 0000000..eaa65fd --- /dev/null +++ b/src/ros2_medkit_fault_manager/test/test_sqlite_storage.cpp @@ -0,0 +1,265 @@ +// 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. + +#include + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "ros2_medkit_fault_manager/sqlite_fault_storage.hpp" +#include "ros2_medkit_msgs/msg/fault.hpp" + +using ros2_medkit_fault_manager::SqliteFaultStorage; +using ros2_medkit_msgs::msg::Fault; + +class SqliteFaultStorageTest : public ::testing::Test { + protected: + void SetUp() override { + // Create a unique temp file for each test using random_device for better entropy + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dist; + temp_db_path_ = std::filesystem::temp_directory_path() / ("test_faults_" + std::to_string(dist(gen)) + ".db"); + storage_ = std::make_unique(temp_db_path_.string()); + } + + void TearDown() override { + storage_.reset(); + // Clean up temp file + std::filesystem::remove(temp_db_path_); + // Also remove WAL and SHM files if they exist + std::filesystem::remove(temp_db_path_.string() + "-wal"); + std::filesystem::remove(temp_db_path_.string() + "-shm"); + } + + std::filesystem::path temp_db_path_; + std::unique_ptr storage_; +}; + +TEST_F(SqliteFaultStorageTest, ReportNewFault) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + bool is_new = storage_->report_fault("MOTOR_OVERHEAT", Fault::SEVERITY_ERROR, "Motor temperature exceeded threshold", + "/powertrain/motor", timestamp); + + EXPECT_TRUE(is_new); + EXPECT_EQ(storage_->size(), 1u); + EXPECT_TRUE(storage_->contains("MOTOR_OVERHEAT")); +} + +TEST_F(SqliteFaultStorageTest, ReportExistingFaultUpdates) { + rclcpp::Clock clock; + auto timestamp1 = clock.now(); + auto timestamp2 = clock.now(); + + storage_->report_fault("MOTOR_OVERHEAT", Fault::SEVERITY_WARN, "Initial report", "/powertrain/motor1", timestamp1); + + bool is_new = storage_->report_fault("MOTOR_OVERHEAT", Fault::SEVERITY_ERROR, "Second report", "/powertrain/motor2", + timestamp2); + + EXPECT_FALSE(is_new); + EXPECT_EQ(storage_->size(), 1u); + + auto fault = storage_->get_fault("MOTOR_OVERHEAT"); + ASSERT_TRUE(fault.has_value()); + EXPECT_EQ(fault->occurrence_count, 2u); + EXPECT_EQ(fault->severity, Fault::SEVERITY_ERROR); // Updated to higher severity + EXPECT_EQ(fault->reporting_sources.size(), 2u); +} + +TEST_F(SqliteFaultStorageTest, GetFaultsDefaultReturnsConfirmedOnly) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + // Report a fault (starts as PENDING) + storage_->report_fault("FAULT_1", Fault::SEVERITY_ERROR, "Test", "/node1", timestamp); + + // Default query should return empty (only PENDING exists) + auto faults = storage_->get_faults(false, 0, {}); + EXPECT_EQ(faults.size(), 0u); +} + +TEST_F(SqliteFaultStorageTest, GetFaultsWithPendingStatus) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + storage_->report_fault("FAULT_1", Fault::SEVERITY_ERROR, "Test", "/node1", timestamp); + + // Query with PENDING status + auto faults = storage_->get_faults(false, 0, {"PENDING"}); + EXPECT_EQ(faults.size(), 1u); + EXPECT_EQ(faults[0].fault_code, "FAULT_1"); + EXPECT_EQ(faults[0].status, Fault::STATUS_PENDING); +} + +TEST_F(SqliteFaultStorageTest, GetFaultsFilterBySeverity) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + storage_->report_fault("FAULT_INFO", Fault::SEVERITY_INFO, "Info", "/node1", timestamp); + storage_->report_fault("FAULT_ERROR", Fault::SEVERITY_ERROR, "Error", "/node1", timestamp); + + // Filter by ERROR severity + auto faults = storage_->get_faults(true, Fault::SEVERITY_ERROR, {"PENDING"}); + EXPECT_EQ(faults.size(), 1u); + EXPECT_EQ(faults[0].fault_code, "FAULT_ERROR"); +} + +TEST_F(SqliteFaultStorageTest, ClearFault) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + storage_->report_fault("MOTOR_OVERHEAT", Fault::SEVERITY_ERROR, "Test", "/node1", timestamp); + + bool cleared = storage_->clear_fault("MOTOR_OVERHEAT"); + EXPECT_TRUE(cleared); + + auto fault = storage_->get_fault("MOTOR_OVERHEAT"); + ASSERT_TRUE(fault.has_value()); + EXPECT_EQ(fault->status, Fault::STATUS_CLEARED); +} + +TEST_F(SqliteFaultStorageTest, ClearNonExistentFault) { + bool cleared = storage_->clear_fault("NON_EXISTENT"); + EXPECT_FALSE(cleared); +} + +TEST_F(SqliteFaultStorageTest, GetClearedFaults) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + storage_->report_fault("FAULT_1", Fault::SEVERITY_ERROR, "Test", "/node1", timestamp); + storage_->clear_fault("FAULT_1"); + + // Query cleared faults + auto faults = storage_->get_faults(false, 0, {"CLEARED"}); + EXPECT_EQ(faults.size(), 1u); + EXPECT_EQ(faults[0].status, Fault::STATUS_CLEARED); +} + +TEST_F(SqliteFaultStorageTest, InvalidStatusDefaultsToConfirmed) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + storage_->report_fault("FAULT_1", Fault::SEVERITY_ERROR, "Test", "/node1", timestamp); + + // Query with invalid status - defaults to CONFIRMED (fault is PENDING, so no matches) + auto faults = storage_->get_faults(false, 0, {"INVALID_STATUS"}); + EXPECT_EQ(faults.size(), 0u); +} + +// SQLite-specific persistence test +TEST_F(SqliteFaultStorageTest, PersistenceAcrossRestarts) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + // Report some faults + storage_->report_fault("FAULT_1", Fault::SEVERITY_ERROR, "Persistent fault 1", "/node1", timestamp); + storage_->report_fault("FAULT_2", Fault::SEVERITY_WARN, "Persistent fault 2", "/node2", timestamp); + storage_->clear_fault("FAULT_2"); + + // Verify initial state + EXPECT_EQ(storage_->size(), 2u); + + // Close the storage + storage_.reset(); + + // Reopen the storage + storage_ = std::make_unique(temp_db_path_.string()); + + // Verify faults persisted + EXPECT_EQ(storage_->size(), 2u); + EXPECT_TRUE(storage_->contains("FAULT_1")); + EXPECT_TRUE(storage_->contains("FAULT_2")); + + auto fault1 = storage_->get_fault("FAULT_1"); + ASSERT_TRUE(fault1.has_value()); + EXPECT_EQ(fault1->severity, Fault::SEVERITY_ERROR); + EXPECT_EQ(fault1->status, Fault::STATUS_PENDING); + EXPECT_EQ(fault1->description, "Persistent fault 1"); + + auto fault2 = storage_->get_fault("FAULT_2"); + ASSERT_TRUE(fault2.has_value()); + EXPECT_EQ(fault2->status, Fault::STATUS_CLEARED); +} + +// Test timestamp precision +TEST_F(SqliteFaultStorageTest, TimestampPrecision) { + // Create a timestamp with nanosecond precision + int64_t test_ns = 1735312456123456789LL; // Specific nanosecond timestamp + rclcpp::Time timestamp(test_ns, RCL_SYSTEM_TIME); + + storage_->report_fault("FAULT_TS", Fault::SEVERITY_INFO, "Timestamp test", "/node1", timestamp); + + auto fault = storage_->get_fault("FAULT_TS"); + ASSERT_TRUE(fault.has_value()); + + // Convert builtin_interfaces::msg::Time back to rclcpp::Time for comparison + rclcpp::Time first_ts(fault->first_occurred); + rclcpp::Time last_ts(fault->last_occurred); + + // Verify nanosecond precision is preserved + EXPECT_EQ(first_ts.nanoseconds(), test_ns); + EXPECT_EQ(last_ts.nanoseconds(), test_ns); +} + +// Test in-memory SQLite database +TEST(SqliteInMemoryTest, InMemoryDatabase) { + SqliteFaultStorage storage(":memory:"); + rclcpp::Clock clock; + auto timestamp = clock.now(); + + storage.report_fault("MEM_FAULT", Fault::SEVERITY_WARN, "In-memory test", "/test", timestamp); + + EXPECT_EQ(storage.size(), 1u); + EXPECT_TRUE(storage.contains("MEM_FAULT")); +} + +// Test reporting sources JSON handling +TEST_F(SqliteFaultStorageTest, ReportingSourcesJsonHandling) { + rclcpp::Clock clock; + auto timestamp = clock.now(); + + // Add multiple sources for the same fault + storage_->report_fault("MULTI_SRC", Fault::SEVERITY_ERROR, "Multi-source", "/node/path/with/slashes", timestamp); + storage_->report_fault("MULTI_SRC", Fault::SEVERITY_ERROR, "Multi-source", "/another/node", timestamp); + storage_->report_fault("MULTI_SRC", Fault::SEVERITY_ERROR, "Multi-source", "/special\"chars", timestamp); + + auto fault = storage_->get_fault("MULTI_SRC"); + ASSERT_TRUE(fault.has_value()); + EXPECT_EQ(fault->reporting_sources.size(), 3u); + + // Verify all sources are present (order may vary due to set) + std::set sources(fault->reporting_sources.begin(), fault->reporting_sources.end()); + EXPECT_TRUE(sources.count("/node/path/with/slashes") > 0); + EXPECT_TRUE(sources.count("/another/node") > 0); + EXPECT_TRUE(sources.count("/special\"chars") > 0); +} + +// Test database path accessor +TEST_F(SqliteFaultStorageTest, DbPathAccessor) { + EXPECT_EQ(storage_->db_path(), temp_db_path_.string()); +} + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/src/ros2_medkit_fault_reporter/test/test_integration.test.py b/src/ros2_medkit_fault_reporter/test/test_integration.test.py index d39cd40..7975a67 100644 --- a/src/ros2_medkit_fault_reporter/test/test_integration.test.py +++ b/src/ros2_medkit_fault_reporter/test/test_integration.test.py @@ -58,12 +58,14 @@ def get_coverage_env(): def generate_test_description(): """Generate launch description with fault_manager node.""" # We need fault_manager running for FaultReporter to connect to + # Use in-memory storage to avoid filesystem permission issues in CI fault_manager_node = launch_ros.actions.Node( package='ros2_medkit_fault_manager', executable='fault_manager_node', name='fault_manager', output='screen', additional_env=get_coverage_env(), + parameters=[{'storage_type': 'memory'}], ) return ( diff --git a/src/ros2_medkit_gateway/test/test_integration.test.py b/src/ros2_medkit_gateway/test/test_integration.test.py index 4eff4fb..81260db 100644 --- a/src/ros2_medkit_gateway/test/test_integration.test.py +++ b/src/ros2_medkit_gateway/test/test_integration.test.py @@ -209,12 +209,14 @@ def generate_test_description(): ) # Launch the fault_manager node for fault REST API tests + # Use in-memory storage to avoid filesystem permission issues in CI fault_manager_node = launch_ros.actions.Node( package='ros2_medkit_fault_manager', executable='fault_manager_node', name='fault_manager', output='screen', additional_env=coverage_env, + parameters=[{'storage_type': 'memory'}], ) # Start demo nodes with a delay to ensure gateway starts first