fix(serial): Correct uart messages

Reviewed-on: http://git.wessel.gg/inholland/ros2-assignments/pulls/11

.editorconfig

@@ -1,66 +1,75 @@
-root = true
-
 [*]
-indent_style = space
-indent_size = 2
-charset = utf-8
-trim_trailing_whitespace = true
-insert_final_newline = true
 cpp_indent_braces=false
 cpp_indent_multi_line_relative_to=innermost_parenthesis
 cpp_indent_within_parentheses=indent
-cpp_indent_preserve_within_parentheses=false
-cpp_indent_case_labels=false
+cpp_indent_preserve_within_parentheses=true
+cpp_indent_case_labels=true
 cpp_indent_case_contents=true
 cpp_indent_case_contents_when_block=false
 cpp_indent_lambda_braces_when_parameter=true
 cpp_indent_goto_labels=one_left
 cpp_indent_preprocessor=leftmost_column
 cpp_indent_access_specifiers=false
-cpp_indent_namespace_contents=true
+cpp_indent_namespace_contents=false
 cpp_indent_preserve_comments=false
-cpp_new_line_before_open_brace_namespace=ignore
-cpp_new_line_before_open_brace_type=ignore
+
+cpp_new_line_before_open_brace_namespace=false
+cpp_new_line_before_open_brace_type=false
 cpp_new_line_before_open_brace_function=ignore
 cpp_new_line_before_open_brace_block=ignore
 cpp_new_line_before_open_brace_lambda=ignore
+
 cpp_new_line_scope_braces_on_separate_lines=false
 cpp_new_line_close_brace_same_line_empty_type=false
 cpp_new_line_close_brace_same_line_empty_function=false
-cpp_new_line_before_catch=true
-cpp_new_line_before_else=true
+cpp_new_line_before_catch=false
+cpp_new_line_before_else=false
 cpp_new_line_before_while_in_do_while=false
+
 cpp_space_before_function_open_parenthesis=remove
 cpp_space_within_parameter_list_parentheses=false
 cpp_space_between_empty_parameter_list_parentheses=false
 cpp_space_after_keywords_in_control_flow_statements=true
 cpp_space_within_control_flow_statement_parentheses=false
 cpp_space_before_lambda_open_parenthesis=false
+
 cpp_space_within_cast_parentheses=false
 cpp_space_after_cast_close_parenthesis=false
 cpp_space_within_expression_parentheses=false
 cpp_space_before_block_open_brace=true
 cpp_space_between_empty_braces=false
-cpp_space_before_initializer_list_open_brace=false
+cpp_space_before_initializer_list_open_brace=true
 cpp_space_within_initializer_list_braces=true
 cpp_space_preserve_in_initializer_list=true
+
 cpp_space_before_open_square_bracket=false
 cpp_space_within_square_brackets=false
 cpp_space_before_empty_square_brackets=false
 cpp_space_between_empty_square_brackets=false
 cpp_space_group_square_brackets=true
+
 cpp_space_within_lambda_brackets=false
 cpp_space_between_empty_lambda_brackets=false
 cpp_space_before_comma=false
 cpp_space_after_comma=true
+
 cpp_space_remove_around_member_operators=true
+
 cpp_space_before_inheritance_colon=true
 cpp_space_before_constructor_colon=true
+
 cpp_space_remove_before_semicolon=true
-cpp_space_after_semicolon=false
+cpp_space_after_semicolon=true
+
 cpp_space_remove_around_unary_operator=true
+
 cpp_space_around_binary_operator=insert
 cpp_space_around_assignment_operator=insert
 cpp_space_pointer_reference_alignment=left
 cpp_space_around_ternary_operator=insert
 cpp_wrap_preserve_blocks=one_liners
+indent_style = space
+indent_size = 4
+charset = utf-8
+trim_trailing_whitespace = true
+insert_final_newline = true

.fish/README.md

@@ -0,0 +1,71 @@
+# Exported Fish Environment: ros2-assignments
+
+This directory contains a self-contained Fish shell environment that can be used
+without requiring the original Fish configuration.
+
+## Files Structure
+```
+.fish/
+├── activate.fish    # Main environment configuration
+└── README.md        # This file
+```
+
+## Usage
+
+### Automatic Activation (Recommended)
+The environment will automatically activate when you `cd` into this directory
+if your Fish shell is configured with the auto-activation feature that checks
+for `.fish/activate.fish`.
+
+### Manual Activation
+To manually activate the environment, run from the project root:
+```bash
+source ./.fish/activate.fish
+```
+
+### Deactivation
+To deactivate the environment, run:
+```bash
+env deactivate
+```
+
+Or simply `cd` to a different directory if using auto-activation.
+
+## What This Environment Provides
+
+- Custom prompt showing the environment name
+- Environment-specific aliases and functions
+- Custom environment variables
+- Automatic cleanup when deactivated
+
+## Requirements
+
+- Fish shell
+- If this is a ROS2 environment: `bass` plugin (`fisher install edc/bass`)
+
+## Sharing
+
+This environment is completely self-contained. You can:
+1. Copy this directory to another machine
+2. Share it with other Fish shell users
+3. Version control it with your project (add .fish/ to your repo)
+
+The environment will work on any system with Fish shell, regardless of whether
+they have the original environment management system installed.
+
+## Auto-activation Setup
+
+To enable auto-activation for .fish/activate.fish, add this to your Fish config.fish:
+```fish
+function check_and_source_activate
+  if test -f (pwd)/.fish/activate.fish
+    source (pwd)/.fish/activate.fish
+  elif test -f (pwd)/activate.fish
+    source (pwd)/activate.fish
+  end
+end
+
+function cd
+  builtin cd $argv && check_and_source_activate
+end
+```

.fish/activate.fish

@@ -0,0 +1,74 @@
+# Self-contained environment: ros2-assignments
+# Exported on: Tue Sep 23 10:20:42 AM CEST 2025
+# Original environment from: /home/wessel/.config/fish/environments/configs/ros2-assignments
+
+# ROS2 development environment (requires distrobox)
+# Environment: ros2-assignments
+
+# First check if running inside distrobox
+if not test -f /run/.containerenv; and test -z "$CONTAINER_ID"
+    echo (set_color red)"This ROS2 environment should only be run inside a distrobox container"(set_color normal)
+    return 1
+end
+
+# Check if a previous initialization has occurred
+if test -n "$__ENV_INITIALIZED"
+    echo (set_color yellow)"Environment already initialized"(set_color normal)
+    return 0
+end
+
+# Mark as initialized (only after distrobox check passes)
+set -gx __ENV_INITIALIZED "1"
+set -gx CURRENT_ENV "ros2-assignments"
+# Source ROS2 setup files using bass
+if type -q bass
+    bass source /opt/ros/jazzy/setup.bash
+    if test -f ./install/setup.bash
+        bass source ./install/setup.bash
+    end
+else
+    echo (set_color red)"Error: bass is required for ROS2 environment. Install with: fisher install edc/bass"(set_color normal)
+    return 1
+end
+
+# Set environment variable for the prompt prefix
+set -gx ROS2_ACTIVE 1
+
+# Save the original prompt function if it exists
+# Only save if we don't already have a backup or if current prompt is not from an environment
+if not functions -q __env_orig_prompt
+    if functions -q fish_prompt
+        functions -c fish_prompt __env_orig_prompt
+    else
+        function __env_orig_prompt
+            echo -n (whoami)'@'(prompt_hostname)' '(set_color $fish_color_cwd)(prompt_pwd)(set_color normal)'> '
+        end
+    end
+else
+    # If we already have a backup, we're switching environments
+    # No need to create a new backup
+end
+
+# Define new prompt with ROS2 prefix
+function fish_prompt
+    echo -n (set_color magenta)'(ros2-assignments)'(set_color normal)' '
+    __env_orig_prompt
+end
+
+# ROS2 aliases and functions
+alias cb="colcon build"
+alias cbs="colcon build --symlink-install"
+alias cbt="colcon build --packages-select"
+alias ct="colcon test"
+alias ctr="colcon test-result"
+
+echo (set_color green)"Activated ROS2 environment: ros2-assignments"(set_color normal)
+
+# Custom deactivation function
+function __env_custom_deactivate
+    # Remove ROS2-specific variables and aliases
+    set -e ROS2_ACTIVE
+    functions -e cb cbs cbt ct ctr 2>/dev/null
+
+    echo (set_color blue)"ROS2 environment cleanup completed"(set_color normal)
+end

.gitignore

@@ -1,9 +1,39 @@
-!.gitkeep
 
-tmp/
-src/tmp
+# Created by https://www.toptal.com/developers/gitignore/api/ros2
+# Edit at https://www.toptal.com/developers/gitignore?templates=ros2
 
-node_modules/
+### ROS2 ###
+install/
+log/
+build/
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+*~
+
+# Emacs
+.#*
+
+# Colcon custom files
+COLCON_IGNORE
+AMENT_IGNORE
+
+.vscode
+
+# End of https://www.toptal.com/developers/gitignore/api/ros2
 
-obj/
-out/

IMU/.clangd

@@ -0,0 +1,2 @@
+CompileFlags:
+    Remove: [-f*, -m*]

IMU/.devcontainer/Dockerfile

@@ -0,0 +1,13 @@
+ARG DOCKER_TAG=latest
+FROM espressif/idf:${DOCKER_TAG}
+
+ENV LC_ALL=C.UTF-8
+ENV LANG=C.UTF-8
+
+RUN apt-get update -y && apt-get install udev -y
+
+RUN echo "source /opt/esp/idf/export.sh > /dev/null 2>&1" >> ~/.bashrc
+
+ENTRYPOINT [ "/opt/esp/entrypoint.sh" ]
+
+CMD ["/bin/bash", "-c"]

IMU/.devcontainer/devcontainer.json

@@ -0,0 +1,21 @@
+{
+	"name": "ESP-IDF QEMU",
+	"build": {
+		"dockerfile": "Dockerfile"
+	},
+	"customizations": {
+		"vscode": {
+			"settings": {
+				"terminal.integrated.defaultProfile.linux": "bash",
+				"idf.espIdfPath": "/opt/esp/idf",
+				"idf.toolsPath": "/opt/esp",
+				"idf.gitPath": "/usr/bin/git"
+			},
+			"extensions": [
+				"espressif.esp-idf-extension",
+				"espressif.esp-idf-web"
+			]
+		}
+	},
+	"runArgs": ["--privileged"]
+}

IMU/CMakeLists.txt

@@ -0,0 +1,6 @@
+# The following lines of boilerplate have to be in your project's CMakeLists
+# in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.16)
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(IMU)

IMU/main/CMakeLists.txt

@@ -0,0 +1,2 @@
+idf_component_register(SRCS "mpu6886.c"
+                       INCLUDE_DIRS ".")

IMU/main/Kconfig.projbuild

@@ -0,0 +1,77 @@
+menu "ESP32 IMU Project Configuration"
+
+    orsource "$IDF_PATH/examples/common_components/env_caps/$IDF_TARGET/Kconfig.env_caps"
+
+    menu "I2C Master Configuration"
+    config I2C_MASTER_SCL
+        int "SCL GPIO Num"
+        range ENV_GPIO_RANGE_MIN ENV_GPIO_OUT_RANGE_MAX
+        default 22
+        help
+            GPIO number for I2C Master clock line.
+
+    config I2C_MASTER_SDA
+        int "SDA GPIO Num"
+        range ENV_GPIO_RANGE_MIN ENV_GPIO_OUT_RANGE_MAX
+        default 21
+        help
+            GPIO number for I2C Master data line.
+
+    config I2C_MASTER_FREQUENCY
+        int "Master Frequency"
+        default 100000
+        help
+            I2C Speed of Master device.
+    endmenu
+
+    menu "MQTT Configuration"
+    config MQTT_BROKER_URI
+        string "MQTT Broker URI"
+        default "mqtt://192.168.4.2:1883"
+        help
+            URI of the MQTT broker to connect to.
+
+    config MQTT_TOPIC
+        string "MQTT Topic"
+        default "esp32/imu"
+        help
+            MQTT topic to publish IMU data to.
+    endmenu
+
+    menu "WiFi Access Point Configuration"
+    config WIFI_AP_MODE
+        bool "Enable WiFi AP Mode"
+        default y
+        help
+            Enable this option to start the device in Access Point mode.
+            If disabled, the device will start in Station mode.
+
+    config WIFI_SSID
+        string "WiFi SSID"
+        default "YourNetworkName"
+        depends on !WIFI_AP_MODE
+        help
+            SSID of WiFi network to connect to.
+
+    config WIFI_PASSWORD
+        string "WiFi Password"
+        default "YourPassword"
+        depends on !WIFI_AP_MODE
+        help
+            Password of WiFi network to connect to.
+
+    config WIFI_AP_SSID
+        string "WiFi AP SSID"
+        default "ESP32_IMU_AP"
+        depends on WIFI_AP_MODE
+        help
+            SSID of the WiFi Access Point when in AP mode.
+
+    config WIFI_AP_PASSWORD
+        string "WiFi AP Password"
+        default "esp32imuap"
+        depends on WIFI_AP_MODE
+        help
+            Password of the WiFi Access Point when in AP mode.
+    endmenu
+endmenu

IMU/main/mpu6886.c

@@ -0,0 +1,346 @@
+#include "mpu6886.h"
+
+
+static void IRAM_ATTR button_isr_handler(void* arg) {
+    uint32_t gpio_num = (uint32_t)arg;
+    // Keep it short — avoid delays, logging, malloc, etc.
+    mqtt_toggle = !mqtt_toggle;
+    toggle_completed = false;
+    esp_rom_printf("Button pressed! GPIO: %lu\n", gpio_num);
+}
+
+static void init_button(void) {
+    gpio_config_t io_conf = {
+        .intr_type = GPIO_INTR_NEGEDGE,   // Trigger on falling edge
+        .mode = GPIO_MODE_INPUT,
+        .pin_bit_mask = (1ULL << BOOT_BUTTON_GPIO),
+        .pull_up_en = GPIO_PULLUP_ENABLE, // Use pull-up since button pulls low
+        .pull_down_en = GPIO_PULLDOWN_DISABLE
+    };
+    gpio_config(&io_conf);
+
+    // Install GPIO ISR service
+    gpio_install_isr_service(0); // Pass 0 for default interrupt flags
+    // Attach the interrupt handler
+    gpio_isr_handler_add(BOOT_BUTTON_GPIO, button_isr_handler, (void*)BOOT_BUTTON_GPIO);
+}
+
+static void wifi_init()
+{
+    ESP_ERROR_CHECK(nvs_flash_init());
+    ESP_ERROR_CHECK(esp_netif_init());
+    ESP_ERROR_CHECK(esp_event_loop_create_default());
+
+    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
+
+#ifdef CONFIG_WIFI_AP_MODE
+    wifi_netif = esp_netif_create_default_wifi_ap();
+
+    wifi_config_t wifi_config = { 0 };
+    strncpy((char*)wifi_config.ap.ssid, CONFIG_WIFI_AP_SSID, sizeof(wifi_config.ap.ssid));
+    wifi_config.ap.ssid_len = strlen(CONFIG_WIFI_AP_SSID);
+    strncpy((char*)wifi_config.ap.password, CONFIG_WIFI_AP_PASSWORD, sizeof(wifi_config.ap.password));
+    wifi_config.ap.max_connection = 4;
+    if (strlen(CONFIG_WIFI_AP_PASSWORD) == 0) {
+        wifi_config.ap.authmode = WIFI_AUTH_OPEN;
+    } else {
+        wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK;
+    }
+
+    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
+    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config));
+    ESP_ERROR_CHECK(esp_wifi_start());
+    ESP_LOGI("WIFI", "AP started SSID:%s", CONFIG_WIFI_AP_SSID);
+#else
+    wifi_netif = esp_netif_create_default_wifi_sta();
+
+    wifi_config_t wifi_config = {
+        .sta = {
+            .ssid = CONFIG_WIFI_SSID,
+            .password = CONFIG_WIFI_PASSWORD,
+        },
+    };
+
+    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
+    ESP_ERROR_CHECK(esp_wifi_start());
+    ESP_ERROR_CHECK(esp_wifi_connect());
+    ESP_LOGI("WIFI", "STA started, connecting to: %s", CONFIG_WIFI_SSID);
+#endif
+    wifi_initialized = true;
+}
+
+static void wifi_deinit(void)
+{
+    ESP_ERROR_CHECK(esp_wifi_stop());
+    ESP_ERROR_CHECK(esp_wifi_deinit());
+
+    if (wifi_netif != NULL) {
+    esp_netif_destroy_default_wifi(wifi_netif);
+    wifi_netif = NULL;
+    }
+
+    ESP_ERROR_CHECK(esp_event_loop_delete_default());
+    // ESP_ERROR_CHECK(esp_netif_deinit());
+    ESP_ERROR_CHECK(nvs_flash_deinit());
+    ESP_LOGI("WIFI", "Wi-Fi stopped and deinitialized");
+    wifi_initialized = false;
+}
+
+static void mqtt_app_start(void)
+{
+    esp_mqtt_client_config_t mqtt_cfg = { 0 };
+
+    mqtt_cfg.broker.address.uri = CONFIG_MQTT_BROKER_URI;
+
+    s_mqtt_client = esp_mqtt_client_init(&mqtt_cfg);
+    if (s_mqtt_client == NULL) {
+        ESP_LOGW("MQTT", "failed to init mqtt client");
+        return;
+    }
+    esp_mqtt_client_start(s_mqtt_client);
+}
+
+static void mqtt_app_stop(void)
+{
+    if (s_mqtt_client != NULL) {
+        esp_mqtt_client_stop(s_mqtt_client);
+        esp_mqtt_client_destroy(s_mqtt_client);
+        s_mqtt_client = NULL;
+    }
+}
+
+static esp_err_t mpu6886_write_byte(mpu6886_t* dev, uint8_t reg, uint8_t data) {
+    uint8_t tx[2] = { reg, data };
+    return i2c_master_write_to_device(dev->i2c_port, dev->address, tx, sizeof(tx), pdMS_TO_TICKS(100));
+}
+
+static esp_err_t mpu6886_read_bytes(mpu6886_t* dev, uint8_t reg, uint8_t* data, size_t len) {
+    return i2c_master_write_read_device(dev->i2c_port, dev->address, &reg, 1, data, len, pdMS_TO_TICKS(100));
+}
+
+static int16_t bytes_to_int16(uint8_t high, uint8_t low) {
+    return (int16_t)((high << 8) | low);
+}
+
+static esp_err_t mpu6886_update_sensitivity(mpu6886_t* dev)
+{
+    uint8_t aconf = 0, gconf = 0;
+    esp_err_t err;
+
+    err = mpu6886_read_bytes(dev, MPU6886_ACCEL_CONFIG, &aconf, 1);
+    if (err != ESP_OK) return err;
+    err = mpu6886_read_bytes(dev, MPU6886_GYRO_CONFIG, &gconf, 1);
+    if (err != ESP_OK) return err;
+
+    uint8_t a_fs = (aconf >> 3) & 0x03; // AFS_SEL bits [4:3]
+    switch (a_fs) {
+        case 0: dev->accel_div = ACCEL_SO_2G; break;   // ±2g
+        case 1: dev->accel_div = ACCEL_SO_4G; break;   // ±4g
+        case 2: dev->accel_div = ACCEL_SO_8G; break;   // ±8g
+        case 3: dev->accel_div = ACCEL_SO_16G; break;  // ±16g
+        default: dev->accel_div = ACCEL_SO_2G; break;
+    }
+
+    uint8_t g_fs = (gconf >> 3) & 0x03; // FS_SEL bits [4:3]
+    switch (g_fs) {
+        case 0: dev->gyro_div = GYRO_SO_250DPS; break;   // ±250 dps
+        case 1: dev->gyro_div = GYRO_SO_500DPS; break;   // ±500 dps
+        case 2: dev->gyro_div = GYRO_SO_1000DPS; break;  // ±1000 dps
+        case 3: dev->gyro_div = GYRO_SO_2000DPS; break;  // ±2000 dps
+        default: dev->gyro_div = GYRO_SO_250DPS; break;
+    }
+
+    return ESP_OK;
+}
+
+static esp_err_t i2c_master_init(i2c_port_t i2c_num, gpio_num_t sda_io, gpio_num_t scl_io, uint32_t clk_speed_hz)
+{
+    i2c_config_t conf = {
+        .mode = I2C_MODE_MASTER,
+        .sda_io_num = sda_io,
+        .sda_pullup_en = GPIO_PULLUP_ENABLE,
+        .scl_io_num = scl_io,
+        .scl_pullup_en = GPIO_PULLUP_ENABLE,
+        .master.clk_speed = clk_speed_hz,
+    };
+    esp_err_t err = i2c_param_config(i2c_num, &conf);
+    if (err != ESP_OK) return err;
+    return i2c_driver_install(i2c_num, I2C_MODE_MASTER, 0, 0, 0);
+}
+
+esp_err_t mpu6886_init(mpu6886_t* dev, i2c_port_t i2c_port) {
+    dev->i2c_port = i2c_port;
+    dev->address = MPU6886_ADDR;
+
+    uint8_t who_am_i = 0;
+    if (mpu6886_read_bytes(dev, MPU6886_WHO_AM_I, &who_am_i, 1) != ESP_OK) {
+        return ESP_FAIL;
+    }
+    if (who_am_i != 0x19) {
+        return ESP_FAIL;
+    }
+
+    // Reset
+    if (mpu6886_write_byte(dev, MPU6886_PWR_MGMT_1, 0x80) != ESP_OK) return ESP_FAIL;
+    vTaskDelay(pdMS_TO_TICKS(100));
+
+    // Auto select clock
+    if (mpu6886_write_byte(dev, MPU6886_PWR_MGMT_1, 0x01) != ESP_OK) return ESP_FAIL;
+
+    // Default config: set to ±2G accel, ±250DPS gyro
+    mpu6886_write_byte(dev, MPU6886_ACCEL_CONFIG, 0x00);
+    mpu6886_write_byte(dev, MPU6886_GYRO_CONFIG, 0x00);
+
+    dev->gyro_offset = (vec3_t) { 0, 0, 0 };
+    dev->accel_offset = (vec3_t) { 0, 0, 0 };
+
+    // detect actual sensitivities from device registers and set divisors
+    if (mpu6886_update_sensitivity(dev) != ESP_OK) {
+        // fallback to defaults if read fails
+        dev->accel_div = ACCEL_SO_2G;
+        dev->gyro_div = GYRO_SO_250DPS;
+    }
+
+    return ESP_OK;
+}
+
+esp_err_t mpu6886_read_accel(mpu6886_t* dev, vec3_t* accel) {
+    uint8_t buf[6];
+    esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_ACCEL_XOUT_H, buf, 6);
+    if (ret != ESP_OK) return ret;
+
+    // convert raw -> m/s^2 and apply stored offsets
+    accel->x = (float)bytes_to_int16(buf[0], buf[1]) / dev->accel_div * SF_M_S2 - dev->accel_offset.x;
+    accel->y = (float)bytes_to_int16(buf[2], buf[3]) / dev->accel_div * SF_M_S2 - dev->accel_offset.y;
+    accel->z = (float)bytes_to_int16(buf[4], buf[5]) / dev->accel_div * SF_M_S2 - dev->accel_offset.z;
+    return ESP_OK;
+}
+
+esp_err_t mpu6886_read_gyro(mpu6886_t* dev, vec3_t* gyro) {
+    uint8_t buf[6];
+    esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_GYRO_XOUT_H, buf, 6);
+    if (ret != ESP_OK) return ret;
+
+    gyro->x = (float)bytes_to_int16(buf[0], buf[1]) / dev->gyro_div * SF_RAD_S - dev->gyro_offset.x;
+    gyro->y = (float)bytes_to_int16(buf[2], buf[3]) / dev->gyro_div * SF_RAD_S - dev->gyro_offset.y;
+    gyro->z = (float)bytes_to_int16(buf[4], buf[5]) / dev->gyro_div * SF_RAD_S - dev->gyro_offset.z;
+    return ESP_OK;
+}
+
+esp_err_t mpu6886_read_temp(mpu6886_t* dev, float* temp) {
+    static uint8_t buf[2];
+    esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_TEMP_OUT_H, buf, 2);
+    if (ret != ESP_OK) return ret;
+
+    int16_t raw = bytes_to_int16(buf[0], buf[1]);
+    *temp = ((float)raw / TEMP_SO) + TEMP_OFFSET;
+    return ESP_OK;
+}
+
+esp_err_t mpu6886_calibrate_gyro(mpu6886_t* dev, int samples, int delay_ms) {
+    vec3_t sum = { 0, 0, 0 }, g;
+    for (int i = 0; i < samples; i++) {
+        if (mpu6886_read_gyro(dev, &g) != ESP_OK) return ESP_FAIL;
+        sum.x += g.x;
+        sum.y += g.y;
+        sum.z += g.z;
+        vTaskDelay(pdMS_TO_TICKS(delay_ms));
+    }
+    dev->gyro_offset.x = sum.x / samples;
+    dev->gyro_offset.y = sum.y / samples;
+    dev->gyro_offset.z = sum.z / samples;
+    return ESP_OK;
+}
+
+esp_err_t mpu6886_calibrate_accel(mpu6886_t* dev, int samples, int delay_ms)
+{
+    // Calibrate accelerometer offsets while the device is stationary.
+    // The function will detect the device full-scale and compute offsets in m/s^2.
+    vec3_t sum = { 0, 0, 0 }, a;
+    for (int i = 0; i < samples; i++) {
+        if (mpu6886_read_accel(dev, &a) != ESP_OK) {
+            return ESP_FAIL;
+        }
+        sum.x += a.x;
+        sum.y += a.y;
+        sum.z += a.z;
+        vTaskDelay(pdMS_TO_TICKS(delay_ms));
+    }
+    vec3_t avg = { sum.x / samples, sum.y / samples, sum.z / samples };
+
+    // For X/Y expect ~0 m/s^2 when stationary; offset = measured average
+    dev->accel_offset.x = avg.x;
+    dev->accel_offset.y = avg.y;
+
+    // For Z expect ~+1g (SF_M_S2) if +Z points up. If device is flipped you'll get -1g.
+    // Compute expected gravity sign from measured avg.z magnitude and sign.
+    float expected_g = (avg.z < 0) ? -SF_M_S2 : SF_M_S2;
+    dev->accel_offset.z = avg.z - expected_g;
+
+    return ESP_OK;
+}
+
+void app_main(void)
+{
+    esp_err_t err = i2c_master_init(I2C_NUM_0, CONFIG_I2C_MASTER_SDA, CONFIG_I2C_MASTER_SCL, CONFIG_I2C_MASTER_FREQUENCY); // adjust pins if needed
+    if (err != ESP_OK) {
+        ESP_LOGE("MPU6886", "I2C init failed: %d", err);
+        return;
+    }
+
+    init_button();
+    mpu6886_t mpu;
+    mpu.i2c_port = I2C_NUM_0;
+    mpu.address = MPU6886_ADDR;
+
+    esp_err_t ret = mpu6886_init(&mpu, I2C_NUM_0);
+    if (ret != ESP_OK) {
+        ESP_LOGE("MPU6886", "init failed");
+        return;
+    }
+
+    mpu6886_calibrate_gyro(&mpu, 100, 10);
+    mpu6886_calibrate_accel(&mpu, 100, 10);
+    vec3_t accel, gyro;
+    float temp;
+
+    while (1) {
+        if (mqtt_toggle && !toggle_completed && !wifi_initialized) {
+            wifi_init();
+            mqtt_app_start();
+            ESP_LOGI("BOOT", "Boot button pressed: starting MQTT mode");
+            toggle_completed = true;
+        } else if (!mqtt_toggle && !toggle_completed && wifi_initialized) {
+            mqtt_app_stop();
+            wifi_deinit();
+            ESP_LOGI("BOOT", "Boot button not pressed: starting serial-only mode");
+            toggle_completed = true;
+        }
+        mpu6886_read_accel(&mpu, &accel);
+        mpu6886_read_gyro(&mpu, &gyro);
+        mpu6886_read_temp(&mpu, &temp);
+
+        if (mqtt_toggle && s_mqtt_client != NULL) {
+            char payload[256];
+            int n = snprintf(payload, sizeof(payload),
+                "{\"accel\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"gyro\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"Temp\":%8.2f}",
+                accel.x, accel.y, accel.z,
+                gyro.x, gyro.y, gyro.z,
+                temp);
+            if (n > 0 && n < (int)sizeof(payload)) {
+                int msg_id = esp_mqtt_client_publish(s_mqtt_client, CONFIG_MQTT_TOPIC, payload, 0, 1, 0);
+                ESP_LOGD("MQTT", "published msg_id=%d payload_len=%d", msg_id, n);
+            } else {
+                ESP_LOGW("MQTT", "payload truncated or encoding error");
+            }
+        } else {
+            printf("{\"accel\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"gyro\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"Temp\":%8.2f}\n",
+               accel.x, accel.y, accel.z,
+               gyro.x, gyro.y, gyro.z,
+               temp);
+        }
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+}

IMU/main/mpu6886.h

@@ -0,0 +1,90 @@
+#ifndef MPU6886_H
+#define MPU6886_H
+
+#include "driver/i2c.h"
+#include "esp_err.h"
+#include "esp_wifi.h"
+#include "esp_event.h"
+#include "esp_log.h"
+#include "nvs_flash.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include <math.h>
+#include "mqtt_client.h"
+#include <string.h>
+#include <stdio.h>
+#include "driver/gpio.h"
+#include "esp_rom_sys.h"
+
+
+#define I2C_MASTER_SCL_IO           CONFIG_I2C_MASTER_SCL       /*!< GPIO number used for I2C master clock */
+#define I2C_MASTER_SDA_IO           CONFIG_I2C_MASTER_SDA       /*!< GPIO number used for I2C master data  */
+#define I2C_MASTER_NUM              I2C_NUM_0                   /*!< I2C port number for master dev */
+#define I2C_MASTER_FREQ_HZ          CONFIG_I2C_MASTER_FREQUENCY /*!< I2C master clock frequency */
+#define I2C_MASTER_TX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
+#define I2C_MASTER_RX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
+#define I2C_MASTER_TIMEOUT_MS       1000
+
+#define BOOT_BUTTON_GPIO 0 // GPIO number for boot mode selection button
+
+#define MPU6886_ADDR 0x68
+
+// MPU6886 Registers
+#define MPU6886_PWR_MGMT_1     0x6B
+#define MPU6886_WHO_AM_I       0x75
+#define MPU6886_ACCEL_XOUT_H   0x3B
+#define MPU6886_GYRO_XOUT_H    0x43
+#define MPU6886_TEMP_OUT_H     0x41
+#define MPU6886_ACCEL_CONFIG   0x1C
+#define MPU6886_GYRO_CONFIG    0x1B
+
+// Sensitivity scales
+#define ACCEL_SO_2G  16384.0f
+#define ACCEL_SO_4G   8192.0f
+#define ACCEL_SO_8G   4096.0f
+#define ACCEL_SO_16G  2048.0f
+
+#define GYRO_SO_250DPS  131.0f
+#define GYRO_SO_500DPS   65.5f
+#define GYRO_SO_1000DPS  32.8f
+#define GYRO_SO_2000DPS  16.4f
+
+#define TEMP_SO 326.8f
+#define TEMP_OFFSET 25.0f
+
+// Scale factors
+#define SF_M_S2 9.80665f
+#define SF_RAD_S 0.017453292519943f // pi/180
+
+typedef struct {
+    float x;
+    float y;
+    float z;
+} vec3_t;
+
+typedef struct {
+    i2c_port_t i2c_port;
+    uint8_t address;
+    float accel_div;
+    float gyro_div;
+    vec3_t gyro_offset;
+    vec3_t accel_offset;
+} mpu6886_t;
+
+bool mqtt_toggle = false;
+bool toggle_completed = false;
+bool wifi_initialized = false;
+
+esp_netif_t* wifi_netif = NULL;
+
+static esp_mqtt_client_handle_t s_mqtt_client = NULL;
+
+// Function declarations
+esp_err_t mpu6886_init(mpu6886_t *dev, i2c_port_t i2c_port);
+esp_err_t mpu6886_read_accel(mpu6886_t *dev, vec3_t *accel);
+esp_err_t mpu6886_read_gyro(mpu6886_t *dev, vec3_t *gyro);
+esp_err_t mpu6886_read_temp(mpu6886_t *dev, float *temp);
+esp_err_t mpu6886_calibrate_gyro(mpu6886_t *dev, int samples, int delay_ms);
+esp_err_t mpu6886_calibrate_accel(mpu6886_t *dev, int samples, int delay_ms);
+
+#endif

README.md

@@ -10,3 +10,21 @@ Assignments made for the first semester of ROS2
 <br><br>
 
 ## Table of contents
+
+- [System Architecture](#system-architecture)
+- [Components](#components)
+- [Installation](#installation)
+
+---
+
+## System Architecture
+
+For the complete system architecture see [architecture.md](doc/architecture/architecture.md) located in the `doc/architecture` folder
+
+### Testing
+
+The testing documentation can be found in the [doc/tests](doc/tests/) folder for each node
+
+## Installation
+
+For installation instructions see [Installation.md](doc/installation/installation.md) located in the `doc/installation` folder

doc/architecture/IMU/ESP32-IMU.md

@@ -0,0 +1,77 @@
+# IMU (MPU6886) ESP-IDF
+=====================
+
+Small ESP-IDF project that reads an MPU6886 IMU over I2C and publishes readings over MQTT.
+Includes an option to run WiFi in STA (client) or AP (access point) mode.
+
+### Features
+
+- I2C communication functions for MPU6886 (accelerometer, gyro, temp)
+- MQTT client using `esp-mqtt`
+- WiFi: STA or AP mode selectable at build time
+- AP mode defaults to static IP 192.168.10.1/24 (configurable in code)
+- MQTT, Serial toggle using the boot button on the esp32
+
+## Quick start
+
+### Prerequisites
+
+- ESP-IDF installed and activated (the repo was developed with ESP-IDF v5.x).
+- Toolchain and Python deps per ESP-IDF instructions.
+
+### Build and flash
+
+From the IMU root:
+
+```bash
+# configure project options
+idf.py menuconfig
+
+# build
+idf.py build
+
+# flash and monitor (set your serial port)
+idf.py -p /dev/ttyUSB0 flash monitor
+```
+
+## Project Configuration (menuconfig)
+
+Open `idf.py menuconfig` and there you will see the `ESP32 IMU Project Configuration` submenu where you can configure specific data for I2C, MQTT and WIFI
+
+### I2C
+---
+In the `I2C Master configuration` submenu there are three options:
+- SCL GPIO Num  - Default 21 - GPIO used for I2C SDA
+- SDA GPIO Num  - Default 22 - GPIO used for I2C SCL
+- Master Frequency - Default 100000hz - I2C bus speed
+
+### MQTT
+
+- Broker URI - Default: `"mqtt://192.168.4.2:1883"`
+- MQTT Topic - Default: `"esp32/imu"`
+
+### WiFi / network
+
+- Toggle AP mode at build time by enabling `CONFIG_WIFI_AP_MODE` in menuconfig. If enabled the firmware will start as an access point on boot; if disabled the device starts as a station and attempts to connect to the configured SSID using the set Password
+- Toggle AP mode swaps the available settings when toggled between AP conf- and station configurations respectively
+- AP credentials can be set with `WiFi AP SSID` and `Wifi AP Password`.
+- Station credentials can be set with `WiFi SSID` and `WiFi Password`.
+
+### AP IP / DHCP
+
+- The AP is configured by default to use a static IP of `192.168.4.1` with netmask `255.255.255.0` and a DHCP server is started so clients receive addresses on that subnet.
+
+## Notes about units and macros
+
+- The macro `SF_RAD_S` (in `main/mpu6886.h`) is the degrees-to-radians conversion factor: PI/180. Gyroscope readings are converted from degrees/sec to radians/sec using this scale.
+- Sensitivity scale constants (e.g. `ACCEL_SO_2G`, `GYRO_SO_250DPS`) are used to convert raw sensor counts to physical units.
+
+## Troubleshooting
+
+- "`MQTT_TOPIC` undefined" at build time: run menuconfig
+- "I2C init failed": confirm SDA/SCL pins in menuconfig or wiring.
+
+## Files of interest
+
+- `main/mpu6886.c` - main application, WiFi init, MQTT publish loop
+- `main/mpu6886.h` - sensor constants and types

doc/architecture/architecture.md

@@ -0,0 +1,104 @@
+# ROS2 IMU Reader — Design Document
+
+## Project Overview
+
+This projects reads data from a ESP32 communicating with serial or MQTT and writes this into a database for further processing
+
+## System Architecture
+
+### High-Level Architecture
+
+The system consists of multiple ROS2 nodes that communicate through standardized topics and services to process imu data and store it in a database
+
+### Key Design Principles
+
+- **Microservices Architecture**: Each component has a single responsibility
+- **Asynchronous Communication**: Uses ROS2 topics and services for loose coupling
+- **Data Persistence**: Centralized database management for datastorage
+- **Comprehensive Testing**: Unit tests ensure code reliability
+
+## System Components
+
+### ESP32-IMU
+*For ESP32 specific documentation see [ESP32-IMU.md](IMU/ESP32-IMU.md)*
+
+### Core Nodes
+
+#### 1. IMU Databse writer Node
+**Namespace**: `assignments::two::imu_database_writer`
+
+**Brief Description**: Recieves and stores IMU data.
+
+**Key Features**: Database persistence
+
+*For detailed documentation, see: [IMUDatabaseWriter.md](nodes/IMUDatabaseWriter.md)*
+
+#### 2. LifeCycle Node - NEEDS TO BE EDITED STILL
+**Namespace**: `assignments::one::grade_calculator`
+
+**Brief Description**: Provides grade calculation service with business logic including bonus points and grade validation.
+
+**Key Features**: Average calculation, special student rules, grade bounds validation (10-100)
+
+*For detailed documentation, see: [GradeCalculator.md](nodes/GradeCalculator.md)*
+
+### Data Management
+
+#### DatabaseManager
+**Brief Description**: PostgreSQL database interface handling connections, table management, and data persistence.
+
+**Key Features**: Connection management, automatic table creation, student enrollment tracking, exam result storage
+
+*For detailed documentation, see: [DatabaseManager.md](../DatabaseManager.md)*
+
+#### ConfigManager
+**Brief Description**: TOML-based configuration management system allowing runtime configuration without recompilation.
+
+**Key Features**: Automatic config file discovery, type-safe TOML parsing, database connection configuration
+
+*For detailed documentation, see: [ConfigManager.md](../ConfigManager.md)*
+
+### Communication Interfaces
+
+#### ROS2 Message Interface
+**Brief Description**: This project uses the ROS2 standard IMU sensor message
+
+## System Workflow
+
+### 1. Exam Result Processing
+
+1. **Input**: IMU data is sent from the ESP32 to the lifecycle node
+2. **Collection**: The lifcycle node recieves the data via a serial or MQTT connection
+4. **Data passthrough**: When data is recieved it is sent to the database writer
+
+### 2. Data Management
+
+1. **Database Storage**: IMU data is persisted/stored in the database
+
+## Configuration Management
+
+### TOML Configuration Structure
+
+The system uses TOML files for environment-specific configuration:
+
+```toml
+[database]
+host = "localhost"
+port = 5432
+name = "grade_db"
+user = "grade_user"
+password = "secure_password"
+
+[grade_calculation]
+collection_amount = 5
+min_grade = 10
+max_grade = 100
+
+[logging]
+level = "INFO"
+output_file = "grade_system.log"
+```
+
+## Testing
+
+The testing documentation can be found in the [doc/tests](/doc/tests/) folder for each node

doc/architecture/managers/ConfigManager.md

@@ -0,0 +1,133 @@
+# ConfigManager (`assignments::one::ConfigManager`)
+
+## Overview
+
+The `ConfigManager` class is used to be able to store configuration values in a `TOML` file making it
+possible to change project settings without the need of recompiling the codebase.
+
+The [`toml++`](https://marzer.github.io/tomlplusplus/) library is used to parse TOML configuration
+files and provides type-safe access to configuration parameters. It is automatically installed
+using the `FetchContent_Declare` macro inside of the `CMakeLists.txt`.
+
+## Implementation Details
+
+### Dependencies
+- **toml++**: Modern header-only C++17 TOML parser and serializer
+- **rclcpp**: ROS2 C++ client library for logging
+- **filesystem**: C++17 filesystem library for file operations
+- **DatabaseConfig**: Custom data structure for database configuration
+
+### Key Components
+
+#### Constructor
+```cpp
+ConfigManager(rclcpp::Logger logger)
+```
+- Initializes the configuration manager making use of a ROS2 logger
+- Calls `find_config_file()` and `load_config()` for automatic setup
+- Automatically attempts to locate and load configuration from a list of pre-defined paths.
+
+#### Configuration Loading
+
+**`bool load_config(const std::string& config_file_path)`**
+> Returns `true` on successful load, `false` on failure
+
+- Uses `toml::parse_file()`
+- Loads TOML configuration from specified file path
+- Validates file existence before attempting to parse
+- Error handling for:
+  - File not found errors
+  - TOML parsing errors
+  - General I/O exceptions
+- Updates internal `loaded_` state flag
+
+**`std::string find_config_file() const`**
+> Returns first found configuration file path
+> Returns empty string if no configuration file is found
+
+- Automatically searches for configuration files in predefined locations
+- Search paths (in order):
+```cpp
+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"
+};
+```
+#### Configuration Access
+
+**`std::optional<DatabaseConfig> get_database_config() const`**
+> Returns `std::optional<DatabaseConfig>` for safe null handling
+> Returns `std::nullopt` if:
+>   - Configuration is not loaded
+>   - Database section is missing
+>   - Parsing fails due to invalid format
+
+- Calls `parse_database_config()` for actual parsing
+
+**`bool is_loaded() const`**
+> Returns `true` if configuration has been successfully loaded and parsed
+
+- Getter for configuration load status
+- Used by other components to verify configuration availability
+
+### Configuration Parsing
+
+**`DatabaseConfig parse_database_config(const toml::table& config) const`**
+- parser for database configuration section
+- Extracts all database-related parameters with defaults
+
+#### Default Values and Fallbacks
+- **host**: `"localhost"` - Local database server
+- **port**: `5432` - Standard PostgreSQL port
+- **dbname**: `"grades"` - Application-specific database
+- **user**: `"postgres"` - Default PostgreSQL user
+- **password**: `"postgres"` - Default PostgreSQL password
+- **timeout**: `30` seconds - connection timeout
+- **ssl**: `false` - Disabled by default for development
+- **min_connections**: `1` - Minimal connection pool
+- **max_connections**: `10` - connection pool limit
+
+### Logging
+- Uses ROS2 logger with `[CFG]` prefix for configuration operations
+- Includes file paths in log messages
+
+### Usage Examples
+
+#### Automatic Configuration Loading
+```cpp
+// ConfigManager automatically finds and loads configuration
+ConfigManager config_manager(node->get_logger());
+
+if (config_manager.is_loaded()) {
+    auto db_config = config_manager.get_database_config();
+    if (db_config.has_value()) {
+        // Use database configuration
+        DatabaseManager db_manager(db_config.value());
+    }
+}
+```
+
+### Configuration File Format
+
+Example complete TOML configuration:
+
+```toml
+# Database connection settings
+[database]
+host = "localhost"
+port = 5432
+dbname = "grades"
+user = "postgres"
+password = "postgres"
+timeout = 30
+ssl = false
+
+[database.pool]
+min_connections = 1
+max_connections = 10
+```

doc/architecture/managers/DatabaseManager.md

@@ -0,0 +1,125 @@
+# DatabaseManager (`assignments::one::DatabaseManager`)
+
+## Overview
+
+The `DatabaseManager` class is a PostgreSQL database interface for the ROS2 grade calculator.
+It handles all database operations including connection management, table creation and data insertion.
+
+## Implementation Details
+
+### Dependencies
+- **pqxx**: Modern C++ PostgreSQL library for database connectivity
+- **rclcpp**: ROS2 C++ client library for logging
+- **ConfigManager**: Configuration manager for database settings
+
+### Key Components
+
+#### Constructor
+```cpp
+DatabaseManager(rclcpp::Logger logger)
+```
+- Initializes the database manager with ROS2 logging
+- Creates a ConfigManager instance for configuration handling
+- Automatically calls `init_database()` to establish connection and setup
+
+#### Connection Management
+
+**`bool connect(const std::string& connection_string)`**
+> Returns `true` on successful connection, `false` on failure
+
+- Establishes connection to PostgreSQL database using connection information from the config TOML
+- Connection string format: `"host=localhost port=5432 dbname=grades user=postgres password=postgres"`
+
+**`bool is_connected() const`**
+>  Returns `true` if connection exists and is open
+
+- Check for active database connection status
+
+#### Database Initialization
+
+**`void init_database()`**
+- Database setup process
+- Loads configuration from ConfigManager
+- Establishes connection using configuration settings
+- Creates necessary tables and inserts sample data
+- Error handling for configuration and connection failures
+
+**`void create_tables()`**
+- Creates all required database tables using SQL queries from `SQLQueries.hpp`
+- Tables created:
+  - `enrollments`: Student course enrollments
+  - `exam_results`: Individual exam scores
+  - `course_results`: Final course grades and statistics
+- Uses transactions for atomic table creation
+
+**`void insert_sample_data()`**
+- Inserts predefined sample student data
+
+### Data Operations
+
+#### Student Course Management
+
+**`std::vector<StudentCourse> queue_pending_combinations()`**
+> Returns vector of StudentCourse objects for processing queue
+
+- Gets all student-course combinations that need exam results generated
+- Executes complex SQL query to find missing exam results
+
+**`bool enroll_student_into_course(const StudentCourse& sc)`**
+> Returns `true` on successful enrollment, `false` on failure
+
+- Enrolls a student into a specific course
+
+#### Exam Result Processing
+
+**`bool store_exam_result(const std::string& student_name, const std::string& course_name, int grade)`**
+- Stores individual exam results in the database
+- Parameters:
+  - `student_name`: Name of the student
+  - `course_name`: Name of the course
+  - `grade`: Exam score (10-100)
+
+**`bool store_final_course_result(const StudentCourse& sc, int exam_count, int final_grade)`**
+- Stores calculated final course results
+- Parameters:
+  - `sc`: StudentCourse object containing student and course names
+  - `exam_count`: Number of exams taken
+  - `final_grade`: Calculated final grade
+- Used by grade calculation nodes for final result storage
+
+#### Grade Retrieval
+
+**`int get_final_course_grade(const StudentCourse& sc)`**
+> Returns:
+>  - `> 0`: Valid final grade (rounded average)
+>  - `-1`: No exams taken or no results found
+
+- Gets final calculated grade for a student-course combination
+- Performs average calculation with proper rounding
+- Used by nodes to check if final grading is complete
+
+### Logging
+
+- Uses ROS2 logger with `[DBS]` prefix for database operations
+- Different log levels:
+  - `INFO`: Successful operations, connection status
+  - `ERROR`: SQL errors, connection failures, configuration issues
+  - `WARN`: Non-critical issues, missing configurations
+
+
+### Usage Examples
+
+#### Basic Database Setup
+```cpp
+// Create DatabaseManager with ROS2 logger
+DatabaseManager db_manager(node->get_logger());
+
+// Check connection status
+if (db_manager.is_connected()) {
+    // Database ready for operations
+    bool success = db_manager.store_exam_result("Wessel", "ROS2", 85);
+    if (success) {
+        RCLCPP_INFO(logger, "Exam result stored successfully");
+    }
+}
+```

doc/architecture/nodes/IMUDatabaseWriter.md

@@ -0,0 +1,29 @@
+# IMUDatabaseWriter (`assignments::two::imu_database_writer`)
+
+The `IMUDatabaseWriter` node subscribes to IMU sensor data (`sensor_msgs/msg/Imu`) and saves published data into the PostgreSQL database via the `DatabaseManager`. It is a lightweight sink node intended record IMU measurements (linear acceleration and angular velocity) together with timestamps.
+
+## Implementation Details
+
+**Parameters**
+
+- No node-specific parameters are required by default. Database connection and table configuration are handled by `DatabaseManager` and the project's `ConfigManager`.
+
+**Constructor**
+```cpp
+IMUDatabaseWriter()
+```
+- Initializes ROS2 node with name `imu_database_writer`
+- Creates `DatabaseManager` instance
+- Creates a subscription to the `imu_data` topic using `sensor_msgs::msg::Imu`
+
+## Core Functionality
+
+**`void imu_data_callback(const sensor_msgs::msg::Imu::SharedPtr msg)`**
+- Primary callback invoked whenever an IMU message is received
+- Forwards: linear acceleration (x, y, z) and angular velocity (x, y, z) to `DatabaseManager::store_imu_data`
+
+## ROS2 Interface
+
+**Subscriptions**
+- `imu_data` (sensor_msgs/msg/Imu)
+  - Receives raw IMU samples.

doc/installation/installation.md

@@ -0,0 +1,43 @@
+## Installation
+
+### Prerequisites
+
+- ROS2 Jazzy or newer installed ([ROS2 Installation Guide](https://docs.ros.org/en/jazzy/Installation.html))
+- CMake (version 3.8+)
+- Python 3.8+
+- libtomlplusplus-dev
+- libpqxx-dev
+- Colcon build tool
+- Docker compose
+
+### Clone the Repository
+
+```bash
+git clone https://git.wessel.gg/inholland/ros2-assignments.git
+cd ros2-assignments
+```
+
+### Build the Workspace
+
+```bash
+colcon build
+```
+Any parameters can be changed before building by editing the `imu_reader.launch.xml` in the launch folder
+
+### Source the Workspace
+
+```bash
+source install/setup.bash
+```
+
+### Start the database
+```bash
+sudo docker-compose up
+```
+You can configure specific database settings in the `docker-compose.yaml` in the root folder or the `config.toml` file in the `src/` folder
+
+### Start the Grade calculator program
+```bash
+ros2 launch g2_2025_imu_reader_pkg imu_reader.launch.xml
+```
+To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed

doc/tests/ConfigManager.md

@@ -0,0 +1,79 @@
+# Config Manager Unit Tests
+
+Unit tests for `ConfigManager` are implemented in `src/g2_2025_imu_reader_pkg/test/ConfigManager.test.cpp` using Google Test and ROS2 test utilities. The tests use temporary TOML files to validate configuration loading and parsing functionality.
+
+## Test Cases
+
+### 1. ConstructorTest
+
+**Description:** Verifies that ConfigManager can be created with a ROS2 logger without crashing.
+
+- **Test Action:** Create ConfigManager instance with ROS2 logger
+- **Expected Result:** Instance created successfully without exceptions
+
+### 2. LoadValidConfigTest
+
+**Description:** Tests loading of valid TOML configuration files with proper parsing.
+
+- **Test Action:**
+  - Create temporary TOML file with valid configuration
+  - Call `load_config()` with file path
+- **Expected Result:**
+  - Returns `true`
+  - `is_loaded()` returns `true`
+
+### 3. LoadInvalidFileTest
+
+**Description:** Tests error handling when attempting to load non-existent configuration files.
+
+- **Test Action:** Call `load_config()` with non-existent file path
+- **Expected Result:**
+  - Returns `false`
+  - `is_loaded()` returns `false`
+
+### 4. DatabaseConfigParsingTest
+
+**Description:** Tests complete database configuration parsing with all parameters.
+
+- **Test Configuration:**
+  ```toml
+  [database]
+  host = "test_host"
+  port = 1234
+  dbname = "test_db"
+  user = "test_user"
+  password = "test_password"
+  timeout = 60
+  ssl = true
+
+  [database.pool]
+  min_connections = 2
+  max_connections = 20
+  ```
+- **Expected Result:** All configuration values parsed correctly with proper types
+
+### 5. DatabaseConfigWithoutPoolTest
+
+**Description:** Tests default values when optional pool section is missing from configuration.
+
+- **Test Configuration:**
+  ```toml
+  [database]
+  host = "localhost"
+  port = 5432
+  dbname = "grades"
+  user = "postgres"
+  password = "postgres"
+  ```
+- **Expected Result:**
+  - Main database config parsed correctly
+  - Default pool values: `min_connections = 1`, `max_connections = 10`
+
+### 6. GetConfigWithoutLoadingTest
+
+**Description:** Tests behavior when attempting to access configuration before loading any file.
+
+- **Test Action:** Call `get_database_config()` without loading configuration
+- **Expected Result:**
+  - Returns `std::nullopt`
+  - `is_loaded()` returns `false`

doc/tests/DatabaseManager.md

@@ -0,0 +1,33 @@
+# Database Manager Unit Tests
+
+Unit tests for `DatabaseManager` are implemented in `src/g2_2025_imu_reader_pkg/test/DatabaseManager.test.cpp` using Google Test and ROS2 test utilities. The tests are designed to work reliably whether a database connection is available or not, focusing on error handling and method behavior validation.
+
+## Test Cases
+
+### 1. ConstructorTest
+
+**Description:** Verifies that DatabaseManager can be created without crashing and proper initialization occurs.
+
+- **Test Action:** Create DatabaseManager instance with ROS2 logger
+- **Expected Result:** Instance created successfully without exceptions
+
+### 2. ConnectionStatusTest
+
+**Description:** Tests that the `is_connected()` method returns a valid boolean value.
+
+- **Test Action:** Call `is_connected()` method
+- **Expected Result:** Returns either `true` or `false` (no crashes or invalid states)
+
+### 3. QueuePendingCombinationsTest
+
+**Description:** Verifies retrieval of pending student-course combinations that need exam results.
+
+- Test action: Call `store_imu_data(linear_x, linear_y, linear_z, ang_x, ang_y, ang_z)` without an active DB connection.
+- Expected result: Returns `false` and does not throw — method must check connection status before DB operations.
+
+### 4. CreateTablesNoCrash
+
+Description: Verifies calling `create_tables()` without an active connection is safe.
+
+- Test action: Call `create_tables()` on a manager that is not connected.
+- Expected result: No exception thrown; the function should be a no-op when no DB connection exists.

doc/tests/IMUDatabaseWriter.md

@@ -0,0 +1,18 @@
+# IMU Database Writer Unit Tests
+
+Unit tests for `IMUDatabaseWriter` are implemented in `src/g2_2025_imu_reader_pkg/test/IMUDatabaseWriter.test.cpp` using Google Test and ROS2 test utilities. These tests validate that the node subscribes to the `imu_data` topic, receives IMU messages, and forwards parsed values to the `DatabaseManager` interface. Tests use dependency injection with a mock `DatabaseManager` to avoid requiring a real database connection.
+
+## Test Cases
+
+### 1. ConstructorTest
+
+**Description:** Verifies that `IMUDatabaseWriter` can be constructed.
+
+- **Test Action:** Create `IMUDatabaseWriter` node with a `MockDatabaseManager`
+- **Expected Result:** Node instance created successfully without exceptions
+
+### 2. ReceivesAndForwardsIMU
+
+**Description:** Tests that the node receives IMU messages on the `imu_data` topic and calls `store_imu_data(...)` on the injected database manager.
+
+- **Expected Result:** Mock's `called_` flag is set to `true`, confirming the node forwarded the IMU data to the database manager

doc/tests/IntegrationTests.md

@@ -0,0 +1,284 @@
+# IMU System Integration Tests
+
+This document describes integration tests for the IMU data pipeline (IMU reader ESP32, Lifecycle MITM node, Database writer node).
+These tests will verify end-to-end functionality from ESP32 sensor data to database storage.
+
+## Test Overview
+
+The integration tests validate the complete flow through the main components:
+1. ESP32 MPU6886 Sensor - Data fetching and transmission (MQTT or Serial)
+2. ROS2 Lifecycle Node - Message reception from ESP and forwarding to database writer
+3. IMU Database Writer Node - Database storage
+
+## Test Environment Setup
+
+### Prerequisites
+
+- ESP32 with MPU6886 sensor configured and flashed
+- PostgreSQL database running (can be ran using dockerfile documented in the [README](/README.md))
+- ROS2 workspace built and sourced
+- if testing MQTT mode, MQTT broker running
+- if testing Serial mode, Serial port access
+
+### Configuration Files
+
+- `src/config.toml` - Database connection parameters
+- `IMU/sdkconfig` - Serial/MQTT mode selection and network settings
+
+---
+
+## Integration Test Cases
+
+### Test 1: Database Writer Persistence Verification
+
+> Verify that the Database Writer node correctly saves data to the PostgreSQL database when
+> IMU messages are received.
+
+#### Test Setup
+
+1. Start PostgreSQL database (docker compose up)
+2. Verify database connection, if incorrect change settings settings in `src/config.toml`
+3. Clear or note the current state of the IMU data table
+4. Launch the Database Writer node:
+   ```bash
+   ros2 run g2_2025_imu_reader_pkg g2_2025_imu_database_writer_node
+   ```
+
+#### Test Procedure
+
+1. Query the database for initial row count
+   ```sql
+   SELECT COUNT(*) FROM imu_data;
+   ```
+
+2. Use ROS2 CLI to publish IMU data
+   ```bash
+   ros2 topic pub --once /imu_data sensor_msgs/msg/Imu "{
+     linear_acceleration: {x: 0.5, y: 0.6, z: 9.8},
+     angular_velocity: {x: 0.1, y: 0.2, z: 0.3}
+   }"
+   ```
+
+3. Query the database for new entries
+   ```sql
+   SELECT * FROM imu_data ORDER BY timestamp DESC LIMIT 1;
+   ```
+
+4. Send 10 messages with varying data
+   ```bash
+   for i in {1..10}; do
+     ros2 topic pub --once /imu_data sensor_msgs/msg/Imu "{
+       linear_acceleration: {x: $(echo "scale=2; $i * 0.1" | bc), y: 0.0, z: 9.8},
+       angular_velocity: {x: 0.0, y: 0.0, z: 0.0}
+     }"
+     sleep 0.5
+   done
+   ```
+
+5. Confirm all 10 messages were persisted
+   ```sql
+   SELECT COUNT(*) FROM imu_data WHERE timestamp > (NOW() - INTERVAL '1 minute');
+   ```
+
+#### Expected Results
+
+- [ ] Each published message creates one database row
+- [ ] Linear acceleration values (x, y, z) match published data
+- [ ] Angular velocity values (x, y, z) match published data
+- [ ] Timestamps are automatically generated and sequential
+- [ ] No data loss occurs
+
+### Test 2: Lifecycle Node Message Forwarding
+
+> Verify that the Lifecycle Node correctly receives IMU data from either Serial or MQTT sources and
+> forwards it to the `imu_data` topic for consumption by the Database Writer.
+
+#### Test Setup
+
+1. Ensure ROS2 environment is sourced
+2. Launch the Lifecycle Node and Database Writer Node:
+   ```bash
+   ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node
+   ros2 run g2_2025_imu_reader_pkg g2_2025_imu_database_writer_node
+   ```
+4. Monitor the `imu_data` topic:
+   ```bash
+   ros2 topic echo /imu_data
+   ```
+
+#### Test Procedure - Serial Mode
+
+1. Configure ESP32 for Serial Output:
+   - Ensure `CONFIG_ENV_MQTT_ENABLED` is not defined in ESP32 sdkconfig
+   - Flash ESP32 with serial configuration
+
+2. Connect ESP32 via Serial:
+   - Connect ESP32 to computer via USB
+   - Identify serial port (e.g., `/dev/ttyUSB0` on Linux)
+   - Configure lifecycle node to read from this serial port
+
+3. Verify Data Flow:
+   - Observe lifecycle node logs for incoming serial data
+   - Confirm `ros2 topic echo /imu_data` displays messages
+   - Verify message fields match ESP32 output:
+     - `linear_acceleration.x/y/z` matches `accel.x/y/z`
+     - `angular_velocity.x/y/z` matches `gyro.x/y/z`
+
+4. Verify Database storage:
+   - Check database for new entries
+   - Confirm values match ESP32 sensor readings
+
+#### Test Procedure - MQTT Mode
+
+1. Start MQTT Broker:
+   ```bash
+   mosquitto -v
+   ```
+
+2. **Configure ESP32 for MQTT Output**:
+   - Enable `CONFIG_ENV_MQTT_ENABLED` in ESP32 sdkconfig
+   - Configure MQTT broker URI (`mqtt://192.168.1.100:1883`)
+   - Set MQTT topic (`CONFIG_MQTT_TOPIC = "imu/data"`)
+   - Configure WiFi credentials
+   - Flash ESP32 with MQTT configuration
+
+3. Verify MQTT Publishing:
+   - Subscribe to MQTT topic to confirm ESP32 is publishing:
+     ```bash
+     mosquitto_sub -h localhost -t "imu/data" -v
+     ```
+   - Expected MQTT payload format:
+     ```json
+     {"accel":{"x":0.123,"y":0.456,"z":9.800},"gyro":{"x":0.012,"y":0.023,"z":0.034},"Temp":25.50}
+     ```
+
+4. Configure Lifecycle Node for MQTT:
+   - Set lifecycle node to subscribe to MQTT broker and topic
+   - Restart lifecycle node with MQTT configuration
+
+5. Verify Data Flow:
+   - Observe lifecycle node logs for incoming MQTT messages
+   - Confirm `ros2 topic echo /imu_data` displays messages
+
+6. Verify Database storage:
+   - Check database for continuously arriving data
+   - Confirm timestamps are recent and sequential
+
+#### Expected Results
+
+**Serial Mode:**
+- [ ] Lifecycle node successfully reads JSON-formatted messages from serial port
+- [ ] Messages are parsed and converted to `sensor_msgs/msg/Imu` format
+- [ ] All IMU data fields are correctly mapped
+- [ ] Messages are published to `/imu_data` topic
+- [ ] Database writer receives and persists data
+
+**MQTT Mode:**
+- [ ] Lifecycle node successfully subscribes to MQTT broker
+- [ ] MQTT messages are received and parsed
+- [ ] Messages are converted to `sensor_msgs/msg/Imu` format
+- [ ] All IMU data fields are correctly mapped
+- [ ] Messages are published to `/imu_data`
+- [ ] Database writer receives and persists data
+
+### Test 3: ESP32 Data Format Validation
+> Verify that the ESP32 correctly formats and transmits IMU data in both Serial
+> and MQTT modes according to the expected JSON schema.
+
+#### Test Setup
+
+1. ESP32 with MPU6886 sensor properly wired and powered
+2. IMU sensor calibrated (100 samples for gyro and accel)
+3. Serial terminal or MQTT subscriber ready to capture output
+
+#### Data Quality Checks
+
+- [ ] Accelerometer Z-axis reads ~9.8 (m/s)^2 when device is stationary and level
+- [ ] Gyroscope values near zero when device is stationary
+- [ ] Temperature reading is within expected range
+- [ ] No NaN or Inf values in output
+- [ ] Calibration offsets are properly applied
+
+---
+
+## End-to-End Integration Test
+
+**Objective**: Validate complete system integration from ESP32 sensor to database persistence.
+
+### Test Setup
+
+1. Clean database state (truncate IMU data table)
+2. Start PostgreSQL database
+3. Start MQTT broker (for MQTT test variant)
+4. Launch all ROS2 nodes:
+   - Lifecycle node
+   - Database writer node
+5. Power on and connect ESP32
+
+### Test Procedure
+
+1. System Initialization:
+   - Verify all nodes are running and healthy
+   - Check lifecycle node is connected to data source (Serial/MQTT)
+   - Confirm database writer node is subscribed to `/imu_data`
+
+2. Data Flow Verification:
+   - Let system run for 2 minutes
+   - Monitor ROS2 topics:
+     ```bash
+     ros2 topic hz /imu_data
+     ros2 topic bw /imu_data
+     ```
+
+3. **Database Query**:
+   ```sql
+   SELECT COUNT(*) FROM imu_data WHERE timestamp > (NOW() - INTERVAL '2 minutes');
+   SELECT
+     AVG(linear_accel_z) as avg_accel_z,
+     AVG(angular_vel_x) as avg_gyro_x,
+     MIN(timestamp) as first_sample,
+     MAX(timestamp) as last_sample
+   FROM imu_data
+   WHERE timestamp > (NOW() - INTERVAL '2 minutes');
+   ```
+
+4. Physical Movement Test:
+   - Pick up ESP32 and rotate it
+   - Observe changes in database values
+   - Verify accelerometer and gyroscope values change
+
+5. Stress Test:
+   - Let system run for 30 minutes
+   - Check for memory leaks or connection drops
+   - Verify continuous data storage
+
+### Expected Results
+
+- [ ] Data flows from ESP32 -> Lifecycle Node -> Database Writer -> PostgreSQL
+- [ ] Publishing rate at `/imu_data`
+- [ ] Database receives ~240 rows in 2 minutes
+- [ ] Average Z-axis acceleration is ~9.8 (m/s)^2 during stationary periods
+- [ ] Physical movements are reflected in database values
+- [ ] No data loss over extended operation (30 minutes)
+- [ ] All components remain stable without crashes
+
+## Test Execution Checklist
+
+### Pre-Test Verification
+- [ ] PostgreSQL database is running and accessible
+- [ ] Database schema is created (IMU data table exists)
+- [ ] ROS2 workspace is built and sourced
+- [ ] ESP32 firmware is flashed with correct configuration
+- [ ] MQTT broker is running
+- [ ] Serial port permissions are correct
+
+### During Test
+- [ ] Monitor node logs for errors or warnings
+- [ ] Check ROS2 topic publishing rates
+- [ ] Verify database connection remains active
+- [ ] Observe IMU data values
+
+### Post-Test Analysis
+- [ ] Review test results and logs
+- [ ] Document any failures or anomalies
+- [ ] Clean up test data if necessary

docker-compose.yml

@@ -0,0 +1,23 @@
+version: '3'
+
+services:
+    postgres-db:
+        image: postgres:15
+        container_name: postgres-db
+        environment:
+            - POSTGRES_PASSWORD=postgres
+            - POSTGRES_USER=postgres
+            - POSTGRES_DB=grades
+        ports:
+            - "5432:5432"
+    mosquitto:
+        image: eclipse-mosquitto
+        container_name: mosquitto
+        restart: unless-stopped
+        ports:
+            - "1883:1883"
+            - "9001:9001"
+        volumes:
+            - ./mosquitto/config:/mosquitto/config
+            - ./mosquitto/data:/mosquitto/data
+            - ./mosquitto/log:/mosquitto/log

mosquitto/config/mosquitto.conf

@@ -0,0 +1,15 @@
+# Simple Mosquitto configuration allowing anonymous access (insecure for production)
+
+# pid_file /var/run/mosquitto.pid
+
+# Persistence
+#persistence true
+#persistence_location /var/lib/mosquitto/
+
+# Logging
+
+# Default MQTT listener
+listener 1883
+allow_anonymous true
+
+# Include additional config fragments

src/config.toml

@@ -0,0 +1,12 @@
+[database]
+host = "localhost"
+port = 5432
+dbname = "grades"
+user = "postgres"
+password = "postgres"
+timeout = 30
+ssl = false
+
+[database.pool]
+min_connections = 1
+max_connections = 10

src/g2_2025_imu_reader_pkg/CMakeLists.txt

@@ -0,0 +1,105 @@
+cmake_minimum_required(VERSION 3.8)
+project(g2_2025_imu_reader_pkg)
+
+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)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+add_executable(g2_2025_imu_database_writer_node
+    src/g2_2025_imu_database_writer_node/Main.cpp
+    src/database/DatabaseManager.cpp
+    src/config/ConfigManager.cpp
+    src/g2_2025_imu_database_writer_node/nodes/IMUDatabaseWriter.cpp
+)
+target_include_directories(g2_2025_imu_database_writer_node PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_database_writer_node
+)
+ament_target_dependencies(g2_2025_imu_database_writer_node rclcpp sensor_msgs)
+target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tomlplusplus)
+
+install(
+    TARGETS
+        g2_2025_imu_database_writer_node
+    DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  # Add gtest for ConfigManager
+  ament_add_gtest(${PROJECT_NAME}_test_config_manager
+    test/ConfigManager.test.cpp
+    src/config/ConfigManager.cpp
+  )
+  target_include_directories(${PROJECT_NAME}_test_config_manager PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test_config_manager
+    rclcpp
+  )
+  target_link_libraries(${PROJECT_NAME}_test_config_manager
+    tomlplusplus::tomlplusplus
+  )
+
+  # Add gtest for IMUDatabaseWriter node
+  ament_add_gtest(${PROJECT_NAME}_test_imu_database_writer
+    test/IMUDatabaseWriter.test.cpp
+    src/g2_2025_imu_database_writer_node/nodes/IMUDatabaseWriter.cpp
+    src/database/DatabaseManager.cpp
+    src/config/ConfigManager.cpp
+  )
+  target_include_directories(${PROJECT_NAME}_test_imu_database_writer PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_database_writer_node
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test_imu_database_writer
+    rclcpp
+    sensor_msgs
+  )
+  target_link_libraries(${PROJECT_NAME}_test_imu_database_writer
+    pqxx pq tomlplusplus::tomlplusplus
+  )
+
+  # Add gtest for DatabaseManager
+  ament_add_gtest(${PROJECT_NAME}_test_database_manager
+    test/DatabaseManager.test.cpp
+    src/database/DatabaseManager.cpp
+    src/config/ConfigManager.cpp
+  )
+  target_include_directories(${PROJECT_NAME}_test_database_manager PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test_database_manager
+    rclcpp
+  )
+  target_link_libraries(${PROJECT_NAME}_test_database_manager
+    pqxx pq tomlplusplus::tomlplusplus
+  )
+
+  # Add Python integration tests
+  #   find_package(ament_cmake_pytest REQUIRED)
+  #   ament_add_pytest_test(${PROJECT_NAME}_integration_test test/test_integration_system.py
+  #     TIMEOUT 60
+  #   )
+endif()
+
+ament_package()

src/g2_2025_imu_reader_pkg/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>g2_2025_imu_reader_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wessel@go2it.eu">wessel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/g2_2025_imu_reader_pkg/src/config/ConfigManager.cpp

@@ -0,0 +1,112 @@
+#include "ConfigManager.hpp"
+
+#include <iostream>
+#include <fstream>
+
+namespace assignments::two {
+
+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>("grades");
+    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::two

src/g2_2025_imu_reader_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::two {
+
+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::two

src/g2_2025_imu_reader_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::two {
+
+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::two

src/g2_2025_imu_reader_pkg/src/database/DatabaseManager.cpp

@@ -0,0 +1,116 @@
+#include "DatabaseManager.hpp"
+
+#include <pqxx/pqxx>
+#include "rclcpp/rclcpp.hpp"
+
+#include "SQLQueries.hpp"
+#include "config/ConfigManager.hpp"
+
+namespace assignments::two {
+
+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.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;
+    }
+}
+
+} // namespace assignments::two

src/g2_2025_imu_reader_pkg/src/database/DatabaseManager.hpp

@@ -0,0 +1,46 @@
+/* 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::two {
+
+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_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::two

src/g2_2025_imu_reader_pkg/src/database/SQLQueries.hpp

@@ -0,0 +1,29 @@
+/* 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
+ */
+#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_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);
+)";

src/g2_2025_imu_reader_pkg/src/g2_2025_imu_database_writer_node/Main.cpp

@@ -0,0 +1,21 @@
+/* main.cpp
+ * Entry point for the exam result generator node
+ *
+ * Reviewed by: <x>
+ * Changelog:
+ * [23-09-2025] Wessel T: Simplified main.cpp to entry point only
+ */
+
+#include "rclcpp/rclcpp.hpp"
+#include "nodes/IMUDatabaseWriter.hpp"
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<assignments::two::imu_database_writer::IMUDatabaseWriter>();
+
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+
+    return 0;
+}

src/g2_2025_imu_reader_pkg/src/g2_2025_imu_database_writer_node/nodes/IMUDatabaseWriter.cpp

@@ -0,0 +1,43 @@
+#include "IMUDatabaseWriter.hpp"
+
+namespace assignments::two::imu_database_writer {
+
+IMUDatabaseWriter::IMUDatabaseWriter(std::unique_ptr<DatabaseManager> db_manager)
+    : Node("g2_2025_imu_database_writer_node")
+{
+    // allow injection of mock database manager for tests
+    if (db_manager) {
+        db_manager_ = std::move(db_manager);
+    } else {
+        db_manager_ = std::make_unique<DatabaseManager>(this->get_logger());
+    }
+
+    // Create subscriber for IMU data
+    imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>(
+        "imu_data", 10,
+        std::bind(
+            &IMUDatabaseWriter::imu_data_callback,
+            this,
+            std::placeholders::_1
+        )
+    );
+
+    RCLCPP_INFO(this->get_logger(),
+        "imu_database_writer started, ready to receive IMU data"
+    );
+}
+
+void IMUDatabaseWriter::imu_data_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    RCLCPP_DEBUG(this->get_logger(),
+        "Received: linear_accel=[%.2f, %.2f, %.2f], angular_vel=[%.2f, %.2f, %.2f]",
+        msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,
+        msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z
+    );
+
+    db_manager_->store_imu_data(
+        msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,
+        msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z
+    );
+}
+
+} // namespace assignments::two::imu_database_writer

src/g2_2025_imu_reader_pkg/src/g2_2025_imu_database_writer_node/nodes/IMUDatabaseWriter.hpp

@@ -0,0 +1,38 @@
+/* nodes/IMUDatabaseWriter.hpp
+ * IMU database writer node for storing IMU sensor data.
+ *
+ * Receives IMU sensor data from sensor_msgs/msg/Imu messages and stores
+ * the data to a database for further processing.
+ *
+ * Changelog:
+ * [13-10-2025] Wessel T: Implement template
+ * [14-10-2025] Wessel T: Updated to use sensor_msgs/msg/Imu
+ */
+#pragma once
+
+#include <memory>
+#include <string>
+#include <random>
+#include <vector>
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+#include "database/DatabaseManager.hpp"
+
+namespace assignments::two::imu_database_writer {
+
+class IMUDatabaseWriter : public rclcpp::Node {
+public:
+    IMUDatabaseWriter(std::unique_ptr<DatabaseManager> db_manager = nullptr);
+
+    void imu_data_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
+
+    std::unique_ptr<DatabaseManager> db_manager_;
+};
+
+} // namespace assignments::two::imu_database_writer

src/g2_2025_imu_reader_pkg/test/ConfigManager.test.cpp

@@ -0,0 +1,135 @@
+#include <filesystem>
+#include <fstream>
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include "config/ConfigManager.hpp"
+
+using namespace assignments::two;
+
+static const std::string TEST_CONFIG_CONTENT = R"(
+[database]
+host = "test_host"
+port = 1234
+dbname = "test_db"
+user = "test_user"
+password = "test_password"
+timeout = 60
+ssl = true
+
+[database.pool]
+min_connections = 2
+max_connections = 20
+)";
+
+static const std::string TEST_CONFIG_NO_POOL_CONTENT = R"(
+[database]
+host = "localhost"
+port = 5432
+dbname = "grades"
+user = "postgres"
+password = "postgres"
+)";
+
+
+class ConfigManagerTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        rclcpp::init(0, nullptr);
+        node_ = std::make_shared<rclcpp::Node>("test_config_node");
+    }
+
+    void TearDown() override {
+        // Clean up temporary files
+        if (std::filesystem::exists(temp_config_file_)) {
+            std::filesystem::remove(temp_config_file_);
+        }
+
+        node_.reset();
+        rclcpp::shutdown();
+    }
+
+    void create_temp_config_file() {
+        temp_config_file_ = "test_config.toml";
+        std::ofstream file(temp_config_file_);
+        file << TEST_CONFIG_CONTENT;
+        file.close();
+    }
+
+    std::shared_ptr<rclcpp::Node> node_;
+    std::string temp_config_file_;
+};
+
+TEST_F(ConfigManagerTest, ConstructorTest) {
+    // Test ConfigManager can be constructed with logger
+    ConfigManager config_manager(node_->get_logger());
+    // Constructor should not crash
+    SUCCEED();
+}
+
+TEST_F(ConfigManagerTest, LoadValidConfigTest) {
+    create_temp_config_file();
+
+    ConfigManager config_manager(node_->get_logger());
+    bool result = config_manager.load_config(temp_config_file_);
+
+    EXPECT_TRUE(result);
+    EXPECT_TRUE(config_manager.is_loaded());
+}
+
+TEST_F(ConfigManagerTest, LoadInvalidFileTest) {
+    ConfigManager config_manager(node_->get_logger());
+    bool result = config_manager.load_config("non_existent_file.toml");
+
+    EXPECT_FALSE(result);
+    EXPECT_FALSE(config_manager.is_loaded());
+}
+
+TEST_F(ConfigManagerTest, DatabaseConfigParsingTest) {
+    create_temp_config_file();
+
+    ConfigManager config_manager(node_->get_logger());
+    config_manager.load_config(temp_config_file_);
+
+    auto db_config = config_manager.get_database_config();
+
+    ASSERT_TRUE(db_config.has_value());
+    EXPECT_EQ(db_config->host, "test_host");
+    EXPECT_EQ(db_config->port, 1234);
+    EXPECT_EQ(db_config->dbname, "test_db");
+    EXPECT_EQ(db_config->user, "test_user");
+    EXPECT_EQ(db_config->password, "test_password");
+    EXPECT_EQ(db_config->timeout, 60);
+    EXPECT_TRUE(db_config->ssl);
+    EXPECT_EQ(db_config->min_connections, 2);
+    EXPECT_EQ(db_config->max_connections, 20);
+}
+
+TEST_F(ConfigManagerTest, DatabaseConfigWithoutPoolTest) {
+    // Create config without pool section
+    std::ofstream file("test_config_no_pool.toml");
+    file << TEST_CONFIG_NO_POOL_CONTENT;
+    file.close();
+
+    ConfigManager config_manager(node_->get_logger());
+    config_manager.load_config("test_config_no_pool.toml");
+
+    auto db_config = config_manager.get_database_config();
+
+    ASSERT_TRUE(db_config.has_value());
+    EXPECT_EQ(db_config->host, "localhost");
+    EXPECT_EQ(db_config->port, 5432);
+    EXPECT_EQ(db_config->min_connections, 1);  // default
+    EXPECT_EQ(db_config->max_connections, 10); // default
+
+    std::filesystem::remove("test_config_no_pool.toml");
+}
+
+TEST_F(ConfigManagerTest, GetConfigWithoutLoadingTest) {
+    ConfigManager config_manager(node_->get_logger());
+
+    auto db_config = config_manager.get_database_config();
+
+    EXPECT_FALSE(db_config.has_value());
+    EXPECT_FALSE(config_manager.is_loaded());
+}

src/g2_2025_imu_reader_pkg/test/DatabaseManager.test.cpp

@@ -0,0 +1,47 @@
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include "database/DatabaseManager.hpp"
+
+using namespace assignments::two;
+
+class DatabaseManagerTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        rclcpp::init(0, nullptr);
+
+        node_ = std::make_shared<rclcpp::Node>("test_db_node");
+        db_manager_ = std::make_unique<DatabaseManager>(node_->get_logger());
+    }
+
+    void TearDown() override {
+        db_manager_.reset();
+        node_.reset();
+        rclcpp::shutdown();
+    }
+
+    std::shared_ptr<rclcpp::Node> node_;
+    std::unique_ptr<DatabaseManager> db_manager_;
+};
+
+TEST_F(DatabaseManagerTest, ConstructorTest) {
+    // Test DatabaseManager can be constructed
+    ASSERT_NE(db_manager_, nullptr);
+}
+
+TEST_F(DatabaseManagerTest, ConnectionStatusTest) {
+    // Should return a boolean (connected or not) — here no DB configured so expect false
+    bool status = db_manager_->is_connected();
+    EXPECT_TRUE(status == true || status == false);
+}
+
+TEST_F(DatabaseManagerTest, StoreIMUDataWhenNotConnected) {
+    // Without a real DB connection, storing IMU data should return false
+    bool result = db_manager_->store_imu_data(1.0, 2.0, 3.0, 0.1, 0.2, 0.3);
+    EXPECT_FALSE(result);
+}
+
+TEST_F(DatabaseManagerTest, CreateTablesNoCrash) {
+    // create_tables should be safe to call even when not connected (no throw)
+    EXPECT_NO_THROW(db_manager_->create_tables());
+}

src/g2_2025_imu_reader_pkg/test/IMUDatabaseWriter.test.cpp

@@ -0,0 +1,69 @@
+#include <chrono>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <gtest/gtest.h>
+
+#include "g2_2025_imu_database_writer_node/nodes/IMUDatabaseWriter.hpp"
+#include "mocks/MockDatabaseManager.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+using namespace std::chrono_literals;
+using namespace assignments::two::imu_database_writer;
+
+class IMUDatabaseWriterTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        rclcpp::init(0, nullptr);
+        test_node_ = std::make_shared<rclcpp::Node>("test_node");
+
+        // create mock db and inject
+        mock_db_ = std::make_unique<assignments::two::MockDatabaseManager>(rclcpp::get_logger("test_mock_db"));
+        mock_db_ptr_ = mock_db_.get();
+
+        imu_node_ = std::make_shared<IMUDatabaseWriter>(std::move(mock_db_));
+
+        // publisher in test node
+        imu_publisher_ = test_node_->create_publisher<sensor_msgs::msg::Imu>("imu_data", 10);
+    }
+
+    void TearDown() override {
+        imu_node_.reset();
+        test_node_.reset();
+        rclcpp::shutdown();
+    }
+
+    void spin_some_time(std::chrono::milliseconds duration = 200ms) {
+        auto start = std::chrono::steady_clock::now();
+        while (std::chrono::steady_clock::now() - start < duration) {
+            rclcpp::spin_some(test_node_);
+            if (imu_node_) rclcpp::spin_some(imu_node_);
+            std::this_thread::sleep_for(10ms);
+        }
+    }
+
+    std::shared_ptr<rclcpp::Node> test_node_;
+    std::shared_ptr<IMUDatabaseWriter> imu_node_;
+    std::unique_ptr<assignments::two::MockDatabaseManager> mock_db_;
+    assignments::two::MockDatabaseManager* mock_db_ptr_ = nullptr;
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
+};
+
+TEST_F(IMUDatabaseWriterTest, ConstructorTest) {
+    ASSERT_NE(imu_node_, nullptr);
+}
+
+TEST_F(IMUDatabaseWriterTest, ReceivesAndForwardsIMU) {
+    auto msg = sensor_msgs::msg::Imu();
+    msg.linear_acceleration.x = 1.23;
+    msg.linear_acceleration.y = -0.5;
+    msg.linear_acceleration.z = 0.0;
+    msg.angular_velocity.x = 0.01;
+    msg.angular_velocity.y = -0.02;
+    msg.angular_velocity.z = 0.03;
+
+    imu_publisher_->publish(msg);
+
+    spin_some_time(300ms);
+
+    EXPECT_TRUE(mock_db_ptr_->called_);
+}

src/g2_2025_imu_reader_pkg/test/mocks/MockDatabaseManager.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "database/DatabaseManager.hpp"
+
+namespace assignments::two {
+
+class MockDatabaseManager : public DatabaseManager {
+public:
+    explicit MockDatabaseManager(
+        rclcpp::Logger logger = rclcpp::get_logger("fake_db")
+    )
+    : DatabaseManager(logger) {}
+
+    bool is_connected() const override {
+        return connection_status_;
+    }
+
+    void init_database() override {}
+
+    void set_connection_status(bool status) {
+        connection_status_ = status;
+    }
+
+    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
+    ) override {
+        called_ = true;
+        last_la_x_ = linear_accel_x;
+        last_la_y_ = linear_accel_y;
+        last_la_z_ = linear_accel_z;
+        last_av_x_ = angular_vel_x;
+        last_av_y_ = angular_vel_y;
+        last_av_z_ = angular_vel_z;
+        return true;
+    }
+
+    bool called_ = false;
+    double last_la_x_ = 0.0, last_la_y_ = 0.0, last_la_z_ = 0.0;
+    double last_av_x_ = 0.0, last_av_y_ = 0.0, last_av_z_ = 0.0;
+
+private:
+    bool connection_status_ = true;
+};
+
+} // namespace assignments::two