From be8b46e4e4074a1d38c7fbcfd410161e844c05ea Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Wed, 26 Nov 2025 20:01:58 +0100 Subject: [PATCH] feat(database_node): Add config + database managers --- .../src/config/ConfigManager.cpp | 112 ++++++++++++ .../src/config/ConfigManager.hpp | 52 ++++++ .../src/config/DatabaseConfig.hpp | 46 +++++ .../src/database/DatabaseManager.cpp | 164 ++++++++++++++++++ .../src/database/DatabaseManager.hpp | 48 +++++ .../src/database/SQLQueries.hpp | 61 +++++++ 6 files changed, 483 insertions(+) create mode 100644 src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp create mode 100644 src/g2_2025_odometry_pkg/src/config/ConfigManager.hpp create mode 100644 src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp create mode 100644 src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp create mode 100644 src/g2_2025_odometry_pkg/src/database/DatabaseManager.hpp create mode 100644 src/g2_2025_odometry_pkg/src/database/SQLQueries.hpp diff --git a/src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp b/src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp new file mode 100644 index 0000000..164cbd4 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp @@ -0,0 +1,112 @@ +#include "ConfigManager.hpp" + +#include +#include + +namespace assignments::three { + +ConfigManager::ConfigManager(rclcpp::Logger logger) + : logger_(logger), loaded_(false) +{ + // Try to auto-load config from default location + std::string config_path = find_config_file(); + if (!config_path.empty()) { + load_config(config_path); + } +} + +bool ConfigManager::load_config(const std::string& config_file_path) { + try { + RCLCPP_INFO(logger_, "[CFG] '%s': loading configuration", config_file_path.c_str()); + + if (!std::filesystem::exists(config_file_path)) { + RCLCPP_ERROR(logger_, "[CFG] '%s': file does not exist", config_file_path.c_str()); + return false; + } + + config_ = toml::parse_file(config_file_path); + loaded_ = true; + + RCLCPP_INFO(logger_, "[CFG] '%s': configuration loaded", config_file_path.c_str()); + return true; + + } catch (const toml::parse_error& e) { + RCLCPP_ERROR(logger_, "[CFG] '%s': failed to parse: %s", config_file_path.c_str(), e.what()); + loaded_ = false; + return false; + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "[CFG] '%s': failed to load: %s", config_file_path.c_str(), e.what()); + loaded_ = false; + return false; + } +} + +std::optional ConfigManager::get_database_config() const { + if (!loaded_ || !config_.has_value()) { + RCLCPP_ERROR(logger_, "[CFG] database configuration not loaded"); + return std::nullopt; + } + + try { + return parse_database_config(config_.value()); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "[CFG] database configuration failed to parse: %s", e.what()); + return std::nullopt; + } +} + +bool ConfigManager::is_loaded() const { + return loaded_; +} + +std::string ConfigManager::find_config_file() const { + // Look for config file in several locations + for (const auto& path : default_config_paths_) { + if (std::filesystem::exists(path)) { + RCLCPP_INFO(logger_, "[CFG] '%s': found configuration file", path.c_str()); + return path; + } + } + + RCLCPP_WARN(logger_, "[CFG] no configuration file found at default locations"); + return ""; +} + +DatabaseConfig ConfigManager::parse_database_config(const toml::table& config) const { + DatabaseConfig db_config; + + // Parse database section + auto database_section = config["database"]; + if (!database_section) { + throw std::runtime_error("missing [database] section in configuration"); + } + + db_config.host = database_section["host"].value_or("localhost"); + db_config.port = database_section["port"].value_or(5432); + db_config.dbname = database_section["dbname"].value_or("imu_data"); + db_config.user = database_section["user"].value_or("postgres"); + db_config.password = database_section["password"].value_or("postgres"); + db_config.timeout = database_section["timeout"].value_or(30); + db_config.ssl = database_section["ssl"].value_or(false); + + // Parse pool section if present + auto pool_section = database_section["pool"]; + if (pool_section) { + db_config.min_connections = pool_section["min_connections"].value_or(1); + db_config.max_connections = pool_section["max_connections"].value_or(10); + } else { + db_config.min_connections = 1; + db_config.max_connections = 10; + } + + RCLCPP_INFO(logger_, "[CFG] database config parsed - %s:%s@%s:%d", + db_config.user.c_str(), + db_config.dbname.c_str(), + db_config.host.c_str(), + db_config.port + ); + + return db_config; +} + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/config/ConfigManager.hpp b/src/g2_2025_odometry_pkg/src/config/ConfigManager.hpp new file mode 100644 index 0000000..0cf5887 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/config/ConfigManager.hpp @@ -0,0 +1,52 @@ +/* ConfigManager.hpp + * Configuration management for the exam result generator + * Uses toml++ library to parse TOML configuration files + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Created configuration manager class for TOML config loading + */ +#pragma once + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "DatabaseConfig.hpp" + +namespace assignments::three { + +class ConfigManager { +public: + explicit ConfigManager(rclcpp::Logger logger); + ~ConfigManager() = default; + + bool load_config(const std::string& config_file_path); + + std::optional get_database_config() const; + + bool is_loaded() const; + +private: + rclcpp::Logger logger_; + + std::optional config_; + + bool loaded_ { false }; + std::vector default_config_paths_ = { + "config.toml", + "./src/config.toml", + "../config.toml", + "../../config.toml", + "../../../config.toml", + "../../../../config.toml", + "/etc/ros2_grade_calculator/config.toml" + }; + + std::string find_config_file() const; + DatabaseConfig parse_database_config(const toml::table& config) const; +}; + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp b/src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp new file mode 100644 index 0000000..04203e8 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp @@ -0,0 +1,46 @@ +/* DatabaseConfig.hpp + * Database configuration structure + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Create initial configuration structure + */ +#pragma once + +#include + +namespace assignments::three { + +struct DatabaseConfig { + std::string host; + int port; + std::string dbname; + std::string user; + std::string password; + int timeout; + bool ssl; + + // Pool settings + int min_connections; + int max_connections; + + std::string to_connection_string() const { + std::string conn_str = + "host=" + host + + " port=" + std::to_string(port) + + " dbname=" + dbname + + " user=" + user + + " password=" + password + + " connect_timeout=" + std::to_string(timeout); + + if (ssl) { + conn_str += " sslmode=require"; + } else { + conn_str += " sslmode=disable"; + } + + return conn_str; + } +}; + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp new file mode 100644 index 0000000..4912bf1 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp @@ -0,0 +1,164 @@ +#include "DatabaseManager.hpp" + +#include +#include "rclcpp/rclcpp.hpp" + +#include "SQLQueries.hpp" +#include "config/ConfigManager.hpp" + +namespace assignments::three { + +DatabaseManager::DatabaseManager(rclcpp::Logger logger) : logger_(logger) { + config_manager_ = std::make_unique(logger_); + init_database(); +} + +bool DatabaseManager::is_connected() const { + return conn_ && conn_->is_open(); +} + +bool DatabaseManager::connect(const std::string& connection_string) { + try { + RCLCPP_INFO(logger_, "[DBS] connecting to PostgreSQL database..."); + + conn_ = std::make_unique(connection_string); + + if (conn_->is_open()) { + RCLCPP_INFO(logger_, "[DBS] '%s': successfully connected to database", conn_->dbname()); + return true; + } else { + RCLCPP_ERROR(logger_, "[DBS] failed to open database connection"); + return false; + } + + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] sql error: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] connection error: %s", e.what()); + return false; + } +} + +void DatabaseManager::init_database() { + if (!config_manager_ || !config_manager_->is_loaded()) { + RCLCPP_ERROR(logger_, "[DBS] configuration not loaded, cannot initialize database"); + return; + } + + auto db_config = config_manager_->get_database_config(); + if (!db_config.has_value()) { + RCLCPP_ERROR(logger_, "[DBS] failed to get database configuration"); + return; + } + + std::string connection_string = db_config->to_connection_string(); + + if (connect(connection_string)) { + create_tables(); + } +} + +void DatabaseManager::create_tables() { + if (!conn_ || !conn_->is_open()) return; + + try { + pqxx::work txn(*conn_); + + txn.exec(SQL_CREATE_IMU_DATA_TABLE); + txn.exec(SQL_CREATE_POSITION_DATA_TABLE); + txn.exec(SQL_CREATE_VELOCITY_DATA_TABLE); + + txn.commit(); + + RCLCPP_INFO(logger_, "[DBS] database tables ready"); + + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] CREATE TABLE failed: %s", e.what()); + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + } +} + +bool DatabaseManager::store_imu_data( + double linear_accel_x, double linear_accel_y, double linear_accel_z, + double angular_vel_x, double angular_vel_y, double angular_vel_z +) { + if (!is_connected()) { + RCLCPP_WARN(logger_, "[DBS] not connected to database"); + return false; + } + + try { + pqxx::work txn(*conn_); + + txn.exec_params(SQL_INSERT_IMU_DATA, + linear_accel_x, linear_accel_y, linear_accel_z, + angular_vel_x, angular_vel_y, angular_vel_z + ); + + txn.commit(); + + RCLCPP_DEBUG(logger_, "[DBS] stored IMU data: accel=[%.3f,%.3f,%.3f], angular=[%.3f,%.3f,%.3f]", + linear_accel_x, linear_accel_y, linear_accel_z, + angular_vel_x, angular_vel_y, angular_vel_z + ); + + return true; + + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] failed to store IMU data: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + return false; + } +} + +bool DatabaseManager::store_position_data(double x, double y, double theta) { + if (!is_connected()) { + RCLCPP_WARN(logger_, "[DBS] not connected to database"); + return false; + } + + try { + pqxx::work txn(*conn_); + txn.exec_params(SQL_INSERT_POSITION_DATA, x, y, theta); + txn.commit(); + + RCLCPP_DEBUG(logger_, "[DBS] stored position data=[%.3f, %.3f, %.3f]", x, y, theta); + + return true; + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] failed to store position data: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + return false; + } +} + +bool DatabaseManager::store_velocity_data(double vx, double vy, double vz, double omega_z) { + if (!is_connected()) { + RCLCPP_WARN(logger_, "[DBS] not connected to database"); + return false; + } + + try { + pqxx::work txn(*conn_); + txn.exec_params(SQL_INSERT_VELOCITY_DATA, vx, vy, vz, omega_z); + txn.commit(); + + RCLCPP_DEBUG(logger_, "[DBS] stored velocity data=[%.3f, %.3f, %.3f, %.3f]", vx, vy, vz, omega_z); + + return true; + } catch (const pqxx::sql_error &e) { + RCLCPP_ERROR(logger_, "[DBS] failed to store velocity data: %s", e.what()); + return false; + } catch (const std::exception &e) { + RCLCPP_ERROR(logger_, "[DBS] database error: %s", e.what()); + return false; + } +} + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/database/DatabaseManager.hpp b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.hpp new file mode 100644 index 0000000..e6fe8d1 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/database/DatabaseManager.hpp @@ -0,0 +1,48 @@ +/* DatabaseManager.hpp + * Database manager for the IMU data collection system + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Created database manager class for all DB operations + * [14-10-2025] Wessel T: Updated for IMU data storage functionality + */ +#pragma once + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "config/ConfigManager.hpp" + +namespace assignments::three { + +class DatabaseManager { +public: + explicit DatabaseManager(rclcpp::Logger logger); + ~DatabaseManager() = default; + + bool connect(const std::string& connection_string); + virtual bool is_connected() const; + + // Table operations + virtual void init_database(); + void create_tables(); + + // IMU Data operations + virtual bool store_position_data(double x, double y, double theta); + virtual bool store_velocity_data(double vx, double vy, double vz, double omega_z); + virtual bool store_imu_data( + double linear_accel_x, double linear_accel_y, double linear_accel_z, + double angular_vel_x, double angular_vel_y, double angular_vel_z + ); + +private: + rclcpp::Logger logger_; + + std::unique_ptr conn_; + std::unique_ptr config_manager_; +}; + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/database/SQLQueries.hpp b/src/g2_2025_odometry_pkg/src/database/SQLQueries.hpp new file mode 100644 index 0000000..65eb1b0 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/database/SQLQueries.hpp @@ -0,0 +1,61 @@ +/* SQLQueries.hpp + * SQL query definitions for the IMU data collection system + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Created initial database state + * [14-10-2025] Wessel T: Updated for IMU data storage + * [26-11-2025] Wessel T: Added position and velocity measurement tables + */ +#pragma once + +static const std::string SQL_CREATE_IMU_DATA_TABLE = R"( + CREATE TABLE IF NOT EXISTS imu_data ( + id SERIAL PRIMARY KEY, + timestamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP, + linear_accel_x REAL NOT NULL, + linear_accel_y REAL NOT NULL, + linear_accel_z REAL NOT NULL, + angular_vel_x REAL NOT NULL, + angular_vel_y REAL NOT NULL, + angular_vel_z REAL NOT NULL + ); +)"; + +static const std::string SQL_CREATE_POSITION_DATA_TABLE = R"( + CREATE TABLE IF NOT EXISTS position_measurements ( + id SERIAL PRIMARY KEY, + timestamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP, + x REAL NOT NULL, + y REAL NOT NULL, + theta REAL NOT NULL + ); +)"; + +static const std::string SQL_CREATE_VELOCITY_DATA_TABLE = R"( + CREATE TABLE IF NOT EXISTS velocity_measurements ( + id SERIAL PRIMARY KEY, + timestamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP, + vx REAL NOT NULL, + vy REAL NOT NULL, + vz REAL NOT NULL, + omega_z REAL NOT NULL + ); +)"; + +static const std::string SQL_INSERT_IMU_DATA = R"( + INSERT INTO imu_data ( + linear_accel_x, linear_accel_y, linear_accel_z, + angular_vel_x, angular_vel_y, angular_vel_z + ) VALUES ($1, $2, $3, $4, $5, $6); +)"; + +static const std::string SQL_INSERT_POSITION_DATA = R"( + INSERT INTO position_measurements (x, y, theta) + VALUES ($1, $2, $3); +)"; + +static const std::string SQL_INSERT_VELOCITY_DATA = R"( + INSERT INTO velocity_measurements (vx, vy, vz, omega_z) + VALUES ($1, $2, $3, $4); +)";