feat(docs): Add documentation for database handler and imu approximator

doc/architecture/nodes/DatabaseHandlerNode.md

@@ -0,0 +1,41 @@
+# DatabaseHandlerNode (`assignments::three::g2_2025_database_node`)
+
+The `DatabaseHandlerNode` subscribes to position and velocity topics and stores the data in a
+PostgreSQL database via the `DatabaseManager`.
+
+## Implementation Details
+
+**Parameters**
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `save_position_data` | bool | true | Enable storage of position |
+| `save_velocity_data` | bool | true | Enable storage of velocity |
+
+**Constructor**
+```cpp
+DatabaseHandlerNode()
+```
+- Initializes ROS2 node with name `database_handler_node`
+- Creates subscriptions to `estimated_position` and/or `estimated_velocity` based on parameter configuration
+
+## Functions
+
+**`void position_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg)`**
+- Primary callback invoked whenever a position message is received
+- Forwards: x, y, and theta to `DatabaseManager::store_position_data`
+- Throttled warning (5 second interval) on storage failures
+
+**`void velocity_callback(const geometry_msgs::msg::Twist::SharedPtr msg)`**
+- Primary callback invoked whenever a velocity message is received
+- Forwards: linear velocity (x, y, z) and angular velocity (z) to `DatabaseManager::store_velocity_data`
+- Throttled warning (5 second interval) on storage failures
+
+## ROS2 Interface
+
+**Subscriptions**
+- `estimated_position` (geometry_msgs/msg/Pose2D)
+  - Receives calculated position estimates (only subscribed if `save_position_data=true`)
+
+- `estimated_velocity` (geometry_msgs/msg/Twist)
+  - Receives calculated velocity estimates (only subscribed if `save_velocity_data=true`)

doc/architecture/nodes/IMUPositionApproximator.md

@@ -0,0 +1,58 @@
+# IMUPositionApproximator (`assignments::three::g2_2025_imu_position_approximator_node`)
+
+The `IMUPositionApproximator` node calculates position and velocity estimates by integrating
+acceleration data from an IMU sensor. It transforms sensor data from the robot frame to the
+map frame, accounting for rotations and centripetal effects, and provides a position reset
+topic.
+
+## Implementation Details
+
+**Parameters**
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `initial_x` | double | 0.0 | Initial x position in map frame |
+| `initial_y` | double | 0.0 | Initial y position in map frame |
+| `initial_z` | double | 0.0 | Initial z position in map frame |
+| `initial_theta` | double | 0.0 | Initial orientation angle (radians) |
+| `imu_topic` | string | "imu_data" | Topic name for IMU sensor data |
+
+**Constructor**
+```cpp
+IMUPositionApproximator()
+```
+- Initializes ROS2 node with name `imu_position_approximator`
+- Creates subscription to IMU topic (configurable) and `position_reset` topic
+- Creates publishers for `estimated_position` and `estimated_velocity`
+
+## Functions
+
+**`void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)`**
+- Primary callback invoked whenever an IMU message is received
+- Calculates time delta `dt` between measurements; skips invalid deltas (≤0 or >1 second)
+- Updates orientation: `theta += omega_z * dt` and normalizes to [-pi, pi]
+- Applies gravity compensation: subtracts 9.81 m/s^2 from z-axis acceleration
+- Transforms acceleration from robot frame to map frame using rotation matrix
+- Corrects for centripetal acceleration due to rotation
+- Integrates acceleration to update velocity, then integrates velocity to update position
+- Publishes `estimated_position` (Pose2D) and `estimated_velocity` (Twist)
+
+**`void position_reset_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg)`**
+- Updates position (x, y) and orientation (theta) to provided values
+- Resets all velocities (vx, vy, vz) to zero
+
+## ROS2 Interface
+
+**Subscribers**
+- `imu_data` (sensor_msgs/msg/Imu) [configurable via `imu_topic` parameter]
+  - Receives raw IMU sensor data with linear acceleration and angular velocity
+
+- `position_reset` (geometry_msgs/msg/Pose2D)
+  - Receives position reset commands from external position determinator
+
+**Publishers**
+- `estimated_position` (geometry_msgs/msg/Pose2D)
+  - Publishes calculated position (x, y, theta) in map frame
+
+- `estimated_velocity` (geometry_msgs/msg/Twist)
+  - Publishes calculated linear velocities (x, y, z) and angular velocity (z)

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -5,22 +5,65 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+
+# external packages
+include(FetchContent)
+
+FetchContent_Declare(
+    tomlplusplus
+    GIT_REPOSITORY https://github.com/marzer/tomlplusplus.git
+    GIT_TAG v3.4.0
+)
+
+FetchContent_MakeAvailable(tomlplusplus)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
+find_package(rclcpp REQUIRED)
-# further dependencies manually.
+find_package(sensor_msgs REQUIRED)
-# find_package(<dependency> REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
-if(BUILD_TESTING)
+# Add executable for position_speed_approximator_node
-  find_package(ament_lint_auto REQUIRED)
+add_executable(position_speed_approximator_node
-  # the following line skips the linter which checks for copyrights
+  src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.cpp
-  # comment the line when a copyright and license is added to all source files
+  src/g2_2025_imu_position_approximator_node/Main.cpp
-  set(ament_cmake_copyright_FOUND TRUE)
+)
-  # the following line skips cpplint (only works in a git repo)
+
-  # comment the line when this package is in a git repo and when
+target_include_directories(position_speed_approximator_node PRIVATE
-  # a copyright and license is added to all source files
+  ${CMAKE_CURRENT_SOURCE_DIR}/src
-  set(ament_cmake_cpplint_FOUND TRUE)
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_position_approximator_node
-  ament_lint_auto_find_test_dependencies()
+)
-endif()
+
+ament_target_dependencies(position_speed_approximator_node
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+)
+
+add_executable(database_handler_node
+  src/g2_2025_database_node/Main.cpp
+  src/g2_2025_database_node/nodes/DatabaseHandlerNode.cpp
+  src/database/DatabaseManager.cpp
+  src/config/ConfigManager.cpp
+)
+
+target_include_directories(database_handler_node PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/src
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_database_node
+)
+
+target_link_libraries(database_handler_node tomlplusplus::tomlplusplus pqxx pq)
+
+ament_target_dependencies(database_handler_node
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+)
+
+install(TARGETS
+  position_speed_approximator_node
+  database_handler_node
+  DESTINATION lib/${PROJECT_NAME}
+)
 
 ament_package()

src/g2_2025_odometry_pkg/package.xml

@@ -9,6 +9,10 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/g2_2025_odometry_pkg/src/config/ConfigManager.cpp

@@ -0,0 +1,112 @@
+#include "ConfigManager.hpp"
+
+#include <iostream>
+#include <fstream>
+
+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<DatabaseConfig> 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<std::string>("localhost");
+    db_config.port = database_section["port"].value_or<int>(5432);
+    db_config.dbname = database_section["dbname"].value_or<std::string>("imu_data");
+    db_config.user = database_section["user"].value_or<std::string>("postgres");
+    db_config.password = database_section["password"].value_or<std::string>("postgres");
+    db_config.timeout = database_section["timeout"].value_or<int>(30);
+    db_config.ssl = database_section["ssl"].value_or<bool>(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<int>(1);
+        db_config.max_connections = pool_section["max_connections"].value_or<int>(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

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: <x>
+ * Changelog:
+ * [23-09-2025] Wessel T: Created configuration manager class for TOML config loading
+ */
+#pragma once
+
+#include <string>
+#include <optional>
+#include <filesystem>
+#include <toml++/toml.h>
+#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<DatabaseConfig> get_database_config() const;
+
+    bool is_loaded() const;
+
+private:
+    rclcpp::Logger logger_;
+
+    std::optional<toml::table> config_;
+
+    bool loaded_ { false };
+    std::vector<std::string> 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

src/g2_2025_odometry_pkg/src/config/DatabaseConfig.hpp

@@ -0,0 +1,46 @@
+/* DatabaseConfig.hpp
+ * Database configuration structure
+ *
+ * Reviewed by: <x>
+ * Changelog:
+ * [23-09-2025] Wessel T: Create initial configuration structure
+ */
+#pragma once
+
+#include <string>
+
+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

src/g2_2025_odometry_pkg/src/database/DatabaseManager.cpp

@@ -0,0 +1,164 @@
+#include "DatabaseManager.hpp"
+
+#include <pqxx/pqxx>
+#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<ConfigManager>(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<pqxx::connection>(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

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: <x>
+ * 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 <memory>
+#include <vector>
+#include <string>
+#include <pqxx/pqxx>
+#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<pqxx::connection> conn_;
+    std::unique_ptr<ConfigManager> config_manager_;
+};
+
+} // namespace assignments::three

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: <x>
+ * 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);
+)";

src/g2_2025_odometry_pkg/src/g2_2025_database_node/Main.cpp

@@ -0,0 +1,14 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include "nodes/DatabaseHandlerNode.hpp"
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<assignments::three::g2_2025_database_node::DatabaseHandlerNode>();
+
+    rclcpp::spin(node->get_node_base_interface());
+    rclcpp::shutdown();
+
+    return 0;
+}

src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.cpp

@@ -0,0 +1,84 @@
+#include "DatabaseHandlerNode.hpp"
+
+namespace assignments::three::g2_2025_database_node {
+
+DatabaseHandlerNode::DatabaseHandlerNode(): Node("database_handler_node") {
+    RCLCPP_INFO(this->get_logger(), "database handler Node starting");
+
+    save_position_data_ = this->declare_parameter<bool>("save_position_data", true);
+    save_velocity_data_ = this->declare_parameter<bool>("save_velocity_data", true);
+
+    db_manager_ = std::make_unique<assignments::three::DatabaseManager>(this->get_logger());
+
+    if (!db_manager_->is_connected()) {
+        RCLCPP_ERROR(this->get_logger(), "failed to connect to database");
+        return;
+    }
+
+    if (save_position_data_) {
+        position_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose2D>(
+            "estimated_position", 10,
+            std::bind(
+                &DatabaseHandlerNode::position_callback,
+                this,
+                std::placeholders::_1
+            )
+        );
+
+        RCLCPP_INFO(this->get_logger(), "subscribed to 'estimated_position'");
+    }
+
+    if (save_velocity_data_) {
+        velocity_subscriber_ = this->create_subscription<geometry_msgs::msg::Twist>(
+            "estimated_velocity", 10,
+            std::bind(
+                &DatabaseHandlerNode::velocity_callback,
+                this,
+                std::placeholders::_1
+            )
+        );
+        RCLCPP_INFO(this->get_logger(), "subscribed to 'estimated_velocity'");
+    }
+
+    RCLCPP_INFO(this->get_logger(), "database node ready to receive data on topics");
+}
+
+void DatabaseHandlerNode::position_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg) {
+    if (!db_manager_->is_connected()) {
+        return;
+    }
+
+    bool success = db_manager_->store_position_data(msg->x, msg->y, msg->theta);
+
+    if (!success) {
+        RCLCPP_WARN_THROTTLE(this->get_logger(),
+            *this->get_clock(),
+            5000,
+            "failed to store position data in database"
+        );
+    } else {
+        RCLCPP_DEBUG(this->get_logger(), "Stored position=[%.3f, %.3f, %.3f]", msg->x, msg->y, msg->theta);
+    }
+}
+
+void DatabaseHandlerNode::velocity_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
+    if (!db_manager_->is_connected()) {
+        return;
+    }
+
+    bool success = db_manager_->store_velocity_data(msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.z);
+
+    if (!success) {
+        RCLCPP_WARN_THROTTLE(this->get_logger(),
+            *this->get_clock(),
+            5000,
+            "failed to store velocity data in database"
+        );
+    } else {
+        RCLCPP_DEBUG(this->get_logger(), "Stored velocity=[%.3f, %.3f, %.3f, %.3f]",
+            msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.z
+        );
+    }
+}
+
+} // namespace assignments::three::g2_2025_database_node

src/g2_2025_odometry_pkg/src/g2_2025_database_node/nodes/DatabaseHandlerNode.hpp

@@ -0,0 +1,39 @@
+/* DatabaseHandlerNode.hpp
+ * Database handler node for subscribing to topics and storing data
+ *
+ * Changelog:
+ * [26-11-2025] Created database handler node for position and sensor data storage
+ */
+#pragma once
+
+#include "database/DatabaseManager.hpp"
+
+#include <memory>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <geometry_msgs/msg/pose2_d.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+
+namespace assignments::three::g2_2025_database_node {
+
+class DatabaseHandlerNode : public rclcpp::Node {
+public:
+    DatabaseHandlerNode();
+    ~DatabaseHandlerNode() = default;
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
+    rclcpp::Subscription<geometry_msgs::msg::Pose2D>::SharedPtr position_subscriber_;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr velocity_subscriber_;
+
+    std::unique_ptr<assignments::three::DatabaseManager> db_manager_;
+
+    bool save_position_data_ { true };
+    bool save_velocity_data_ { true };
+
+    void position_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg);
+    void velocity_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
+};
+
+} // namespace assignments::three::g2_2025_database_node

src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/Main.cpp

@@ -0,0 +1,14 @@
+#include "rclcpp/rclcpp.hpp"
+
+#include "nodes/IMUPositionApproximator.hpp"
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<assignments::three::g2_2025_imu_position_approximator_node::IMUPositionApproximator>();
+
+    rclcpp::spin(node->get_node_base_interface());
+    rclcpp::shutdown();
+
+    return 0;
+}

src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.cpp

@@ -0,0 +1,126 @@
+#include "IMUPositionApproximator.hpp"
+
+namespace assignments::three::g2_2025_imu_position_approximator_node {
+
+IMUPositionApproximator::IMUPositionApproximator()
+: Node("imu_position_approximator")
+, x_(0.0), y_(0.0), z_(0.0)
+, theta_(0.0)
+, vx_(0.0), vy_(0.0), vz_(0.0)
+, last_time_(this->now())
+{
+    RCLCPP_INFO(this->get_logger(), "IMU Position Approximator node started");
+
+    // Get initial position from parameters
+    x_ = this->declare_parameter<double>("initial_x", 0.0);
+    y_ = this->declare_parameter<double>("initial_y", 0.0);
+    z_ = this->declare_parameter<double>("initial_z", 0.0);
+    theta_ = this->declare_parameter<double>("initial_theta", 0.0);
+    imu_topic_ = this->declare_parameter<std::string>("imu_topic", "imu_data");
+
+    RCLCPP_INFO(this->get_logger(),
+        "Initial position set to: x=%.3f, y=%.3f, z=%.3f, theta=%.3f rad",
+        x_, y_, z_, theta_);
+
+
+    imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>(
+        imu_topic_, 10,
+        std::bind(&IMUPositionApproximator::imu_callback, this, std::placeholders::_1)
+    );
+
+    position_reset_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose2D>(
+        "position_reset", 10,
+        std::bind(&IMUPositionApproximator::position_reset_callback, this, std::placeholders::_1)
+    );
+
+    RCLCPP_INFO(this->get_logger(), "Subscribed to '%s' and 'position_reset'", imu_topic_.c_str());
+
+    position_publisher_ = this->create_publisher<geometry_msgs::msg::Pose2D>("estimated_position", 10);
+    velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("estimated_velocity", 10);
+
+    RCLCPP_INFO(this->get_logger(), "Publishing to 'estimated_position' and 'estimated_velocity'");
+}
+
+void IMUPositionApproximator::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    auto current_time = this->now();
+    double dt = (current_time - last_time_).seconds();
+    last_time_ = current_time;
+
+    if (dt <= 0.0 || dt > 1.0) {
+        RCLCPP_WARN(this->get_logger(), "skipping iteration due to invalid delta time (dt=%.3f)", dt);
+        return;
+    }
+
+    Position pos_robot;
+    pos_robot.x = msg->linear_acceleration.x;
+    pos_robot.y = msg->linear_acceleration.y;
+    // remove gravity, standard gravity is ~9.81 m/s^2
+    pos_robot.z = msg->linear_acceleration.z - 9.81;
+
+    // calculate and normalize theta_ to be within [-pi, pi]
+    double omega_z = msg->angular_velocity.z;
+    theta_ += omega_z * dt;
+
+    while (theta_ > M_PI) theta_ -= 2.0 * M_PI;
+    while (theta_ < -M_PI) theta_ += 2.0 * M_PI;
+
+    // transform from robot frame to map frame
+    Position pos_map;
+    double cos_theta = std::cos(theta_);
+    double sin_theta = std::sin(theta_);
+    pos_map.x = pos_robot.x * cos_theta - pos_robot.y * sin_theta;
+    pos_map.y = pos_robot.x * sin_theta + pos_robot.y * cos_theta;
+    pos_map.z = pos_robot.z;
+
+    // account for centripetal acceleration due to rotation when rotating,
+    // linear velocity creates centripetal acceleration
+    pos_map.x -= -omega_z * vy_;
+    pos_map.y -= omega_z * vx_;
+
+    // integrate acceleration to get velocity
+    vx_ += pos_map.x * dt;
+    vy_ += pos_map.y * dt;
+    vz_ += pos_map.z * dt;
+
+    // integrate velocity to get position
+    x_ += vx_ * dt;
+    y_ += vy_ * dt;
+    z_ += vz_ * dt;
+
+    auto position_msg = geometry_msgs::msg::Pose2D();
+    position_msg.x = x_;
+    position_msg.y = y_;
+    position_msg.theta = theta_;
+    position_publisher_->publish(position_msg);
+
+    auto velocity_msg = geometry_msgs::msg::Twist();
+    velocity_msg.linear.x = vx_;
+    velocity_msg.linear.y = vy_;
+    velocity_msg.linear.z = vz_;
+    velocity_msg.angular.z = omega_z;
+    velocity_publisher_->publish(velocity_msg);
+
+    RCLCPP_INFO(this->get_logger(),
+        "Position: (%.3f, %.3f, %.3f) m, θ=%.3f rad | Velocity: (%.3f, %.3f, %.3f) m/s",
+        x_, y_, z_, theta_, vx_, vy_, vz_
+    );
+}
+
+void IMUPositionApproximator::position_reset_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg) {
+    RCLCPP_INFO(this->get_logger(),
+        "resetting position, was: (%.3f, %.3f, %.3f), now: (%.3f, %.3f, %.3f)",
+        x_, y_, theta_, msg->x, msg->y, msg->theta
+    );
+
+    x_ = msg->x;
+    y_ = msg->y;
+    theta_ = msg->theta;
+
+    vx_ = 0.0;
+    vy_ = 0.0;
+    vz_ = 0.0;
+
+    last_time_ = this->now();
+}
+
+} // namespace assignments::three::g2_2025_imu_position_approximator_node

src/g2_2025_odometry_pkg/src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.hpp

@@ -0,0 +1,36 @@
+#include <cmath>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace assignments::three::g2_2025_imu_position_approximator_node {
+
+struct Position {
+    double x;
+    double y;
+    double z;
+};
+
+class IMUPositionApproximator : public rclcpp::Node {
+public:
+    IMUPositionApproximator();
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
+    rclcpp::Subscription<geometry_msgs::msg::Pose2D>::SharedPtr position_reset_subscriber_;
+    rclcpp::Publisher<geometry_msgs::msg::Pose2D>::SharedPtr position_publisher_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
+
+    std::string imu_topic_ { "imu_data" };
+    double x_, y_, z_;      // Position
+    double theta_;          // Orientation (yaw angle)
+    double vx_, vy_, vz_;   // Velocity
+    rclcpp::Time last_time_;
+
+    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
+    void position_reset_callback(const geometry_msgs::msg::Pose2D::SharedPtr msg);
+};
+
+} // namespace assignments::three::g2_2025_imu_position_approximator_node