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

Reviewed-on: http://git.wessel.gg/inholland/ros2-assignments/pulls/14
Reviewed-by: Wessel T <contact@wessel.gg>

.gitignore

@@ -31,6 +31,7 @@ qtcreator-*
 
 # Colcon custom files
 COLCON_IGNORE
+!src/*/COLCON_IGNORE
 AMENT_IGNORE
 
 .vscode

doc/architecture/architecture.md

@@ -33,14 +33,23 @@ The system consists of multiple ROS2 nodes that communicate through standardized
 
 *For detailed documentation, see: [IMUDatabaseWriter.md](nodes/IMUDatabaseWriter.md)*
 
-#### 2. LifeCycle Node - NEEDS TO BE EDITED STILL
-**Namespace**: `assignments::one::grade_calculator`
+#### 2. LifeCycle Node
+**Namespace**: `assignments::two::lifecycle_manager`
 
-**Brief Description**: Provides grade calculation service with business logic including bonus points and grade validation.
+**Brief Description**: Configures low level communication and manages the hardware interface to read on either serial or mqtt comunications.
 
-**Key Features**: Average calculation, special student rules, grade bounds validation (10-100)
+**Key Features**: Configuration of communications, device read activation and deactivation.
 
-*For detailed documentation, see: [GradeCalculator.md](nodes/GradeCalculator.md)*
+*For detailed documentation, see: [LifecycleManager.md](nodes/LifecycleManager.md)*
+
+#### 3. Hardware Interface
+**Namespace**: `assignments::two::hardware_interface`
+
+**Brief Description**: Manages low-level communication protocols (Serial/MQTT) for ESP32 data acquisition and forwards raw sensor data to processing nodes.
+
+**Key Features**: Multi-protocol communication support, connection management, raw data parsing and streaming.
+
+*For detailed documentation, see: [HardwareInterface.md](nodes/HardwareInterface.md)*
 
 ### Data Management
 
@@ -65,7 +74,7 @@ The system consists of multiple ROS2 nodes that communicate through standardized
 
 ## System Workflow
 
-### 1. Exam Result Processing
+### 1. Imu data  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

doc/architecture/classes/HardwareInterface.md

@@ -0,0 +1,355 @@
+# HardwareInterface (`assignments::two::g2_2025_lifecycle_node`)
+
+## Overview
+The `HardwareInterface` is a c++ class responsible for managing low-level hardware communication with ESP32-IMU combination via serial/MQTT and provides JSON parsing of sensor data, and publishes standardized `sensor_msgs::msg::Imu` messages to the database writer.
+
+#### Implementation Details
+
+**Public Methods**
+
+- **`start_read()`**: Spawns a background thread that continuously reads JSON payloads from the serial device
+- **`stop_read()`**: Signals the reader thread to exit and joins it for clean shutdown
+- **`write(const std::string& data)`**: Writes data to the serial device
+- **`open_device(const std::string& device_path, int baud_rate)`**: Opens and configures a serial port
+- **`is_device_open()`**: Checks if the serial device is currently open
+- **`close_device()`**: Closes the serial port and releases resources
+- **`mqtt_configure()`**: Initializes MQTT client and broker connection setup
+- **`mqtt_reader()`**: Attaches MQTT callbacks to begin receiving messages
+- **`mqtt_connect()`**: Establishes connection to the MQTT broker and subscribes to topics
+- **`close_mqtt_conn()`**: Disconnects from broker and cleans up MQTT resources
+- **`parse_data(const std::string& data)`**: Parses JSON payload into `sensor_msgs::msg::Imu` and publishes
+- **`publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg)`**: Publishes an IMU message to the ROS topic
+
+**Constructor**
+
+```cpp
+HardwareInterface()
+```
+
+- Initializes ROS2 node with name `hardware_interface`
+- Creates a ROS2 publisher for `sensor_msgs::msg::Imu` on topic `imu_data` with queue size 10
+- Logs initialization status
+
+**Member Variables**
+
+- **`serialib serial`**: Encapsulates serial port communication
+- **`std::shared_ptr<mqtt::async_client> mqtt_client`**: Persistent MQTT async client for broker communication
+- **`std::shared_ptr<mqtt::callback> mqtt_cb`**: MQTT callback handler for message arrival events
+- **`std::thread read_thread_`**: Background reader thread for continuous serial data acquisition
+- **`std::atomic_bool reading_`**: Thread-safe flag to signal the reader thread to stop
+- **`std::string partial_buffer_`**: Accumulates fragmented serial reads until complete messages are available
+- **`rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher`**: ROS2 publisher for IMU data
+
+**MQTT Constants**
+
+- **`SERVER_ADDRESS`**: `"tcp://localhost:1883"` — Default MQTT broker address
+- **`CLIENT_ID`**: `"cpp_mqtt_client"` — MQTT client identifier
+- **`TOPIC`**: `"esp32/imu"` — Default subscription topic for IMU data
+
+## Core Functions
+
+### `void start_read()`
+
+Initiates continuous serial data acquisition in a background thread.
+
+**Behavior:**
+- Checks if a reader thread is already running; returns if so
+- Sets the `reading_` atomic flag to true
+- Spawns a thread that:
+  1. Allocates a 116-byte buffer
+  2. Enters a loop that runs while `reading_` is true
+  3. Calls `serial.readString()` with a 1-second timeout
+  4. Accumulates received bytes into `partial_buffer_`
+  5. Splits on newline (`\n`) to extract complete lines
+  6. Trims whitespace and strips leading garbage up to the first `{`
+  7. Validates that a closing `}` exists; if not, waits for more data
+  8. Calls `parse_data()` on each complete JSON line
+- Returns immediately while the thread continues running
+
+**Error Handling:**
+- Invalid JSON lines are logged as errors in `parse_data()` but do not crash the thread
+
+### `void stop_read()`
+
+Cleanly terminates the background reader thread.
+
+**Behavior:**
+- Returns immediately if not currently reading
+- Sets `reading_` atomic flag to false to signal the thread
+- Joins the thread to wait for its completion
+- Ensures all resources are released before returning
+
+**Thread Safety:**
+- Uses atomic flag for lock-free signaling
+- Blocks until thread joins, guaranteeing clean shutdown
+
+### `void parse_data(const std::string& data)`
+
+Deserializes a JSON string into a ROS2 IMU message and publishes it.
+
+**Behavior:**
+- Attempts to parse the input string as JSON using `nlohmann::json`
+- Creates a new `sensor_msgs::msg::Imu` message and populates:
+  - **Header**: Sets `stamp` to current time via `this->now()` and `frame_id` to `"imu_link"`
+  - **Linear Acceleration**: Extracts from JSON `"accel"` object fields `"x"`, `"y"`, `"z"` (defaults to 0.0 if missing)
+  - **Angular Velocity**: Extracts from JSON `"gyro"` object fields `"x"`, `"y"`, `"z"` (defaults to 0.0 if missing)
+- Logs the parsed IMU values at INFO level
+- Calls `publish_imu_data()` to send the message to the ROS topic
+
+**Expected JSON Format:**
+```json
+{
+  "accel": {"x": 0.037, "y": -1.164, "z": 9.775},
+  "gyro": {"x": -0.024, "y": -0.014, "z": -0.001},
+  "Temp": 41.01
+}
+```
+
+**Error Handling:**
+- Catches `nlohmann::json::exception` and logs parsing errors without crashing
+- Handles missing fields gracefully using `.value()` with default 0.0
+
+### `void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg)`
+
+Publishes an IMU message to the ROS2 topic.
+
+**Behavior:**
+- Dereferences the shared pointer and publishes to `imu_publisher`
+- Operation is thread-safe (rclcpp publishers support multi-threaded access)
+
+### `void mqtt_configure()`
+
+Sets up the MQTT infrastructure for broker communication.
+
+**Behavior:**
+- Creates a persistent `mqtt::async_client` pointing to `SERVER_ADDRESS` if not already created
+- Creates a persistent MQTT callback handler if not already created
+- Calls `mqtt_connect()` to establish the connection
+
+**Rationale for Persistence:**
+- Client and callback objects must outlive this function to maintain the connection
+- Using `shared_ptr` ensures proper lifetime management
+
+### `void mqtt_reader()`
+
+Attaches callbacks to the MQTT client to begin receiving messages.
+
+**Behavior:**
+- Sets the callback handler on the async client via `mqtt_client->set_callback(*mqtt_cb)`
+- Logs that the listener has started
+- Returns; the async client handles message reception in background threads
+
+### `void mqtt_connect()`
+
+Establishes connection to the MQTT broker and subscribes to the sensor topic.
+
+**Behavior:**
+- Creates `mqtt::connect_options` with:
+  - Keep-alive interval: 20 seconds
+  - Clean session: true (no prior session state restored)
+- Calls `mqtt_client->connect()` and waits for completion
+- Subscribes to `TOPIC` (default: `"esp32/imu"`) with QoS level 1
+- Logs successful connection and subscription
+
+**Error Handling:**
+- Catches `mqtt::exception` and logs errors; does not throw or crash
+
+### `void close_mqtt_conn()`
+
+Cleanly disconnects from the MQTT broker and cleans up resources.
+
+**Behavior:**
+- Checks if the client is connected before attempting disconnect
+- Calls `mqtt_client->disconnect()` and waits for completion
+- Resets `mqtt_client` and `mqtt_cb` shared pointers to allow object destruction
+- Logs disconnection and cleanup status
+
+**Error Handling:**
+- Catches `mqtt::exception` and logs errors
+- Continues cleanup even if errors occur
+
+### `bool open_device(const std::string& device_path, int baud_rate)`
+
+Opens and configures a serial port device.
+
+**Parameters:**
+- `device_path`: Path to the serial device (e.g., `"/dev/ttyUSB0"`)
+- `baud_rate`: Communication speed in bits per second (e.g., `115200`)
+
+**Returns:**
+- `true` if device opened successfully
+- `false` if an error occurs
+
+**Behavior:**
+- Calls `serial.openDevice()` with the provided path and baud rate
+- Checks if the returned value is 1 (success)
+- Logs success or error status
+
+### `bool is_device_open()`
+
+Queries the current state of the serial device.
+
+**Returns:**
+- `true` if the device is open
+- `false` otherwise
+
+### `void close_device()`
+
+Closes the serial port and releases resources.
+
+**Behavior:**
+- Calls `serial.closeDevice()`
+- Ensures the device is no longer accessible for reads/writes
+
+### `void write(const std::string& data)`
+
+Writes data to the serial device.
+
+**Behavior:**
+- Logs the write operation
+- Calls `serial.writeString()` with the data
+
+## MQTT Callback Handler
+
+### `class callback : public virtual mqtt::callback`
+
+A nested class that implements the Paho MQTT callback interface.
+
+**Method: `message_arrived(mqtt::const_message_ptr msg)`**
+
+- Invoked when a message arrives on a subscribed topic
+- Extracts the payload string via `msg->get_payload_str()`
+- Calls `parse_data()` to deserialize and publish the IMU message
+
+## Data Flow Architecture
+
+### Serial Data Path
+
+```
+Physical IMU Device
+         ↓
+Serial Port (e.g., /dev/ttyUSB0 @ 115200 baud)
+         ↓
+start_read() Background Thread
+         ↓
+serial.readString(buffer, 1000ms timeout)
+         ↓
+Accumulate into partial_buffer_
+         ↓
+Split on '\n' and Extract Complete Lines
+         ↓
+Sanitize (trim, strip garbage before '{')
+         ↓
+Validate JSON Structure (must have '{' and '}')
+         ↓
+parse_data(json_line)
+         ↓
+JSON Parse → sensor_msgs::msg::Imu
+         ↓
+publish_imu_data() → ROS Topic `imu_data`
+```
+
+### MQTT Data Path
+
+```
+MQTT Broker (tcp://localhost:1883)
+         ↓
+MQTT Async Client (mqtt_client)
+         ↓
+Topic Subscription (esp32/imu)
+         ↓
+MQTT Callback (message_arrived)
+         ↓
+parse_data(payload_string)
+         ↓
+JSON Parse → sensor_msgs::msg::Imu
+         ↓
+publish_imu_data() → ROS Topic `imu_data`
+```
+
+## Buffer Management & Message Reconstruction
+
+The `partial_buffer_` member implements a robust strategy for handling fragmented serial reads:
+
+1. **Accumulation**: Each serial read chunk is appended to `partial_buffer_`
+2. **Line Splitting**: Buffer is searched for newline delimiters
+3. **Validation**: Each line is checked for JSON structure (presence of `{` and `}`)
+4. **Sanitization**: Leading garbage (characters before `{`) is stripped
+5. **Incomplete Message Handling**: If a line lacks a closing brace, it's pushed back to the buffer and the loop waits for more data
+6. **Parse & Publish**: Complete JSON lines are parsed and published
+
+**Why This Matters:**
+- Serial reads may return fragments of a JSON message (e.g., `",\"gyro\":{...}"`)
+- Multiple messages can arrive in a single read
+- Buffering ensures robust handling of all edge cases
+
+## Error Handling & Recovery
+
+| Scenario | Behavior | Recovery |
+|----------|----------|----------|
+| Serial read timeout | Loop continues, checks `reading_` flag | Automatic retry on next iteration |
+| Incomplete JSON in buffer | Fragment is retained; waits for next read | No action needed; accumulation handles it |
+| JSON parse error | Error logged; thread continues listening | Move to next message |
+| Serial device disconnect | readString returns 0; loop continues | Application can reconnect via `open_device()` |
+| MQTT broker unreachable | Exception caught and logged | Retry via `mqtt_connect()` |
+| MQTT message error | Exception caught and logged | Connection remains for next message |
+
+## Thread Safety
+
+- **Atomic Flag**: `reading_` uses `std::atomic_bool` for lock-free thread signaling
+- **Publisher Thread-Safety**: rclcpp publishers are thread-safe; `parse_data()` can safely publish from reader thread
+- **Resource Cleanup**: `stop_read()` joins the thread before returning, ensuring clean shutdown
+- **No Shared Mutable State**: Aside from `reading_` and the publisher, thread does not access other class members during execution
+
+---
+
+## Integration with LifecycleManager
+
+The `LifecycleManager` orchestrates `HardwareInterface` lifecycle:
+
+| Lifecycle Phase | LifecycleManager Call | HardwareInterface Action |
+|---|---|---|
+| **Configure** | `hw_interface->open_device()` or `mqtt_configure()` | Open serial port or set up MQTT client |
+| **Activate** | `hw_interface->start_read()` or `mqtt_reader()` | Spawn reader thread or attach MQTT callbacks |
+| **Deactivate** | `hw_interface->stop_read()` or `close_mqtt_conn()` | Stop reader thread and join; disconnect MQTT |
+| **Cleanup** | `hw_interface->close_device()` | Release serial port |
+
+---
+
+## Usage Example
+
+### Direct Instantiation (Advanced)
+
+```cpp
+// Create an instance (normally managed by LifecycleManager)
+auto hw = std::make_shared<HardwareInterface>();
+
+// Serial workflow
+hw->open_device("/dev/ttyUSB0", 115200);
+hw->start_read();
+// ... node spins and publishes IMU data ...
+hw->stop_read();
+hw->close_device();
+
+// MQTT workflow
+hw->mqtt_configure();
+hw->mqtt_reader();
+// ... node spins and publishes IMU data ...
+hw->close_mqtt_conn();
+```
+
+## Design Patterns
+
+1. **Abstraction Pattern**: Encapsulates serial and MQTT complexity behind a unified interface
+2. **Thread Management**: Background reader thread with atomic signaling for clean shutdown
+3. **Buffer Accumulation**: Handles fragmented reads and multi-message batches robustly
+4. **Dual Backend Strategy**: Runtime selection of communication mode (serial or MQTT)
+5. **JSON Deserialization**: Uses industry-standard `nlohmann::json` for robust parsing
+
+## Dependencies
+
+- **rclcpp**: ROS2 C++ client library
+- **sensor_msgs**: ROS2 standard sensor message definitions
+- **paho-mqtt**: Paho C/C++ MQTT client library
+- **nlohmann/json**: Header-only JSON parsing library
+- **serialib**: Custom serial communication wrapper

doc/architecture/classes/Simulator.md

@@ -0,0 +1,119 @@
+# Simulator (`assignments::three::Simulator`)
+
+The `Simulator` class provides a flexible time-based value generation engine that supports multiple interpolation types. It is used by both the IMU and Wheel data simulator nodes to generate configurable sensor data patterns.
+
+## Implementation Details
+
+**Namespace**: `assignments::three`
+
+**Header**: `simulator/Simulator.hpp`
+
+### Data Structures
+
+**SimType Enum**
+```cpp
+enum class SimType {
+    CONSTANT,   // y = c (constant value)
+    LINEAR,     // y = y0 + (y1-y0) * (t-t0)/(t1-t0)
+    QUADRATIC   // Lagrange interpolation through 3 points
+};
+```
+
+**IntervalConfig Struct**
+```cpp
+struct IntervalConfig {
+    SimType type;    // Interpolation type
+    double t_start;  // Interval start time
+    double t_end;    // Interval end time
+    double y_start;  // Start value
+    double y_end;    // End value
+    double t_mid;    // Mid-point time (quadratic only)
+    double y_mid;    // Mid-point value (quadratic only)
+};
+```
+
+### Constructor
+```cpp
+Simulator(rclcpp::Node* node, const std::vector<std::string>& objects)
+```
+- Takes a ROS2 node pointer for parameter access
+- Takes a list of object/channel names to configure
+- Loads interval configurations from ROS2 parameters
+- Validates intervals for overlaps (throws `std::runtime_error` if detected)
+
+## Core Functionality
+
+**`double get_object_value(const std::string& object, double t)`**
+- Returns the simulated value for a given object at time `t`
+- If `t` is within an interval, computes the interpolated value
+- If `t` is after all intervals, holds the last interval's end value
+- If `t` is before all intervals, returns 0.0
+- If object doesn't exist, returns 0.0
+
+**`double compute_value(double t, const IntervalConfig& interval)`** (private)
+- Computes the interpolated value based on interval type:
+  - **CONSTANT**: Returns `y_start`
+  - **LINEAR**: Lagrange interpolation between 2 points
+  - **QUADRATIC**: Lagrange interpolation through 3 points
+
+**`void load_intervals(rclcpp::Node* node, const std::vector<std::string>& objects)`** (private)
+- Declares and loads parameters for each object
+- Validates that intervals don't overlap
+- Respects `max_intervals` limit
+
+## Parameter Configuration
+
+For each object, the following parameters are used:
+
+| Parameter | Type | Description |
+|-----------|------|-------------|
+| `max_intervals` | int | Global maximum intervals per object |
+| `<object>.num_intervals` | int | Number of intervals for this object |
+| `<object>.interval_<n>.type` | string | "constant", "linear", or "quadratic" |
+| `<object>.interval_<n>.t_start` | double | Interval start time |
+| `<object>.interval_<n>.t_end` | double | Interval end time |
+| `<object>.interval_<n>.y_start` | double | Value at start |
+| `<object>.interval_<n>.y_end` | double | Value at end |
+| `<object>.interval_<n>.t_mid` | double | Mid-point time (quadratic) |
+| `<object>.interval_<n>.y_mid` | double | Mid-point value (quadratic) |
+
+## Example Configuration
+
+```yaml
+# Constant acceleration of 5.0 m/s² from t=0 to t=10
+linear_x:
+  num_intervals: 1
+  interval_0:
+    type: "constant"
+    t_start: 0.0
+    t_end: 10.0
+    y_start: 5.0
+
+# Linear ramp from 0 to 10 over 5 seconds
+wheel_fl:
+  num_intervals: 1
+  interval_0:
+    type: "linear"
+    t_start: 0.0
+    t_end: 5.0
+    y_start: 0.0
+    y_end: 10.0
+
+# Quadratic curve peaking at t=5
+angular_z:
+  num_intervals: 1
+  interval_0:
+    type: "quadratic"
+    t_start: 0.0
+    t_end: 10.0
+    y_start: 0.0
+    y_end: 0.0
+    t_mid: 5.0
+    y_mid: 3.14
+```
+
+## Error Handling
+
+- Throws `std::runtime_error` if overlapping intervals are detected for the same object
+- Logs warning for unknown interval types, defaults to CONSTANT
+- Returns 0.0 for non-existent objects (graceful degradation)

doc/architecture/managers/ConfigManager.md

@@ -84,7 +84,7 @@ std::vector<std::string> default_config_paths_ = {
 #### Default Values and Fallbacks
 - **host**: `"localhost"` - Local database server
 - **port**: `5432` - Standard PostgreSQL port
-- **dbname**: `"grades"` - Application-specific database
+- **dbname**: `"imu_data"` - Application-specific database
 - **user**: `"postgres"` - Default PostgreSQL user
 - **password**: `"postgres"` - Default PostgreSQL password
 - **timeout**: `30` seconds - connection timeout
@@ -121,7 +121,7 @@ Example complete TOML configuration:
 [database]
 host = "localhost"
 port = 5432
-dbname = "grades"
+dbname = "imu_data"
 user = "postgres"
 password = "postgres"
 timeout = 30

doc/architecture/managers/DatabaseManager.md

@@ -1,9 +1,9 @@
-# DatabaseManager (`assignments::one::DatabaseManager`)
+# DatabaseManager (`assignments::two::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.
+The `DatabaseManager` class is a PostgreSQL database interface for the ROS2 IMU data collection system.
+It handles all database operations including connection management, table creation and data storage.
 
 ## Implementation Details
 
@@ -28,7 +28,7 @@ DatabaseManager(rclcpp::Logger logger)
 > 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"`
+- Connection string format: `"host=localhost port=5432 dbname=imu_data user=postgres password=postgres"`
 
 **`bool is_connected() const`**
 >  Returns `true` if connection exists and is open
@@ -47,56 +47,25 @@ DatabaseManager(rclcpp::Logger logger)
 **`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
+  - `imu_data`: IMU sensor readings with linear acceleration and angular velocity data
 - Uses transactions for atomic table creation
 
-**`void insert_sample_data()`**
-- Inserts predefined sample student data
-
 ### Data Operations
 
-#### Student Course Management
+#### IMU Data Storage
 
-**`std::vector<StudentCourse> queue_pending_combinations()`**
-> Returns vector of StudentCourse objects for processing queue
+**`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)`**
+> Returns `true` on successful storage, `false` on failure
 
-- 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
+- Stores IMU sensor readings 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
+  - `linear_accel_x`: Linear acceleration on X-axis
+  - `linear_accel_y`: Linear acceleration on Y-axis
+  - `linear_accel_z`: Linear acceleration on Z-axis
+  - `angular_vel_x`: Angular velocity around X-axis
+  - `angular_vel_y`: Angular velocity around Y-axis
+  - `angular_vel_z`: Angular velocity around Z-axis
+- Automatically adds timestamp on insertion
 
 ### Logging
 
@@ -117,9 +86,9 @@ 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);
+    bool success = db_manager.store_imu_data(1.2, -0.5, 9.8, 0.01, 0.02, 0.03);
     if (success) {
-        RCLCPP_INFO(logger, "Exam result stored successfully");
+        RCLCPP_INFO(logger, "IMU data stored successfully");
     }
 }
 ```

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/IMUDataSimulator.md

@@ -0,0 +1,64 @@
+# IMUDataSimulator (`assignments::three::data_simulator_node`)
+
+The `IMUDataSimulator` node generates simulated IMU sensor data (`sensor_msgs/msg/Imu`) based on configurable time-varying intervals. It publishes linear acceleration and angular velocity values that can follow constant, linear, or quadratic trajectories over time.
+
+## Implementation Details
+
+**Parameters**
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `publish_rate` | double | 10.0 | Publishing frequency in Hz |
+| `max_intervals` | int | 4 | Maximum number of intervals per axis |
+| `<axis>.num_intervals` | int | 0 | Number of intervals for each axis |
+| `<axis>.interval_<n>.*` | various | - | Interval configuration (see Simulator) |
+
+**Axes Configured:**
+- `linear_x`, `linear_y`, `linear_z` - Linear acceleration axes
+- `angular_x`, `angular_y`, `angular_z` - Angular velocity axes
+
+**Constructor**
+```cpp
+DataSimulator()
+```
+- Initializes ROS2 node with name `imu_data_simulator`
+- Creates `Simulator` instance for value generation
+- Creates publisher for `simulated_imu_data` topic
+- Sets up timer for periodic publishing
+
+## Core Functionality
+
+**`void publish_imu_data()`**
+- Timer callback invoked at the configured publish rate
+- Calculates elapsed time since node start
+- Queries `Simulator` for current values of all 6 axes
+- Populates IMU message with:
+  - Header timestamp and frame_id (`imu_link`)
+  - Linear acceleration (x, y, z)
+  - Angular velocity (x, y, z)
+- Publishes message to topic
+
+## ROS2 Interface
+
+**Publications**
+- `simulated_imu_data` (sensor_msgs/msg/Imu)
+  - Publishes simulated IMU data at configured rate
+  - Frame ID: `imu_link`
+
+## Usage Example
+
+```bash
+ros2 run g2_2025_odometry_pkg imu_data_simulator_node --ros-args \
+  -p publish_rate:=20.0 \
+  -p linear_x.num_intervals:=1 \
+  -p linear_x.interval_0.type:=constant \
+  -p linear_x.interval_0.t_start:=0.0 \
+  -p linear_x.interval_0.t_end:=10.0 \
+  -p linear_x.interval_0.y_start:=9.81
+```
+
+## Dependencies
+
+- `rclcpp` - ROS2 C++ client library
+- `sensor_msgs` - Standard sensor message types
+- `Simulator` - Internal simulation engine

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)

doc/architecture/nodes/LifecycleManager.md

@@ -0,0 +1,215 @@
+# LifecycleManager (`assignments::two::g2_2025_lifecycle_node`)
+
+## Overview
+The `LifecycleManager` is the core lifecycle-aware node responsible for managing the IMU reader system's operational states and hardware communication. It orchestrates transitions between configuration, activation, and deactivation phases, abstracting the complexity of dual communication backends (serial and MQTT) into a unified interface.
+
+#### Implementation Details
+
+**Parameters**
+
+- **`device_path`** (string, default: "/dev/ttyUSB0"): Serial device path for hardware connection (e.g., USB serial adapter).
+- **`baudrate`** (int, default: 115200): Serial communication baud rate in bits per second.
+- **`comm_t`** (string, default: "serial"): Communication type selector—either "serial" or "mqtt" to determine which backend to use.
+
+**Constructor**
+```cpp
+LifecycleManager()
+```
+- Initializes ROS2 lifecycle node with name `lifecycle_manager`
+- Declares and reads configuration parameters: `device_path`, `baudrate`, and `comm_t`
+- Creates a shared instance of `HardwareInterface` for managing all hardware operations
+- Logs initialization status and readiness
+
+**Core Functions**
+
+**`CallbackReturn on_configure(const State&)`**
+- Enters the *Unconfigured* → *Inactive* transition
+- Checks the `comm_t` parameter to route initialization:
+  - **MQTT mode**: Calls `hw_interface->mqtt_configure()` to set up the MQTT client and broker connection
+  - **Serial mode**: Calls `hw_interface->open_device(device_path_, baudrate_)` to open and configure the serial port
+- Returns `SUCCESS` if device initialization succeeds, `FAILURE` if serial/MQTT setup fails
+- Logs configuration status and any errors
+- Example test code (currently commented) demonstrates direct JSON parsing for validation
+
+**`CallbackReturn on_activate(const State&)`**
+- Enters the *Inactive* → *Active* transition
+- Checks the `comm_t` parameter to start the appropriate reader:
+  - **MQTT mode**: Calls `hw_interface->mqtt_reader()` to attach MQTT callbacks and begin receiving messages
+  - **Serial mode**: Calls `hw_interface->start_read()` to spawn a background thread that continuously reads from the serial device
+- Returns `SUCCESS` after reader startup
+- Logs activation status and selected communication type
+
+**`CallbackReturn on_deactivate(const State&)`**
+- Enters the *Active* → *Inactive* transition
+- Checks the `comm_t` parameter to cleanly stop operations:
+  - **MQTT mode**: Calls `hw_interface->close_mqtt_conn()` to disconnect from the broker and clean up resources
+  - **Serial mode**:
+    - Verifies device state with `hw_interface->is_device_open()`
+    - Calls `hw_interface->stop_read()` to signal the reader thread to exit and joins it
+    - Calls `hw_interface->close_device()` to release the serial port
+- Returns `SUCCESS` after cleanup completes
+- Logs deactivation and resource release
+
+**`CallbackReturn on_shutdown(const State&)`**
+- Enters the *Inactive* → *Finalized* transition
+- Performs final shutdown logging
+- Returns `SUCCESS`
+
+**`CallbackReturn on_cleanup(const State&)`**
+- Called during error recovery or explicit cleanup commands
+- Performs resource cleanup and state logging
+- Returns `SUCCESS`
+
+
+## Communication Architecture
+
+### Dual Backend Support
+
+The `LifecycleManager` provides a flexible, pluggable communication architecture via the `comm_t` parameter:
+
+#### Serial Communication Path
+1. **Configuration Phase** (`on_configure`):
+   - Opens the serial device at the path specified by `device_path` and baudrate
+   - Validates device readiness
+
+2. **Activation Phase** (`on_activate`):
+   - Spawns a background reader thread via `hw_interface->start_read()`
+   - Thread continuously polls the serial device with a timeout
+   - Reads are accumulated in a partial buffer, split on newline, and parsed as JSON
+   - Each valid JSON IMU payload is parsed into a `sensor_msgs::msg::Imu` and published to the ROS topic `imu/data`
+
+3. **Deactivation Phase** (`on_deactivate`):
+   - Signals the reader thread to stop via atomic flag
+   - Joins the thread to ensure clean termination
+   - Closes the serial device
+
+#### MQTT Communication Path
+1. **Configuration Phase** (`on_configure`):
+   - Creates a persistent MQTT async client pointing to the broker at `SERVER_ADDRESS` (default: `tcp://localhost:1883`)
+   - Initializes MQTT callback infrastructure
+
+2. **Activation Phase** (`on_activate`):
+   - Attaches MQTT callbacks to the client
+   - Subscribes to the topic specified by `TOPIC` (default: `esp32/imu`)
+   - The async client runs background threads to receive messages
+
+3. **Deactivation Phase** (`on_deactivate`):
+   - Disconnects from the broker
+   - Cleans up MQTT client and callback resources
+
+## Lifecycle Commands
+
+To interact with the `LifecycleManager` from the command line, use the following ROS2 lifecycle service calls:
+
+```bash
+# List current lifecycle state
+ros2 lifecycle list /LifecycleManager
+
+# Transition: UNCONFIGURED -> INACTIVE
+ros2 lifecycle set /LifecycleManager configure
+
+# Transition: INACTIVE -> UNCONFIGURED
+ros2 lifecycle set /LifecycleManager cleanup
+
+# Transition: INACTIVE -> ACTIVE
+ros2 lifecycle set /LifecycleManager activate
+
+# Transition: ACTIVE -> INACTIVE
+ros2 lifecycle set /LifecycleManager deactivate
+
+# Transition: INACTIVE -> FINALIZED
+ros2 lifecycle set /LifecycleManager shutdown
+```
+![img](https://design.ros2.org/img/node_lifecycle/life_cycle_sm.png)
+
+## Data Flow
+
+### Serial Data Flow
+```
+Hardware Device
+      ↓
+Serial Port (/dev/ttyUSB0)
+      ↓
+Background Reader Thread (start_read)
+      ↓
+Partial Buffer Accumulation
+      ↓
+JSON Line Extraction & Sanitization
+      ↓
+parse_data() ← Deserializes JSON to sensor_msgs::msg::Imu
+      ↓
+imu_publisher → ROS2 Topic (`imu_data`)
+```
+
+### MQTT Data Flow
+```
+MQTT Broker (localhost:1883)
+      ↓
+MQTT Async Client (Background Thread)
+      ↓
+Subscription to Topic (esp32/imu)
+      ↓
+MQTT Callback Handler
+      ↓
+parse_data() ← Deserializes JSON to sensor_msgs::msg::Imu
+      ↓
+imu_publisher → ROS2 Topic (`imu_data`)
+```
+
+## Error Handling
+
+- **Serial Device Failures**: If `open_device()` fails during configuration, `on_configure()` returns `FAILURE` and the system remains in the `UNCONFIGURED` state
+- **Communication Errors**: JSON parse errors from invalid payloads are caught and logged without crashing the node; the reader continues listening for the next message
+- **Thread Safety**: The reader thread uses an atomic flag (`reading_`) for clean stop signaling and ensures all resources are properly joined before returning from `on_deactivate()`
+
+## Design Patterns
+
+1. **Strategy Pattern**: The `comm_t` parameter enables runtime selection of communication backend without changing node code
+2. **Lifecycle Pattern**: Follows ROS2 managed node pattern for predictable initialization, startup, and shutdown sequences
+3. **Thread Safety**: Atomic flags and resource cleanup ensure the reader thread can be safely started and stopped
+4. **Buffer Accumulation**: Partial message buffering handles fragmented serial reads and ensures complete JSON objects are parsed
+
+## Integration with HardwareInterface
+
+The `LifecycleManager` delegates all hardware operations to the `HardwareInterface` class:
+
+| Operation | Method | Lifecycle Phase |
+|-----------|--------|-----------------|
+| Open serial device | `open_device(path, baud)` | on_configure |
+| Start reading | `start_read()` | on_activate |
+| Stop reading | `stop_read()` | on_deactivate |
+| Close serial device | `close_device()` | on_deactivate |
+| Configure MQTT | `mqtt_configure()` | on_configure |
+| Start MQTT reading | `mqtt_reader()` | on_activate |
+| Close MQTT connection | `close_mqtt_conn()` | on_deactivate |
+| Parse JSON payload | `parse_data(json_string)` | on_activate (continuous) |
+| Publish IMU message | `publish_imu_data(imu_msg)` | on_activate (continuous) |
+
+## Usage Example
+
+```bash
+# Launch the node with serial communication at /dev/ttyUSB0, 115200 baud
+ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \
+  --ros-args \
+  -p device_path:=/dev/ttyUSB0 \
+  -p baudrate:=115200 \
+  -p comm_t:=serial
+
+# Alternatively, launch with MQTT communication
+ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \
+  --ros-args \
+  -p comm_t:=mqtt
+
+# In another terminal, configure and activate the lifecycle
+ros2 lifecycle set /LifecycleManager configure
+ros2 lifecycle set /LifecycleManager activate
+
+# Subscribe to published IMU data
+ros2 topic echo /imu_data
+
+# Deactivate and shutdown
+ros2 lifecycle set /LifecycleManager deactivate
+ros2 lifecycle set /LifecycleManager shutdown
+```
+
+---

doc/architecture/nodes/WheelDataSimulator.md

@@ -0,0 +1,64 @@
+# WheelDataSimulator (`assignments::three::wheel_data_simulator_node`)
+
+The `WheelDataSimulator` node generates simulated wheel encoder/velocity data (`std_msgs/msg/Float64MultiArray`) based on configurable time-varying intervals. It publishes wheel values for a 4-wheel robot configuration that can follow constant, linear, or quadratic trajectories over time.
+
+## Implementation Details
+
+**Parameters**
+
+| Parameter | Type | Default | Description |
+|-----------|------|---------|-------------|
+| `publish_rate` | double | 10.0 | Publishing frequency in Hz |
+| `max_intervals` | int | 4 | Maximum number of intervals per wheel |
+| `<wheel>.num_intervals` | int | 0 | Number of intervals for each wheel |
+| `<wheel>.interval_<n>.*` | various | - | Interval configuration (see Simulator) |
+
+**Wheels Configured:**
+- `wheel_fl` - Front Left wheel
+- `wheel_fr` - Front Right wheel
+- `wheel_rl` - Rear Left wheel
+- `wheel_rr` - Rear Right wheel
+
+**Constructor**
+```cpp
+DataSimulator()
+```
+- Initializes ROS2 node with name `wheel_data_simulator`
+- Creates `Simulator` instance for value generation
+- Creates publisher for `simulated_wheel_data` topic
+- Sets up timer for periodic publishing
+
+## Core Functionality
+
+**`void publish_wheel_data()`**
+- Timer callback invoked at the configured publish rate
+- Calculates elapsed time since node start
+- Queries `Simulator` for current values of all 4 wheels
+- Populates Float64MultiArray message with wheel values in order: [FL, FR, RL, RR]
+- Publishes message to topic
+
+## ROS2 Interface
+
+**Publications**
+- `simulated_wheel_data` (std_msgs/msg/Float64MultiArray)
+  - Publishes simulated wheel data at configured rate
+  - Array contains 4 values: [front_left, front_right, rear_left, rear_right]
+
+## Usage Example
+
+```bash
+ros2 run g2_2025_odometry_pkg wheel_data_simulator_node --ros-args \
+  -p publish_rate:=50.0 \
+  -p wheel_fl.num_intervals:=1 \
+  -p wheel_fl.interval_0.type:=linear \
+  -p wheel_fl.interval_0.t_start:=0.0 \
+  -p wheel_fl.interval_0.t_end:=10.0 \
+  -p wheel_fl.interval_0.y_start:=0.0 \
+  -p wheel_fl.interval_0.y_end:=5.0
+```
+
+## Dependencies
+
+- `rclcpp` - ROS2 C++ client library
+- `std_msgs` - Standard message types
+- `Simulator` - Internal simulation engine

doc/installation/installation.md

@@ -10,6 +10,22 @@
 - Colcon build tool
 - Docker compose
 
+### Paho MQTT library
+
+For this project the Paho MQTT library is needed, which can be built with the following commands:
+
+```bash
+git clone https://github.com/eclipse/paho.mqtt.cpp
+cd paho.mqtt.cpp
+git co v1.5.4
+
+git submodule init
+git submodule update
+
+cmake -Bbuild -H. -DPAHO_WITH_MQTT_C=ON -DPAHO_BUILD_EXAMPLES=ON
+sudo cmake --build build/ --target install
+```
+
 ### Clone the Repository
 
 ```bash
@@ -36,8 +52,24 @@ 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
+### Start the IMU Reader program
 ```bash
-ros2 launch g2_2025_imu_reader_pkg imu_reader.launch.xml
+# For Serial:
+ros2 launch g2_2025_imu_reader_pkg serial.launch.xml
+
+# For MQTT:
+ros2 launch g2_2025_imu_reader_pkg mqtt.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
+
+### Use the Lifecycle Node
+
+To setup the lifecycle node the following commands can be used. They must be used in this order.
+```bash
+ros2 lifecycle set /LifecycleManager configure
+ros2 lifecycle set /LifecycleManager cleanup
+ros2 lifecycle set /LifecycleManager activate
+ros2 lifecycle set /LifecycleManager deactivate
+ros2 lifecycle set /LifecycleManager shutdown
+```
+![img](https://design.ros2.org/img/node_lifecycle/life_cycle_sm.png)

doc/tests/ConfigManager.md

@@ -61,7 +61,7 @@ Unit tests for `ConfigManager` are implemented in `src/g2_2025_imu_reader_pkg/te
   [database]
   host = "localhost"
   port = 5432
-  dbname = "grades"
+  dbname = "imu_data"
   user = "postgres"
   password = "postgres"
   ```

doc/tests/DatabaseManager.md

@@ -18,16 +18,16 @@ Unit tests for `DatabaseManager` are implemented in `src/g2_2025_imu_reader_pkg/
 - **Test Action:** Call `is_connected()` method
 - **Expected Result:** Returns either `true` or `false` (no crashes or invalid states)
 
-### 3. QueuePendingCombinationsTest
+### 3. StoreIMUDataWhenNotConnected
 
-**Description:** Verifies retrieval of pending student-course combinations that need exam results.
+**Description:** Verifies that storing IMU data without an active database connection fails gracefully.
 
-- 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.
+- **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.
+**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.
+- **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/IMUDataSimulator.md

@@ -0,0 +1,67 @@
+# IMU Data Simulator Unit Tests
+
+Unit tests for the `DataSimulator` (IMU) node are implemented in `src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate node initialization, message publishing, and data correctness.
+
+## Test Cases
+
+### 1. NodeInitialization
+
+**Description:** Verifies that the DataSimulator node can be created without errors.
+
+- **Test Action:** Create DataSimulator instance
+- **Expected Result:** No exceptions thrown during construction
+
+### 2. MessagePublishing
+
+**Description:** Tests that IMU messages are published on the correct topic.
+
+- **Test Action:**
+  - Create subscription to `simulated_imu_data` topic
+  - Create DataSimulator node
+  - Spin both nodes for a short period
+- **Expected Result:** At least one `sensor_msgs/msg/Imu` message is received
+
+### 3. MessageFrameId
+
+**Description:** Verifies that published IMU messages have the correct frame_id.
+
+- **Test Action:**
+  - Subscribe to `simulated_imu_data`
+  - Receive a message
+- **Expected Result:** `header.frame_id` equals `"imu_link"`
+
+### 4. LinearAccelerationValues
+
+**Description:** Tests that linear acceleration values are valid (finite numbers).
+
+- **Test Action:**
+  - Subscribe and receive IMU message
+  - Check linear_acceleration fields
+- **Expected Result:**
+  - `linear_acceleration.x` is finite
+  - `linear_acceleration.y` is finite
+  - `linear_acceleration.z` is finite
+
+### 5. AngularVelocityValues
+
+**Description:** Tests that angular velocity values are valid (finite numbers).
+
+- **Test Action:**
+  - Subscribe and receive IMU message
+  - Check angular_velocity fields
+- **Expected Result:**
+  - `angular_velocity.x` is finite
+  - `angular_velocity.y` is finite
+  - `angular_velocity.z` is finite
+
+## Test Infrastructure
+
+The test class `IMUSimulatorTest` provides:
+- Static `SetUpTestSuite()` and `TearDownTestSuite()` for ROS2 init/shutdown
+- Helper method `setupBasicIMUParams()` for setting up default test parameters
+
+## ROS2 Topics Used
+
+| Topic | Message Type | Direction |
+|-------|--------------|-----------|
+| `simulated_imu_data` | sensor_msgs/msg/Imu | Published by node under test |

doc/tests/LifecycleManager.md

@@ -0,0 +1,229 @@
+# LifecycleManager Unit Tests
+
+Unit tests for `LifecycleManager` are implemented in `src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp` using Google Test and ROS2 lifecycle test utilities. The tests are designed to validate all lifecycle state transitions, parameter handling, and hardware integration without requiring actual hardware or MQTT broker connectivity.
+
+## Test Cases
+
+### 1. ConstructorTest
+
+**Description:** Verifies that LifecycleManager can be instantiated without crashing and parameters are correctly declared.
+
+- **Test Action:** Create LifecycleManager instance with default parameters
+- **Expected Result:** Instance created successfully; parameters `device_path`, `baudrate`, and `comm_t` are registered with their defaults
+
+---
+
+### 2. ParameterDeclarationTest
+
+**Description:** Tests that all required parameters are declared during construction with correct default values.
+
+- **Test Action:**
+  - Create LifecycleManager
+  - Query parameters: `device_path`, `baudrate`, `comm_t`
+- **Expected Result:**
+  - `device_path` defaults to `"/dev/ttyUSB0"`
+  - `baudrate` defaults to `115200`
+  - `comm_t` defaults to `"serial"`
+
+---
+
+### 3. ParameterRuntimeReadTest
+
+**Description:** Verifies parameters can be read at runtime and affect behavior.
+
+- **Test Action:**
+  - Set parameters via ROS2 parameter API: `device_path="/dev/ttyACM0"`, `comm_t="mqtt"`
+  - Verify LifecycleManager reads the updated values
+- **Expected Result:** Parameters are correctly read and stored in member variables
+
+---
+
+### 4. InitialStateTest
+
+**Description:** Tests that the node starts in the `UNCONFIGURED` state.
+
+- **Test Action:** Create LifecycleManager and check lifecycle state
+- **Expected Result:** Node is in `UNCONFIGURED` state
+
+---
+
+### 5. ConfigureSerialModeTest
+
+**Description:** Tests the `on_configure` callback in serial communication mode.
+
+- **Test Action:**
+  - Set `comm_t` parameter to `"serial"`
+  - Call `on_configure()` with valid device path (e.g., `/dev/null` for testing)
+- **Expected Result:**
+  - Transition succeeds
+  - Node moves to `INACTIVE` state
+  - `HardwareInterface::open_device()` is called with correct parameters
+
+---
+
+### 6. ConfigureMQTTModeTest
+
+**Description:** Tests the `on_configure` callback in MQTT communication mode.
+
+- **Test Action:**
+  - Set `comm_t` parameter to `"mqtt"`
+  - Call `on_configure()`
+- **Expected Result:**
+  - Transition succeeds
+  - Node moves to `INACTIVE` state
+  - `HardwareInterface::mqtt_configure()` is called
+
+---
+
+### 7. ConfigureSerialFailureTest
+
+**Description:** Tests graceful failure when serial device cannot be opened.
+
+- **Test Action:**
+  - Set `device_path` to non-existent path (e.g., `/dev/invalid_device`)
+  - Set `comm_t` to `"serial"`
+  - Call `on_configure()`
+- **Expected Result:**
+  - Transition handled gracefully (node doesn't crash)
+  - Error logging occurs
+
+---
+
+### 8. DeactivateSerialModeTest
+
+**Description:** Tests the `on_deactivate` callback in serial communication mode.
+
+- **Test Action:**
+  - Configure to `INACTIVE` state in serial mode
+  - Call `on_deactivate()`
+- **Expected Result:**
+  - Transition handled gracefully
+  - Resources are properly released
+
+---
+
+### 9. ResourceCleanupTest
+
+**Description:** Tests that all resources are properly cleaned up on deactivation and shutdown.
+
+- **Test Action:**
+  - Configure node with serial settings
+  - Let node go out of scope
+  - Verify no crashes or resource leaks
+- **Expected Result:**
+  - No memory leaks
+  - File descriptors are closed
+  - No segmentation faults on cleanup
+
+---
+
+### 10. ErrorLoggingTest
+
+**Description:** Tests that error handling works during configuration attempts.
+
+- **Test Action:**
+  - Set invalid device path and attempt configuration
+  - Verify error is handled gracefully
+- **Expected Result:**
+  - Node doesn't crash on error
+  - Error logging occurs
+
+---
+
+### 11. HardwareInterfaceIntegrationTest
+
+**Description:** Tests the complete integration between LifecycleManager and HardwareInterface.
+
+- **Test Action:**
+  - Create LifecycleManager instance
+  - Verify HardwareInterface instance is created and accessible
+  - Verify we can call HardwareInterface methods
+- **Expected Result:**
+  - HardwareInterface is properly initialized
+  - No segmentation faults when accessing interface methods
+
+---
+
+### 12. DevicePathParameterTest
+
+**Description:** Tests that device_path parameter correctly controls serial device selection.
+
+- **Test Action:**
+  - Set `device_path` to `/dev/null`
+  - Call `on_configure()` in serial mode
+  - Verify correct device path is used
+- **Expected Result:**
+  - Configuration succeeds with correct device path
+
+---
+
+### 13. BaudRateParameterTest
+
+**Description:** Tests that baudrate parameter is correctly configured.
+
+- **Test Action:**
+  - Set `baudrate` to valid value (e.g., 115200)
+  - Call `on_configure()` in serial mode
+- **Expected Result:**
+  - Correct baud rate is passed to the hardware interface
+
+---
+
+### 14. ParameterUpdateBehaviorTest
+
+**Description:** Tests that parameter changes are respected across state transitions.
+
+- **Test Action:**
+  - Set `comm_t` to `"serial"`, configure
+  - Deactivate and transition back to UNCONFIGURED
+  - Change `comm_t` to `"mqtt"`
+  - Re-configure with new communication mode
+- **Expected Result:**
+  - Communication mode switches work correctly
+  - Parameter changes are respected
+
+---
+
+## Test Organization
+
+Tests are organized into logical groups:
+
+1. **Construction & Initialization** (Tests 1-3): Basic object creation and parameter setup
+2. **State Transitions & Configuration** (Tests 4-7): Lifecycle callbacks and state validation
+3. **Parameter Validation** (Tests 12-13): Parameter binding and influence on behavior
+4. **Complete Sequences** (Test 14): Parameter switching across state transitions
+5. **Resource & Thread Safety** (Tests 8-9): Resource cleanup and safe deactivation
+6. **Error Handling & Integration** (Tests 10-11): Error resilience and component integration
+
+---
+
+## Test Execution
+
+### Run All Tests
+
+```bash
+# From workspace root
+colcon test --packages-select g2_2025_imu_reader_pkg
+```
+
+### Run Specific Test Suite
+
+```bash
+# Run only LifecycleManager tests
+colcon test --packages-select g2_2025_imu_reader_pkg --ctest-args -R "LifecycleManager"
+```
+
+### Run with Verbose Output
+
+```bash
+colcon test --packages-select g2_2025_imu_reader_pkg --ctest-args --verbose
+```
+
+### Run Tests Directly
+
+```bash
+# From workspace root
+./build/g2_2025_imu_reader_pkg/g2_2025_imu_reader_pkg_test_lifecycle_manager
+```
+
+---

doc/tests/Simulator.md

@@ -0,0 +1,98 @@
+# Simulator Unit Tests
+
+Unit tests for the `Simulator` class are implemented in `src/g2_2025_odometry_pkg/test/test_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate interval-based value generation with different interpolation types.
+
+## Test Cases
+
+### 1. ConstantInterval
+
+**Description:** Verifies constant interpolation returns the same value throughout the interval.
+
+- **Test Configuration:**
+  - Single interval: t=[0, 10], y_start=5.0
+  - Type: constant
+- **Expected Result:**
+  - Value is 5.0 at t=0, t=5, t=10
+  - Value holds at 5.0 after interval (t=15)
+
+### 2. LinearInterval
+
+**Description:** Tests linear interpolation between start and end values.
+
+- **Test Configuration:**
+  - Single interval: t=[0, 10], y_start=0.0, y_end=10.0
+  - Type: linear
+- **Expected Result:**
+  - Value is 0.0 at t=0
+  - Value is 5.0 at t=5
+  - Value is 10.0 at t=10
+  - Value holds at 10.0 after interval (t=15)
+
+### 3. QuadraticInterval
+
+**Description:** Tests quadratic (Lagrange) interpolation through 3 points.
+
+- **Test Configuration:**
+  - Single interval: t=[0, 10], y_start=0.0, y_end=0.0, t_mid=5.0, y_mid=10.0
+  - Type: quadratic
+- **Expected Result:**
+  - Value is 0.0 at t=0
+  - Value is 10.0 at t=5 (peak)
+  - Value is 0.0 at t=10
+  - Value holds at 0.0 after interval (t=15)
+
+### 4. MultipleIntervals
+
+**Description:** Tests behavior with multiple non-overlapping intervals and gaps.
+
+- **Test Configuration:**
+  - Interval 0: t=[0, 5], constant 5.0
+  - Interval 1: t=[10, 15], constant 10.0
+  - Gap between t=5 and t=10
+- **Expected Result:**
+  - Value is 5.0 at t=2.5 (first interval)
+  - Value holds at 5.0 in gap (t=7.5)
+  - Value is 10.0 at t=12.5 (second interval)
+  - Value holds at 10.0 after all intervals (t=20)
+
+### 5. ValueBeforeIntervals
+
+**Description:** Tests behavior when querying time before any interval starts.
+
+- **Test Configuration:**
+  - Single interval: t=[10, 20]
+- **Expected Result:**
+  - Value is 0.0 at t=5 (before interval starts)
+
+### 6. NonExistentChannel
+
+**Description:** Tests graceful handling of queries for unconfigured channels.
+
+- **Test Action:** Query `get_object_value("nonexistent_channel", 5.0)`
+- **Expected Result:** Returns 0.0
+
+### 7. OverlappingIntervalsThrowsException
+
+**Description:** Verifies that overlapping intervals are detected and rejected.
+
+- **Test Configuration:**
+  - Interval 0: t=[0, 10]
+  - Interval 1: t=[5, 15] (overlaps)
+- **Expected Result:** Constructor throws `std::runtime_error`
+
+### 8. MaxIntervalsLimit
+
+**Description:** Tests that `max_intervals` parameter limits loaded intervals.
+
+- **Test Configuration:**
+  - max_intervals=2
+  - num_intervals=3 (defines 3 intervals)
+- **Expected Result:**
+  - Only first 2 intervals are loaded
+  - Third interval values default to holding second interval's end value
+
+## Test Infrastructure
+
+The test class `SimulatorTest` provides:
+- Static `SetUpTestSuite()` and `TearDownTestSuite()` for ROS2 init/shutdown
+- Helper method `createNodeWithParams()` for creating nodes with parameter overrides

doc/tests/WheelDataSimulator.md

@@ -0,0 +1,73 @@
+# Wheel Data Simulator Unit Tests
+
+Unit tests for the `DataSimulator` (Wheel) node are implemented in `src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate node initialization, message publishing, array structure, and data correctness.
+
+## Test Cases
+
+### 1. NodeInitialization
+
+**Description:** Verifies that the Wheel DataSimulator node can be created without errors.
+
+- **Test Action:** Create DataSimulator instance
+- **Expected Result:** No exceptions thrown during construction
+
+### 2. WheelDataPublishing
+
+**Description:** Tests that wheel data messages are published on the correct topic.
+
+- **Test Action:**
+  - Create subscription to `simulated_wheel_data` topic
+  - Create DataSimulator node
+  - Spin both nodes for a short period
+- **Expected Result:** At least one `std_msgs/msg/Float64MultiArray` message is received
+
+### 3. WheelDataArraySize
+
+**Description:** Verifies that the published array contains the correct number of wheel values.
+
+- **Test Action:**
+  - Subscribe to `simulated_wheel_data`
+  - Receive a message
+  - Check array size
+- **Expected Result:** `data.size()` equals 4 (FL, FR, RL, RR)
+
+### 4. ValidVelocityValues
+
+**Description:** Tests that all wheel velocity values are valid (finite numbers).
+
+- **Test Action:**
+  - Subscribe and receive wheel data message
+  - Check each value in the array
+- **Expected Result:** All 4 values are finite numbers
+
+### 5. MultipleMessagesReceived
+
+**Description:** Tests that multiple messages are published over time.
+
+- **Test Action:**
+  - Subscribe and count received messages
+  - Spin for a short duration
+- **Expected Result:** Message count is greater than 0
+
+## Test Infrastructure
+
+The test class `WheelSimulatorTest` provides:
+- Static `SetUpTestSuite()` and `TearDownTestSuite()` for ROS2 init/shutdown
+- Helper method `setupBasicWheelParams()` for setting up default test parameters
+
+## ROS2 Topics Used
+
+| Topic | Message Type | Direction |
+|-------|--------------|-----------|
+| `simulated_wheel_data` | std_msgs/msg/Float64MultiArray | Published by node under test |
+
+## Data Array Format
+
+The published Float64MultiArray contains wheel values in the following order:
+
+| Index | Wheel |
+|-------|-------|
+| 0 | Front Left (FL) |
+| 1 | Front Right (FR) |
+| 2 | Rear Left (RL) |
+| 3 | Rear Right (RR) |

docker-compose.yml

@@ -7,7 +7,7 @@ services:
         environment:
             - POSTGRES_PASSWORD=postgres
             - POSTGRES_USER=postgres
-            - POSTGRES_DB=grades
+            - POSTGRES_DB=imu_data
         ports:
             - "5432:5432"
     mosquitto:

src/IMU/main/mpu6886.c

@@ -291,6 +291,9 @@ void app_main(void)
     }
 
     init_button();
+    gpio_reset_pin(LED_GPIO);
+    gpio_set_direction(LED_GPIO, GPIO_MODE_OUTPUT);
+
     mpu6886_t mpu;
     mpu.i2c_port = I2C_NUM_0;
     mpu.address = MPU6886_ADDR;
@@ -311,11 +314,13 @@ void app_main(void)
             wifi_init();
             mqtt_app_start();
             ESP_LOGI("BOOT", "Boot button pressed: starting MQTT mode");
+            gpio_set_level(LED_GPIO, 1);
             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");
+            gpio_set_level(LED_GPIO, 0);
             toggle_completed = true;
         }
         mpu6886_read_accel(&mpu, &accel);

src/IMU/main/mpu6886.h

@@ -26,6 +26,7 @@
 #define I2C_MASTER_TIMEOUT_MS       1000
 
 #define BOOT_BUTTON_GPIO 0 // GPIO number for boot mode selection button
+#define LED_GPIO 33         // GPIO number for onboard LED
 
 #define MPU6886_ADDR 0x68
 
@@ -71,7 +72,7 @@ typedef struct {
     vec3_t accel_offset;
 } mpu6886_t;
 
-bool mqtt_toggle = false;
+bool mqtt_toggle = true;
 bool toggle_completed = false;
 bool wifi_initialized = false;
 

src/IMU/sdkconfig

@@ -448,8 +448,6 @@ CONFIG_I2C_MASTER_SDA=21
 CONFIG_I2C_MASTER_FREQUENCY=100000
 # end of I2C Master Configuration
 
-CONFIG_ENV_MQTT_ENABLED=y
-
 #
 # MQTT Configuration
 #

src/config.toml

@@ -1,7 +1,7 @@
 [database]
 host = "localhost"
 port = 5432
-dbname = "grades"
+dbname = "imu_data"
 user = "postgres"
 password = "postgres"
 timeout = 30

src/g2_2025_imu_reader_pkg/CMakeLists.txt

@@ -8,13 +8,21 @@ endif()
 # external packages
 include(FetchContent)
 
-fetchcontent_declare(
+FetchContent_Declare(
     tomlplusplus
     GIT_REPOSITORY https://github.com/marzer/tomlplusplus.git
     GIT_TAG v3.4.0
 )
 
-fetchcontent_makeavailable(tomlplusplus)
+FetchContent_MakeAvailable(tomlplusplus)
+
+FetchContent_Declare(
+    nlohmann_json
+    GIT_REPOSITORY https://github.com/nlohmann/json.git
+    GIT_TAG v3.12.0
+)
+
+FetchContent_MakeAvailable(nlohmann_json)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -22,6 +30,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 
 add_executable(g2_2025_imu_database_writer_node
     src/g2_2025_imu_database_writer_node/Main.cpp
@@ -29,6 +38,7 @@ add_executable(g2_2025_imu_database_writer_node
     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
@@ -36,12 +46,39 @@ target_include_directories(g2_2025_imu_database_writer_node PRIVATE
 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)
 
+add_executable(g2_2025_lifecycle_node
+    src/g2_2025_lifecycle_node/Main.cpp
+    src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp
+    src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp
+    lib/serialib.cpp
+)
+
+target_include_directories(g2_2025_lifecycle_node PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/lib
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node
+)
+ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs)
+
+target_link_libraries(g2_2025_lifecycle_node
+  paho-mqttpp3
+  paho-mqtt3c
+  nlohmann_json::nlohmann_json
+)
+
 install(
     TARGETS
         g2_2025_imu_database_writer_node
+        g2_2025_lifecycle_node
     DESTINATION lib/${PROJECT_NAME}
 )
 
+install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}/
+)
+set_target_properties(g2_2025_lifecycle_node PROPERTIES INSTALL_RPATH "/usr/local/lib")
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 
@@ -95,6 +132,30 @@ if(BUILD_TESTING)
     pqxx pq tomlplusplus::tomlplusplus
   )
 
+  ament_add_gtest(${PROJECT_NAME}_test_lifecycle_manager
+    test/LifecycleManager.test.cpp
+    src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp
+    src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp
+    lib/serialib.cpp
+  )
+  target_include_directories(${PROJECT_NAME}_test_lifecycle_manager PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node
+    ${CMAKE_CURRENT_SOURCE_DIR}/lib
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test_lifecycle_manager
+    rclcpp
+    rclcpp_lifecycle
+    std_msgs
+    sensor_msgs
+  )
+  target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager
+    paho-mqttpp3
+    paho-mqtt3c
+    nlohmann_json::nlohmann_json
+  )
+  set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib")
+
   # Add Python integration tests
   #   find_package(ament_cmake_pytest REQUIRED)
   #   ament_add_pytest_test(${PROJECT_NAME}_integration_test test/test_integration_system.py

src/g2_2025_imu_reader_pkg/launch/mqtt.launch.xml

@@ -0,0 +1,6 @@
+<launch>
+    <node pkg="g2_2025_imu_reader_pkg" exec="g2_2025_imu_database_writer_node"/>
+    <node pkg="g2_2025_imu_reader_pkg" exec="g2_2025_lifecycle_node">
+        <param name="comm_t" value="mqtt"/>
+    </node>
+</launch>

src/g2_2025_imu_reader_pkg/launch/serial.launch.xml

@@ -0,0 +1,8 @@
+<launch>
+    <node pkg="g2_2025_imu_reader_pkg" exec="g2_2025_imu_database_writer_node"/>
+    <node pkg="g2_2025_imu_reader_pkg" exec="g2_2025_lifecycle_node">
+        <param name="device_path" value="/dev/ttyUSB0"/>
+        <param name="baudrate" value="115200"/>
+        <param name="comm_t" value="serial"/>
+    </node>
+</launch>

src/g2_2025_imu_reader_pkg/lib/serialib.h

@@ -0,0 +1,270 @@
+/*!
+\file    serialib.h
+\brief   Header file of the class serialib. This class is used for communication over a serial device.
+\author  Philippe Lucidarme (University of Angers)
+\version 2.0
+\date    december the 27th of 2019
+This Serial library is used to communicate through serial port.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
+INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
+PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE X CONSORTIUM BE LIABLE FOR ANY CLAIM,
+DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+This is a licence-free software, it can be used by anyone who try to build a better world.
+*/
+
+
+#ifndef SERIALIB_H
+#define SERIALIB_H
+
+#if defined(__CYGWIN__)
+    // This is Cygwin special case
+    #include <sys/time.h>
+#endif
+
+// Include for windows
+#if defined (_WIN32) || defined (_WIN64)
+#if defined(__GNUC__)
+    // This is MinGW special case
+    #include <sys/time.h>
+#else
+    // sys/time.h does not exist on "actual" Windows
+    #define NO_POSIX_TIME
+#endif
+    // Accessing to the serial port under Windows
+    #include <windows.h>
+#endif
+
+// Include for Linux
+#if defined (__linux__) || defined(__APPLE__)
+    #include <stdlib.h>
+    #include <sys/types.h>
+    #include <sys/shm.h>
+    #include <termios.h>
+    #include <string.h>
+    #include <iostream>
+    #include <sys/time.h>
+    // File control definitions
+    #include <fcntl.h>
+    #include <unistd.h>
+    #include <sys/ioctl.h>
+#endif
+
+/*! To avoid unused parameters */
+#define UNUSED(x) (void)(x)
+
+/**
+ * number of serial data bits
+ */
+enum SerialDataBits {
+    SERIAL_DATABITS_5, /**< 5 databits */
+    SERIAL_DATABITS_6, /**< 6 databits */
+    SERIAL_DATABITS_7, /**< 7 databits */
+    SERIAL_DATABITS_8,  /**< 8 databits */
+    SERIAL_DATABITS_16,  /**< 16 databits */
+};
+
+/**
+ * number of serial stop bits
+ */
+enum SerialStopBits {
+    SERIAL_STOPBITS_1, /**< 1 stop bit */
+    SERIAL_STOPBITS_1_5, /**< 1.5 stop bits */
+    SERIAL_STOPBITS_2, /**< 2 stop bits */
+};
+
+/**
+ * type of serial parity bits
+ */
+enum SerialParity {
+    SERIAL_PARITY_NONE, /**< no parity bit */
+    SERIAL_PARITY_EVEN, /**< even parity bit */
+    SERIAL_PARITY_ODD, /**< odd parity bit */
+    SERIAL_PARITY_MARK, /**< mark parity */
+    SERIAL_PARITY_SPACE /**< space bit */
+};
+
+/*!  \class     serialib
+     \brief     This class is used for communication over a serial device.
+*/
+class serialib
+{
+public:
+
+    //_____________________________________
+    // ::: Constructors and destructors :::
+
+
+
+    // Constructor of the class
+    serialib    ();
+
+    // Destructor
+    ~serialib   ();
+
+
+
+    //_________________________________________
+    // ::: Configuration and initialization :::
+
+
+    // Open a device
+    char openDevice(const char *Device, const unsigned int Bauds,
+                    SerialDataBits Databits = SERIAL_DATABITS_8,
+                    SerialParity Parity = SERIAL_PARITY_NONE,
+                    SerialStopBits Stopbits = SERIAL_STOPBITS_1);
+
+    // Check device opening state
+    bool isDeviceOpen();
+
+    // Close the current device
+    void    closeDevice();
+
+
+
+
+    //___________________________________________
+    // ::: Read/Write operation on characters :::
+
+
+    // Write a char
+    int     writeChar   (char);
+
+    // Read a char (with timeout)
+    int     readChar    (char *pByte,const unsigned int timeOut_ms=0);
+
+
+
+
+    //________________________________________
+    // ::: Read/Write operation on strings :::
+
+
+    // Write a string
+    int     writeString (const char *String);
+
+    // Read a string (with timeout)
+    int     readString  (   char *receivedString,
+                            char finalChar,
+                            unsigned int maxNbBytes,
+                            const unsigned int timeOut_ms=0);
+
+
+
+    // _____________________________________
+    // ::: Read/Write operation on bytes :::
+
+
+    // Write an array of bytes
+    int     writeBytes(const void *Buffer, const unsigned int NbBytes, unsigned int *NbBytesWritten);
+    int     writeBytes  (const void *Buffer, const unsigned int NbBytes);
+
+    // Read an array of byte (with timeout)
+    int     readBytes   (void *buffer,unsigned int maxNbBytes,const unsigned int timeOut_ms=0, unsigned int sleepDuration_us=100);
+
+
+
+
+    // _________________________
+    // ::: Special operation :::
+
+
+    // Empty the received buffer
+    char    flushReceiver();
+
+    // Return the number of bytes in the received buffer
+    int     available();
+
+
+
+
+    // _________________________
+    // ::: Access to IO bits :::
+
+
+    // Set CTR status (Data Terminal Ready, pin 4)
+    bool    DTR(bool status);
+    bool    setDTR();
+    bool    clearDTR();
+
+    // Set RTS status (Request To Send, pin 7)
+    bool    RTS(bool status);
+    bool    setRTS();
+    bool    clearRTS();
+
+    // Get RI status (Ring Indicator, pin 9)
+    bool    isRI();
+
+    // Get DCD status (Data Carrier Detect, pin 1)
+    bool    isDCD();
+
+    // Get CTS status (Clear To Send, pin 8)
+    bool    isCTS();
+
+    // Get DSR status (Data Set Ready, pin 9)
+    bool    isDSR();
+
+    // Get RTS status (Request To Send, pin 7)
+    bool    isRTS();
+
+    // Get CTR status (Data Terminal Ready, pin 4)
+    bool    isDTR();
+
+
+private:
+    // Read a string (no timeout)
+    int             readStringNoTimeOut  (char *String,char FinalChar,unsigned int MaxNbBytes);
+
+    // Current DTR and RTS state (can't be read on WIndows)
+    bool            currentStateRTS;
+    bool            currentStateDTR;
+
+
+
+
+
+#if defined (_WIN32) || defined( _WIN64)
+    // Handle on serial device
+    HANDLE          hSerial;
+    // For setting serial port timeouts
+    COMMTIMEOUTS    timeouts;
+#endif
+#if defined (__linux__) || defined(__APPLE__)
+    int             fd;
+#endif
+
+};
+
+
+
+/*!  \class     timeOut
+     \brief     This class can manage a timer which is used as a timeout.
+   */
+// Class timeOut
+class timeOut
+{
+public:
+
+    // Constructor
+    timeOut();
+
+    // Init the timer
+    void                initTimer();
+
+    // Return the elapsed time since initialization
+    unsigned long int   elapsedTime_ms();
+
+private:
+#if defined (NO_POSIX_TIME)
+    // Used to store the previous time (for computing timeout)
+    LONGLONG       counterFrequency;
+    LONGLONG       previousTime;
+#else
+    // Used to store the previous time (for computing timeout)
+    struct timeval      previousTime;
+#endif
+};
+
+#endif // serialib_H

src/g2_2025_imu_reader_pkg/src/config/ConfigManager.cpp

@@ -83,7 +83,7 @@ DatabaseConfig ConfigManager::parse_database_config(const toml::table& config) c
 
     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.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);

src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/Main.cpp

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

src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp

@@ -0,0 +1,218 @@
+#include "HardwareInterface.hpp"
+
+namespace assignments::two::g2_2025_lifecycle_node {
+
+HardwareInterface::HardwareInterface(MQTTParameters mqtt_config)
+: Node("HardwareInterface")
+, mqtt_config_(mqtt_config)
+{
+    RCLCPP_INFO(this->get_logger(), "created HardwareInterface node");
+
+    imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("imu_data", 10);
+    RCLCPP_INFO(this->get_logger(), "created IMU Publisher on topic 'imu_data'");
+}
+
+HardwareInterface::~HardwareInterface() {
+    uart_stop_read();
+
+    if (uart_device_is_open()) {
+        uart_close_device();
+    }
+
+    mqtt_close_connection();
+}
+
+void HardwareInterface::parse_and_publish_imu_data(const std::string& data) {
+    try {
+        auto json_data = nlohmann::json::parse(data);
+        auto imu_msg = std::make_shared<sensor_msgs::msg::Imu>();
+
+        imu_msg->header.stamp = this->now();
+        imu_msg->header.frame_id = "imu_link";
+
+        // angular velocity
+        if (json_data.contains("accel") && json_data["accel"].is_object()) {
+            imu_msg->linear_acceleration.x = json_data["accel"].value("x", 0.0);
+            imu_msg->linear_acceleration.y = json_data["accel"].value("y", 0.0);
+            imu_msg->linear_acceleration.z = json_data["accel"].value("z", 0.0);
+        }
+
+        // linear acceleration
+        if (json_data.contains("gyro") && json_data["gyro"].is_object()) {
+            imu_msg->angular_velocity.x = json_data["gyro"].value("x", 0.0);
+            imu_msg->angular_velocity.y = json_data["gyro"].value("y", 0.0);
+            imu_msg->angular_velocity.z = json_data["gyro"].value("z", 0.0);
+        }
+
+        RCLCPP_INFO(this->get_logger(),
+            "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]",
+            imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z,
+            imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z
+        );
+
+        imu_publisher_->publish(*imu_msg);
+    } catch (const nlohmann::json::exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "JSON parsing error: %s", e.what());
+    }
+}
+
+void HardwareInterface::uart_start_read() {
+    if (uart_reading_.load()) {
+        return;
+    }
+
+    uart_reading_.store(true);
+
+    uart_read_thread_ = std::thread([this]() {
+        char buffer[116];
+        RCLCPP_INFO(this->get_logger(), "reader thread started");
+
+        while (uart_reading_.load()) {
+            int ret = serial_.readString(buffer, '\n', sizeof(buffer), 1000);
+            if (ret > 0) {
+                RCLCPP_INFO(this->get_logger(), "Received buffer: %s", buffer);
+                parse_and_publish_imu_data(buffer);
+            }
+        }
+
+        RCLCPP_INFO(this->get_logger(), "reader thread exiting");
+    });
+}
+
+void HardwareInterface::uart_stop_read() {
+    if (!uart_reading_.load()) return;
+    uart_reading_.store(false);
+
+    if (uart_read_thread_.joinable()) {
+        uart_read_thread_.join();
+    }
+}
+
+bool HardwareInterface::uart_open_device(const std::string& device_path, int baud_rate) {
+    char errorOpening = serial_.openDevice(device_path.c_str(), baud_rate);
+
+    if (errorOpening != 1) {
+        RCLCPP_ERROR(this->get_logger(), "Error opening serial_ port: %d", errorOpening);
+        return false;
+    }
+
+    RCLCPP_INFO(this->get_logger(), "serial_ port opened successfully.");
+    return true;
+}
+
+bool HardwareInterface::uart_device_is_open() {
+    return serial_.isDeviceOpen();
+}
+
+void HardwareInterface::uart_close_device() {
+    serial_.closeDevice();
+}
+
+void HardwareInterface::mqtt_message_callback(
+    const std::string& topic,
+    const std::string& payload,
+    HardwareInterface* self
+) {
+    if (self) {
+        RCLCPP_INFO(self->get_logger(), "Message received on topic %s: %s", topic.c_str(), payload.c_str());
+
+        self->parse_and_publish_imu_data(payload);
+    }
+}
+
+void HardwareInterface::mqtt_configure() {
+    try {
+        RCLCPP_INFO(this->get_logger(), "Creating MQTT client with server: %s, client_id: %s",
+            mqtt_config_.server_address.c_str(), mqtt_config_.client_id.c_str()
+        );
+
+        // Create client with basic constructor (no persistence, no create options)
+        mqtt_client_ = std::make_unique<mqtt::client>(
+            mqtt_config_.server_address,
+            mqtt_config_.client_id
+        );
+
+        RCLCPP_INFO(this->get_logger(), "MQTT client created successfully");
+
+        mqtt_connect();
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(this->get_logger(), "Exception in mqtt_configure: %s", e.what());
+        throw;
+    }
+}
+
+void HardwareInterface::mqtt_start_listen() {
+    RCLCPP_INFO(this->get_logger(), "MQTT listener started - consuming messages synchronously");
+
+    // Start a background thread to consume messages
+    if (!mqtt_reading_.load()) {
+        mqtt_reading_.store(true);
+        mqtt_read_thread_ = std::thread([this]() {
+            RCLCPP_INFO(this->get_logger(), "MQTT consumer thread started");
+
+            try {
+                while (mqtt_reading_.load() && mqtt_client_ && mqtt_client_->is_connected()) {
+                    mqtt::const_message_ptr msg;
+                    if (mqtt_client_->try_consume_message(&msg)) {
+                        mqtt_message_callback(msg->get_topic(), msg->get_payload_str(), this);
+                    } else {
+                        // No message available, sleep briefly to avoid busy-waiting
+                        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+                    }
+                }
+            } catch (const mqtt::exception& exc) {
+                RCLCPP_ERROR(this->get_logger(), "MQTT consumer error: %s", exc.what());
+            }
+
+            RCLCPP_INFO(this->get_logger(), "MQTT consumer thread exiting");
+        });
+    }
+}
+
+void HardwareInterface::mqtt_connect() {
+    if (!mqtt_client_) {
+        RCLCPP_ERROR(this->get_logger(), "MQTT client is not initialized");
+        return;
+    }
+
+    try {
+        RCLCPP_INFO(this->get_logger(), "Connecting to MQTT broker...");
+
+        mqtt_client_->connect();
+        RCLCPP_INFO(this->get_logger(), "Connected to broker");
+
+        RCLCPP_INFO(this->get_logger(), "Subscribing to topic: %s", mqtt_config_.topic.c_str());
+        mqtt_client_->subscribe(mqtt_config_.topic, 1);
+        RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", mqtt_config_.topic.c_str());
+    } catch (const mqtt::exception& exc) {
+        RCLCPP_ERROR(this->get_logger(), "MQTT Error: %s", exc.what());
+        throw;
+    }
+}
+
+void HardwareInterface::mqtt_stop_listen() {
+    if (mqtt_reading_.load()) {
+        mqtt_reading_.store(false);
+        if (mqtt_read_thread_.joinable()) {
+            mqtt_read_thread_.join();
+        }
+    }
+}
+
+void HardwareInterface::mqtt_close_connection() {
+    try {
+        mqtt_stop_listen();
+
+        if (mqtt_client_) {
+            if (mqtt_client_->is_connected()) {
+                mqtt_client_->disconnect();
+                RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client");
+            }
+            mqtt_client_.reset();
+        }
+    } catch (const mqtt::exception& exc) {
+        RCLCPP_ERROR(this->get_logger(), "Error while disconnecting MQTT: %s", exc.what());
+    }
+}
+
+} // namespace assignments::two::g2_2025_lifecycle_node

src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp

@@ -0,0 +1,73 @@
+/* nodes/HardwareInterface.hpp
+ * Hardware interface implementation for the IMU reader system.
+ *
+ * Manages the serial communication with the IMU hardware, including initialization,
+ * data acquisition, and shutdown procedures.
+ *
+ * Changelog:
+ * [28-10-2025] M.khalaf: Implemented template.
+ * [28-10-2025] M.khalaf: Added serial communication support.
+ * [30-10-2025] M.khalaf: Added MQTT support.
+ * [20-10-2025] M.khalaf: Refactored code for readability.
+ * [30-10-2025] M.khalaf: Improved error handling.
+ * [31-10-2025] M.khalaf: Fixed serial read and mqtt configurations.
+ * [06-11-2025] Wessel T, Vincent W: Clean up code, remove uncessary imports
+ */
+#pragma once
+
+#include <memory>
+#include <thread>
+#include <atomic>
+#include <string>
+#include <nlohmann/json.hpp>
+#include <mqtt/client.h>
+
+#include "serialib.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
+#include "MQTTParameters.hpp"
+
+namespace assignments::two::g2_2025_lifecycle_node {
+
+class HardwareInterface : public rclcpp::Node {
+
+public:
+    HardwareInterface(MQTTParameters mqtt_config);
+    ~HardwareInterface();
+
+    bool uart_open_device(const std::string& device_path, int baud_rate);
+    bool uart_device_is_open();
+    void uart_close_device();
+
+    void uart_start_read();
+    void uart_stop_read();
+
+    void mqtt_configure();
+    void mqtt_connect();
+
+    void mqtt_start_listen();
+    void mqtt_stop_listen();
+    void mqtt_close_connection();
+
+    void parse_and_publish_imu_data(const std::string& data);
+
+private:
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
+
+    serialib serial_;
+
+    MQTTParameters mqtt_config_;
+    std::unique_ptr<mqtt::client> mqtt_client_;
+
+    std::thread uart_read_thread_;
+    std::atomic_bool uart_reading_ {false};
+
+    std::thread mqtt_read_thread_;
+    std::atomic_bool mqtt_reading_ {false};
+
+    static void mqtt_message_callback(const std::string& topic, const std::string& payload, HardwareInterface* self);
+};
+
+}  // namespace assignments::two::g2_2025_lifecycle_node

src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp

@@ -0,0 +1,121 @@
+#include "LifecycleManager.hpp"
+#include "HardwareInterface.hpp"
+
+namespace assignments::two::g2_2025_lifecycle_node {
+
+LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("LifecycleManager") {
+    mqtt_config_.server_address =
+        this->declare_parameter<std::string>("mqtt_server_address", "tcp://localhost:1883");
+
+    mqtt_config_.client_id =
+        this->declare_parameter<std::string>("mqtt_client_id", "cpp_mqtt-client");
+
+    mqtt_config_.topic =
+        this->declare_parameter<std::string>("mqtt_subscribe_topic", "esp32/imu");
+
+    RCLCPP_INFO(this->get_logger(), "LifecycleManager node is ready");
+    device_path_ = this->declare_parameter<std::string>("device_path", "/dev/ttyUSB0");
+    baudrate_ = this->declare_parameter<int>("baudrate", 115200);
+    communication_type_ = this->declare_parameter<std::string>("comm_t", "serial");
+
+    hw_interface = std::make_shared<HardwareInterface>(mqtt_config_);
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+LifecycleManager::on_error(const rclcpp_lifecycle::State&) {
+    RCLCPP_ERROR(this->get_logger(), "Error occurred in lifecycle...");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+LifecycleManager::on_configure(const rclcpp_lifecycle::State&) {
+    RCLCPP_INFO(this->get_logger(), "configuring lifecycle...");
+
+    if (communication_type_ == "mqtt") {
+        RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication...");
+        hw_interface->mqtt_configure();
+    } else {
+        RCLCPP_INFO(this->get_logger(), "Using serial communication.");
+
+        if (!hw_interface->uart_open_device(device_path_, baudrate_)) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to open hardware device during configuration.");
+            return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
+        }
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Lifecycle configured successfully.");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+LifecycleManager::on_cleanup(const rclcpp_lifecycle::State&) {
+    RCLCPP_INFO(this->get_logger(), "cleaning up lifecycle...");
+    if (communication_type_ == "mqtt") {
+        hw_interface->mqtt_close_connection();
+    } else {
+        if (hw_interface->uart_device_is_open()) {
+            hw_interface->uart_stop_read();
+            hw_interface->uart_close_device();
+        }
+    }
+
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+LifecycleManager::on_activate(const rclcpp_lifecycle::State&) {
+    RCLCPP_INFO(this->get_logger(), "activating lifecycle...");
+
+    if (communication_type_ == "mqtt") {
+        RCLCPP_INFO(this->get_logger(), "Reading on MQTT...");
+        hw_interface->mqtt_start_listen();
+        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+
+    } else {
+        RCLCPP_INFO(this->get_logger(), "Reading on Serial...");
+        hw_interface->uart_start_read();
+    }
+
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) {
+    RCLCPP_INFO(this->get_logger(), "deactivating lifecycle...");
+
+    if (communication_type_ == "mqtt")
+    {
+        hw_interface->mqtt_stop_listen();
+        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+
+    } else
+    {
+        if (!hw_interface->uart_device_is_open()) {
+            RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation.");
+            hw_interface->uart_close_device();
+        }
+        RCLCPP_INFO(this->get_logger(), "Hardware device is open,closing device...");
+        hw_interface->uart_stop_read();
+
+        RCLCPP_INFO(this->get_logger(), "Lifecycle deactivated successfully.");
+        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    }
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+LifecycleManager::on_shutdown(const rclcpp_lifecycle::State&) {
+    if (communication_type_ == "mqtt") {
+        hw_interface->mqtt_close_connection();
+        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+    } else {
+        if (hw_interface->uart_device_is_open()) {
+            hw_interface->uart_stop_read();
+            hw_interface->uart_close_device();
+        }
+    }
+
+    RCLCPP_INFO(this->get_logger(), "shutting down lifecycle...");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+}  // namespace assignments::two::g2_2025_lifecycle_node

src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp

@@ -0,0 +1,51 @@
+/* nodes/LifecycleManager.hpp
+ * Lifecycle node implementation for managing the lifecycle of the IMU reader system.
+ *
+ * Manages the different states of the lifecycle node, including configuration,
+ * and hardware interface management.
+ *
+ * Changelog:
+ * [28-10-2025] M.khalaf: Implemented template.
+ */
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "HardwareInterface.hpp"
+
+#include "MQTTParameters.hpp"
+
+namespace assignments::two::g2_2025_lifecycle_node {
+
+class LifecycleManager : public rclcpp_lifecycle::LifecycleNode {
+public:
+    LifecycleManager();
+
+    // Hardware interface to interact with the IMU device. Made public for testing purposes
+    std::shared_ptr<HardwareInterface> hw_interface;
+
+private:
+    std::string device_path_;
+    int baudrate_;
+    std::string communication_type_;
+
+    MQTTParameters mqtt_config_ {};
+
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+        on_error(const rclcpp_lifecycle::State&);
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+        on_cleanup(const rclcpp_lifecycle::State&);
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+        on_configure(const rclcpp_lifecycle::State&);
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+        on_activate(const rclcpp_lifecycle::State&);
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+        on_deactivate(const rclcpp_lifecycle::State&);
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+        on_shutdown(const rclcpp_lifecycle::State&);
+};
+
+}  // namespace assignments::two::g2_2025_lifecycle_node

src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <string>
+
+namespace assignments::two {
+
+struct MQTTParameters {
+  std::string server_address;
+  std::string client_id;
+  std::string topic;
+};
+
+} // namespace assignments::two

src/g2_2025_imu_reader_pkg/test/ConfigManager.test.cpp

@@ -26,7 +26,7 @@ static const std::string TEST_CONFIG_NO_POOL_CONTENT = R"(
 [database]
 host = "localhost"
 port = 5432
-dbname = "grades"
+dbname = "imu_data"
 user = "postgres"
 password = "postgres"
 )";

src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp

@@ -0,0 +1,235 @@
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <lifecycle_msgs/msg/state.hpp>
+
+#include "g2_2025_lifecycle_node/nodes/LifecycleManager.hpp"
+
+using namespace assignments::two::g2_2025_lifecycle_node;
+
+class LifecycleManagerTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        // Initialize ROS2
+        rclcpp::init(0, nullptr);
+    }
+
+    void TearDown() override {
+        // Shutdown ROS2
+        rclcpp::shutdown();
+    }
+};
+
+TEST_F(LifecycleManagerTest, ConstructorTest) {
+    // Test 1: Verifies that LifecycleManager can be instantiated without crashing
+    ASSERT_NO_THROW({
+        auto node = std::make_shared<LifecycleManager>();
+        EXPECT_NE(node, nullptr);
+    });
+}
+
+TEST_F(LifecycleManagerTest, ParameterDeclarationTest) {
+    // Test 2: Verify all required parameters are declared with correct defaults
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Check that parameters exist
+    auto device_path = node->get_parameter("device_path").as_string();
+    auto baudrate = node->get_parameter("baudrate").as_int();
+    auto comm_t = node->get_parameter("comm_t").as_string();
+
+    EXPECT_EQ(device_path, "/dev/ttyUSB0");
+    EXPECT_EQ(baudrate, 115200);
+    EXPECT_EQ(comm_t, "serial");
+}
+
+TEST_F(LifecycleManagerTest, ParameterRuntimeReadTest) {
+    // Test 3: Verify parameters can be read at runtime
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Set new parameter values
+    node->set_parameter(rclcpp::Parameter("device_path", "/dev/ttyACM0"));
+    node->set_parameter(rclcpp::Parameter("comm_t", "mqtt"));
+
+    // Verify new values are read
+    auto device_path = node->get_parameter("device_path").as_string();
+    auto comm_t = node->get_parameter("comm_t").as_string();
+
+    EXPECT_EQ(device_path, "/dev/ttyACM0");
+    EXPECT_EQ(comm_t, "mqtt");
+}
+
+
+TEST_F(LifecycleManagerTest, InitialStateTest) {
+    // Test 4: Verify node starts in UNCONFIGURED state
+    auto node = std::make_shared<LifecycleManager>();
+
+    EXPECT_EQ(node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+}
+
+TEST_F(LifecycleManagerTest, ConfigureSerialModeTest) {
+    // Test 5: Test on_configure in serial communication mode
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Use /dev/null as a safe test device
+    node->set_parameter(rclcpp::Parameter("device_path", "/dev/null"));
+    node->set_parameter(rclcpp::Parameter("comm_t", "serial"));
+
+    // Attempt to configure
+    node->configure();
+
+    // Check state after configure
+    auto state_id = node->get_current_state().id();
+    // Should be INACTIVE (2) if successful or UNCONFIGURED (0) if failed
+    EXPECT_TRUE(state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+                state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+}
+
+TEST_F(LifecycleManagerTest, ConfigureMQTTModeTest) {
+    // Test 6: Test on_configure in MQTT communication mode
+    auto node = std::make_shared<LifecycleManager>();
+
+    node->set_parameter(rclcpp::Parameter("comm_t", "mqtt"));
+
+    // Attempt to configure (MQTT setup doesn't require actual broker)
+    node->configure();
+
+    //should be INACTIVE or UNCONFIGURED
+    auto state_id = node->get_current_state().id();
+    EXPECT_TRUE(state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+                state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+}
+
+TEST_F(LifecycleManagerTest, ConfigureSerialFailureTest) {
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Use /dev/null which should open successfully for this test
+    node->set_parameter(rclcpp::Parameter("device_path", "/dev/null"));
+    node->set_parameter(rclcpp::Parameter("comm_t", "serial"));
+
+    // Attempt to configure
+    node->configure();
+
+    SUCCEED();
+}
+
+TEST_F(LifecycleManagerTest, DeactivateSerialModeTest) {
+    // Test 8: Test on_deactivate in serial communication mode
+    auto node = std::make_shared<LifecycleManager>();
+
+    node->set_parameter(rclcpp::Parameter("device_path", "/dev/null"));
+    node->set_parameter(rclcpp::Parameter("comm_t", "serial"));
+
+    // Configure to INACTIVE state
+    node->configure();
+    auto config_state = node->get_current_state().id();
+
+    if (config_state == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
+        // Deactivate from INACTIVE
+        node->deactivate();
+        auto final_state = node->get_current_state().id();
+        // Should still be in INACTIVE or return to UNCONFIGURED
+        EXPECT_TRUE(final_state == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+                    final_state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+    }
+}
+
+
+TEST_F(LifecycleManagerTest, DevicePathParameterTest) {
+    // Test 24: Verify device_path parameter is correctly used
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Test multiple device paths
+    std::vector<std::string> device_paths = {"/dev/ttyUSB0", "/dev/ttyACM0", "/dev/null"};
+
+    for (const auto& path : device_paths) {
+        node->set_parameter(rclcpp::Parameter("device_path", path));
+        auto retrieved_path = node->get_parameter("device_path").as_string();
+        EXPECT_EQ(retrieved_path, path);
+    }
+}
+
+TEST_F(LifecycleManagerTest, BaudRateParameterTest) {
+    // Test 25: Verify baudrate parameter is correctly used
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Test multiple baud rates
+    std::vector<int> baud_rates = {9600, 115200, 230400};
+
+    for (int baud : baud_rates) {
+        node->set_parameter(rclcpp::Parameter("baudrate", baud));
+        auto retrieved_baud = node->get_parameter("baudrate").as_int();
+        EXPECT_EQ(retrieved_baud, baud);
+    }
+}
+
+TEST_F(LifecycleManagerTest, ParameterUpdateBehaviorTest) {
+    // Test 19: Test that parameter changes are respected across transitions
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Start with serial
+    node->set_parameter(rclcpp::Parameter("comm_t", "serial"));
+    node->set_parameter(rclcpp::Parameter("device_path", "/dev/null"));
+
+    auto comm_type = node->get_parameter("comm_t").as_string();
+    EXPECT_EQ(comm_type, "serial");
+
+    // Switch to MQTT
+    node->set_parameter(rclcpp::Parameter("comm_t", "mqtt"));
+    comm_type = node->get_parameter("comm_t").as_string();
+    EXPECT_EQ(comm_type, "mqtt");
+
+    // Switch back to serial
+    node->set_parameter(rclcpp::Parameter("comm_t", "serial"));
+    comm_type = node->get_parameter("comm_t").as_string();
+    EXPECT_EQ(comm_type, "serial");
+}
+
+TEST_F(LifecycleManagerTest, ResourceCleanupTest) {
+    // Test 9: Verify resources are properly cleaned up
+    {
+        auto node = std::make_shared<LifecycleManager>();
+
+        node->set_parameter(rclcpp::Parameter("device_path", "/dev/null"));
+        node->set_parameter(rclcpp::Parameter("comm_t", "serial"));
+
+        // Just configure, don't activate (avoid thread spawning)
+        node->configure();
+
+        // Node goes out of scope; verify no crashes
+    }
+
+    // If we reach here without crashes, cleanup was successful
+    SUCCEED();
+}
+
+TEST_F(LifecycleManagerTest, ErrorLoggingTest) {
+    // Test 10: Verify error messages are logged during failures
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Set invalid device path to trigger error
+    node->set_parameter(rclcpp::Parameter("device_path", "/dev/nonexistent_device_xyz"));
+    node->set_parameter(rclcpp::Parameter("comm_t", "serial"));
+
+    // This should attempt to configure
+    node->configure();
+
+    // The node may still transition state, but we verify it handles errors gracefully
+    // by not crashing
+    SUCCEED();
+}
+
+TEST_F(LifecycleManagerTest, HardwareInterfaceIntegrationTest) {
+    // Test 11: Verify complete integration with HardwareInterface
+    auto node = std::make_shared<LifecycleManager>();
+
+    // Verify HardwareInterface instance is created
+    EXPECT_NE(node->hw_interface, nullptr);
+
+    // Verify we can make method calls on the hardware interface
+    EXPECT_FALSE(node->hw_interface->uart_device_is_open());
+}
+
+int main(int argc, char** argv) {
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 3.8)
+project(g2_2025_odometry_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(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+# Add executable for position_speed_approximator_node
+add_executable(position_speed_approximator_node
+  src/g2_2025_imu_position_approximator_node/nodes/IMUPositionApproximator.cpp
+  src/g2_2025_imu_position_approximator_node/Main.cpp
+)
+
+target_include_directories(position_speed_approximator_node PRIVATE
+  ${CMAKE_CURRENT_SOURCE_DIR}/src
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_position_approximator_node
+)
+
+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}
+)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_executable(imu_data_simulator_node
+    src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.cpp
+    src/g2_2025_imu_data_simulator_node/main.cpp
+    src/simulator/Simulator.cpp)
+
+target_include_directories(imu_data_simulator_node PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_data_simulator_node
+)
+
+ament_target_dependencies(imu_data_simulator_node rclcpp sensor_msgs geometry_msgs)
+
+add_executable(wheel_data_simulator_node
+    src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp
+    src/g2_2025_wheel_data_simulator_node/main.cpp
+    src/simulator/Simulator.cpp)
+
+target_include_directories(wheel_data_simulator_node PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_wheel_data_simulator_node
+)
+
+ament_target_dependencies(wheel_data_simulator_node rclcpp std_msgs)
+
+install(TARGETS
+  imu_data_simulator_node
+  wheel_data_simulator_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  # Simulator unit tests
+  ament_add_gtest(test_simulator test/TestSimulator.cpp src/simulator/Simulator.cpp)
+  target_include_directories(test_simulator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src)
+  ament_target_dependencies(test_simulator rclcpp)
+
+  # IMU simulator integration tests
+  ament_add_gtest(test_imu_simulator test/TestImuSimulator.cpp
+    src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.cpp
+    src/simulator/Simulator.cpp)
+  target_include_directories(test_imu_simulator PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_data_simulator_node)
+  ament_target_dependencies(test_imu_simulator rclcpp sensor_msgs geometry_msgs)
+
+  # Wheel simulator integration tests
+  ament_add_gtest(test_wheel_simulator test/TestWheelSimulator.cpp
+    src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp
+    src/simulator/Simulator.cpp)
+  target_include_directories(test_wheel_simulator PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_wheel_data_simulator_node)
+  ament_target_dependencies(test_wheel_simulator rclcpp std_msgs)
+endif()
+
+ament_package()

src/g2_2025_odometry_pkg/config/opt.yaml

@@ -0,0 +1,74 @@
+imu_data_simulator:
+  ros__parameters:
+    max_intervals: 4
+    publish_rate: 10.0
+    linear_x:
+      num_intervals: 4
+      interval_0:
+        type: "linear"
+        t_start: 0.0
+        t_end: 5.0
+        y_start: 0.0
+        y_end: 9.81
+      interval_1:
+        type: "linear"
+        t_start: 7.5
+        t_end: 12.5
+        y_start: 9.81
+        y_end: 0.0
+      interval_2:
+        type: "linear"
+        t_start: 15.0
+        t_end: 20.0
+        y_start: 0.0
+        y_end: -9.81
+      interval_3:
+        type: "linear"
+        t_start: 22.5
+        t_end: 25.0
+        y_start: -9.81
+        y_end: 0.0
+    angular_z:
+      num_intervals: 1
+      interval_0:
+        type: "constant"
+        t_start: 2.0
+        y_start: 1.57
+    angular_x:
+      num_intervals: 2
+      interval_0:
+        type: "quadratic"
+        t_start: 3.0
+        y_start: 0.0
+        t_mid: 4.5
+        y_mid: 1.57
+        t_end: 6.0
+        y_end: 0.0
+      interval_1:
+        type: "quadratic"
+        t_start: 7.0
+        y_start: 0.0
+        t_mid: 8.5
+        y_mid: -1.57
+        t_end: 10.0
+        y_end: 0.0
+
+wheel_data_simulator:
+  ros__parameters:
+    publish_rate: 10.0
+    wheel_fl:
+      num_intervals: 1
+      interval_0:
+        type: "linear"
+        t_start: 0.0
+        t_end: 10.0
+        y_start: 0.0
+        y_end: 5.0
+    wheel_fr:
+      num_intervals: 1
+      interval_0:
+        type: "linear"
+        t_start: 0.0
+        t_end: 10.0
+        y_start: 0.0
+        y_end: 10.0

src/g2_2025_odometry_pkg/launch/imu_sim.launch.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="params_file" default="$(find-pkg-share g2_2025_odometry_pkg)/config/opt.yaml"/>
+    
+    <node pkg="g2_2025_odometry_pkg" exec="imu_data_simulator_node" name="imu_data_simulator" output="screen">
+        <param from="$(var params_file)"/>
+    </node>
+</launch>

src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="params_file" default="$(find-pkg-share g2_2025_odometry_pkg)/config/opt.yaml"/>
+    
+    <node pkg="g2_2025_odometry_pkg" exec="wheel_data_simulator_node" name="wheel_data_simulator" output="screen">
+        <param from="$(var params_file)"/>
+    </node>
+</launch>

src/g2_2025_odometry_pkg/package.xml

@@ -0,0 +1,23 @@
+<?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_odometry_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="contact@wessel.gg">wessel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <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>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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_data_simulator_node/main.cpp

@@ -0,0 +1,18 @@
+#include "rclcpp/rclcpp.hpp"
+#include "nodes/ImuDataSimulator.hpp"
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+
+    try {
+        auto node = std::make_shared<assignments::three::data_simulator_node::DataSimulator>();
+        rclcpp::spin(node);
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(rclcpp::get_logger("imu_data_simulator"), "Failed to initialize node: %s", e.what());
+        rclcpp::shutdown();
+        return 1;
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}

src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.cpp

@@ -0,0 +1,62 @@
+#include "ImuDataSimulator.hpp"
+
+namespace assignments::three::data_simulator_node {
+
+DataSimulator::DataSimulator() : Node("imu_data_simulator") {
+    RCLCPP_INFO(this->get_logger(), "ImuDataSimulator node created");
+
+    start_time_ = this->now();
+
+    this->declare_parameter("publish_rate", 10.0);
+    double rate = this->get_parameter("publish_rate").as_double();
+
+    axes_ = { "linear_x", "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" };
+
+    // Simulator loads parameters itself
+    simulator_ = std::make_unique<Simulator>(this, axes_);
+
+    imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("simulated_imu_data", 10);
+    RCLCPP_INFO(this->get_logger(), "Created IMU Publisher on topic 'simulated_imu_data'");
+
+    // Use publish_rate parameter
+    int timer_ms = static_cast<int>(1000.0 / rate);
+    RCLCPP_INFO(this->get_logger(), "Publishing at %.1f Hz (every %d ms)", rate, timer_ms);
+
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(timer_ms),
+        std::bind(&DataSimulator::publish_imu_data, this)
+    );
+}
+
+DataSimulator::~DataSimulator() {
+    RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed");
+}
+
+void DataSimulator::publish_imu_data() {
+    auto imu_msg = std::make_shared<sensor_msgs::msg::Imu>();
+
+    imu_msg->header.stamp = this->now();
+    imu_msg->header.frame_id = "imu_link";
+
+    // Calculate elapsed time since node start
+    double elapsed_time = (this->now() - start_time_).seconds();
+
+    // Get values for each axis
+    imu_msg->linear_acceleration.x = simulator_->get_object_value("linear_x", elapsed_time);
+    imu_msg->linear_acceleration.y = simulator_->get_object_value("linear_y", elapsed_time);
+    imu_msg->linear_acceleration.z = simulator_->get_object_value("linear_z", elapsed_time);
+    imu_msg->angular_velocity.x = simulator_->get_object_value("angular_x", elapsed_time);
+    imu_msg->angular_velocity.y = simulator_->get_object_value("angular_y", elapsed_time);
+    imu_msg->angular_velocity.z = simulator_->get_object_value("angular_z", elapsed_time);
+
+    RCLCPP_INFO(this->get_logger(),
+        "t=%.2fs - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]",
+        elapsed_time,
+        imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z,
+        imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z
+    );
+
+    imu_publisher_->publish(*imu_msg);
+}
+
+} // namespace assignments::three::data_simulator_node

src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "simulator/Simulator.hpp"
+
+
+namespace assignments::three::data_simulator_node {
+
+using assignments::three::Simulator;
+
+class DataSimulator : public rclcpp::Node {
+public:
+    DataSimulator();
+    ~DataSimulator();
+private:
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Time start_time_;
+
+    std::vector<std::string> axes_;
+    std::unique_ptr<Simulator> simulator_;
+
+    void publish_imu_data();
+
+};
+
+} // namespace assignments::three::data_simulator_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

src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp

@@ -0,0 +1,18 @@
+#include "rclcpp/rclcpp.hpp"
+#include "nodes/WheelDataSimulator.hpp"
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+
+    try {
+        auto node = std::make_shared<assignments::three::wheel_data_simulator_node::DataSimulator>();
+        rclcpp::spin(node);
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(rclcpp::get_logger("wheel_data_simulator"), "Failed to initialize node: %s", e.what());
+        rclcpp::shutdown();
+        return 1;
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}

src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp

@@ -0,0 +1,58 @@
+#include "WheelDataSimulator.hpp"
+
+namespace assignments::three::wheel_data_simulator_node {
+
+DataSimulator::DataSimulator() : Node("wheel_data_simulator") {
+    RCLCPP_INFO(this->get_logger(), "WheelDataSimulator node created");
+
+    start_time_ = this->now();
+
+    this->declare_parameter("publish_rate", 10.0);
+    double rate = this->get_parameter("publish_rate").as_double();
+
+    wheels_ = { "wheel_fl", "wheel_fr", "wheel_rl", "wheel_rr" };
+
+    // Simulator loads parameters itself
+    simulator_ = std::make_unique<Simulator>(this, wheels_);
+
+    wheel_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("simulated_wheel_data", 10);
+    RCLCPP_INFO(this->get_logger(), "Created wheel Publisher on topic 'simulated_wheel_data'");
+
+    // Use publish_rate parameter
+    int timer_ms = static_cast<int>(1000.0 / rate);
+    RCLCPP_INFO(this->get_logger(), "Publishing at %.1f Hz (every %d ms)", rate, timer_ms);
+
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(timer_ms),
+        std::bind(&DataSimulator::publish_wheel_data, this)
+    );
+}
+
+DataSimulator::~DataSimulator() {
+    RCLCPP_INFO(this->get_logger(), "WheelDataSimulator node destroyed");
+}
+
+void DataSimulator::publish_wheel_data() {
+    auto wheel_msg = std::make_shared<std_msgs::msg::Float64MultiArray>();
+
+    // Calculate elapsed time since node start
+    double elapsed_time = (this->now() - start_time_).seconds();
+
+    // For now, just log wheel values (adjust based on your actual message type)
+    wheel_msg->data = {
+        simulator_->get_object_value("wheel_fl", elapsed_time),
+        simulator_->get_object_value("wheel_fr", elapsed_time),
+        simulator_->get_object_value("wheel_rl", elapsed_time),
+        simulator_->get_object_value("wheel_rr", elapsed_time)
+    };
+
+    RCLCPP_INFO(this->get_logger(),
+        "t=%.2fs - Wheel Data [%.3f, %.3f, %.3f, %.3f]",
+        elapsed_time,
+        wheel_msg->data[0], wheel_msg->data[1], wheel_msg->data[2], wheel_msg->data[3]
+    );
+
+    wheel_publisher_->publish(*wheel_msg);
+}
+
+} // namespace assignments::three::wheel_data_simulator_node

src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "simulator/Simulator.hpp"
+#include <vector>
+#include <string>
+#include <map>
+#include <memory>
+
+namespace assignments::three::wheel_data_simulator_node {
+
+using assignments::three::Simulator;
+using assignments::three::IntervalConfig;
+using assignments::three::SimType;
+
+class DataSimulator : public rclcpp::Node {
+public:
+    DataSimulator();
+    ~DataSimulator();
+private:
+    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Time start_time_;
+
+    std::vector<std::string> wheels_;
+    std::unique_ptr<Simulator> simulator_;
+
+    void publish_wheel_data();
+
+};
+
+} // namespace assignments::three::wheel_data_simulator_node

src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp

@@ -0,0 +1,152 @@
+#include "Simulator.hpp"
+
+namespace assignments::three {
+
+Simulator::Simulator(rclcpp::Node* node, const std::vector<std::string>& objects) {
+    load_intervals(node, objects);
+}
+
+Simulator::~Simulator() {
+}
+
+void Simulator::load_intervals(rclcpp::Node* node, const std::vector<std::string>& objects) {
+    node->declare_parameter("max_intervals", 4);
+    max_intervals_ = node->get_parameter("max_intervals").as_int();
+
+    for (const auto& object : objects) {
+        node->declare_parameter(object + ".num_intervals", 0);
+        int num_intervals = node->get_parameter(object + ".num_intervals").as_int();
+
+        RCLCPP_INFO(node->get_logger(), "Loading %d intervals for object '%s'", num_intervals, object.c_str());
+        std::vector<IntervalConfig> intervals;
+
+        for (int i = 0; i < std::min(num_intervals, max_intervals_); i++) {
+            std::string prefix = object + ".interval_" + std::to_string(i);
+
+            node->declare_parameter(prefix + ".type", "constant");
+            node->declare_parameter(prefix + ".t_start", 0.0);
+            node->declare_parameter(prefix + ".t_end", 0.0);
+            node->declare_parameter(prefix + ".y_start", 0.0);
+            node->declare_parameter(prefix + ".y_end", 0.0);
+            node->declare_parameter(prefix + ".t_mid", 0.0);
+            node->declare_parameter(prefix + ".y_mid", 0.0);
+
+            IntervalConfig config;
+            std::string type_str = node->get_parameter(prefix + ".type").as_string();
+            if (type_str == "constant") {
+                config.type = SimType::CONSTANT;
+            } else if (type_str == "linear") {
+                config.type = SimType::LINEAR;
+            } else if (type_str == "quadratic") {
+                config.type = SimType::QUADRATIC;
+            } else {
+                RCLCPP_WARN(node->get_logger(), "Unknown interval type '%s', defaulting to 'constant'", type_str.c_str());
+                config.type = SimType::CONSTANT;
+            }
+            config.t_start = node->get_parameter(prefix + ".t_start").as_double();
+            config.t_end = node->get_parameter(prefix + ".t_end").as_double();
+            config.y_start = node->get_parameter(prefix + ".y_start").as_double();
+            config.y_end = node->get_parameter(prefix + ".y_end").as_double();
+            config.t_mid = node->get_parameter(prefix + ".t_mid").as_double();
+            config.y_mid = node->get_parameter(prefix + ".y_mid").as_double();
+
+            intervals.push_back(config);
+        }
+
+        for (size_t j = 0; j < intervals.size(); ++j) {
+            const auto& cfg = intervals[j];
+            const char* type_str = (cfg.type == SimType::CONSTANT) ? "constant" :
+                (cfg.type == SimType::LINEAR) ? "linear" : "quadratic";
+            RCLCPP_DEBUG(node->get_logger(),
+                "Object '%s' Interval %zu: type='%s', t_start=%.6f, t_end=%.6f, y_start=%.6f, y_end=%.6f, t_mid=%.6f, y_mid=%.6f",
+                object.c_str(), j, type_str,
+                cfg.t_start, cfg.t_end, cfg.y_start, cfg.y_end, cfg.t_mid, cfg.y_mid
+            );
+
+            // Check for overlaps with other intervals
+            for (size_t k = j + 1; k < intervals.size(); ++k) {
+                const auto& other = intervals[k];
+                // Two intervals overlap if one starts before the other ends
+                bool overlaps = (cfg.t_start < other.t_end && cfg.t_end > other.t_start);
+                if (overlaps) {
+                    RCLCPP_ERROR(node->get_logger(),
+                        "Object '%s': Interval %zu [%.2f, %.2f] overlaps with Interval %zu [%.2f, %.2f]",
+                        object.c_str(), j, cfg.t_start, cfg.t_end, k, other.t_start, other.t_end
+                    );
+                    throw std::runtime_error("Overlapping intervals detected for object '" + object + "'");
+                }
+            }
+        }
+
+        object_intervals_[object] = intervals;
+    }
+}
+
+double Simulator::compute_value(double t, const IntervalConfig& interval) {
+    // Check if time is within interval
+    if (t < interval.t_start || t > interval.t_end) {
+        return 0.0;
+    }
+
+    switch (interval.type) {
+        case SimType::CONSTANT:
+            // Constant: y = c
+            return interval.y_start;
+
+        case SimType::LINEAR: {
+            // Linear interpolation: y = y0 + (y1 - y0) * (t - t0) / (t1 - t0)
+            double t0 = interval.t_start;
+            double t1 = interval.t_end;
+            double y0 = interval.y_start;
+            double y1 = interval.y_end;
+
+            double L0 = (t - t1) / (t0 - t1);
+            double L1 = (t - t0) / (t1 - t0);
+
+            return y0 * L0 + y1 * L1;
+        }
+
+        case SimType::QUADRATIC: {
+            // Quadratic using Lagrange interpolation through 3 points
+            double t0 = interval.t_start;
+            double t1 = interval.t_mid;
+            double t2 = interval.t_end;
+            double y0 = interval.y_start;
+            double y1 = interval.y_mid;
+            double y2 = interval.y_end;
+
+            double L0 = ((t - t1) * (t - t2)) / ((t0 - t1) * (t0 - t2));
+            double L1 = ((t - t0) * (t - t2)) / ((t1 - t0) * (t1 - t2));
+            double L2 = ((t - t0) * (t - t1)) / ((t2 - t0) * (t2 - t1));
+
+            return y0 * L0 + y1 * L1 + y2 * L2;
+        }
+    }
+
+    return 0.0;
+}
+
+double Simulator::get_object_value(const std::string& object, double t) {
+    // Check if object exists in configuration
+    auto it = object_intervals_.find(object);
+    if (it == object_intervals_.end()) {
+        return 0.0;  // Default value if object not configured
+    }
+
+    // Check each interval for this object
+    double last_value = 0.0;
+    for (const auto& interval : it->second) {
+        if (t >= interval.t_start && t <= interval.t_end) {
+            // Currently in this interval
+            return compute_value(t, interval);
+        } else if (t > interval.t_end) {
+            // Past this interval - remember its end value for holding
+            last_value = compute_value(interval.t_end, interval);
+        }
+    }
+
+    // Not in any interval - return last known value (or 0.0 if before all intervals)
+    return last_value;
+}
+
+} // namespace assignments::three

src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp

@@ -0,0 +1,41 @@
+#pragma once
+#include <rclcpp/rclcpp.hpp>
+#include <vector>
+#include <string>
+#include <map>
+#include <algorithm>
+
+namespace assignments::three {
+
+enum class SimType {
+    CONSTANT,
+    LINEAR,
+    QUADRATIC
+};
+
+struct IntervalConfig {
+    SimType type;
+    double t_start;
+    double t_end;
+    double y_start;
+    double y_end;
+    double t_mid;      // For quadratic
+    double y_mid;      // For quadratic
+};
+
+class Simulator {
+public:
+    Simulator(rclcpp::Node* node, const std::vector<std::string>& objects);
+    ~Simulator();
+
+    double get_object_value(const std::string& object, double t);
+
+private:
+    int max_intervals_;
+    std::map<std::string, std::vector<IntervalConfig>> object_intervals_;
+
+    void load_intervals(rclcpp::Node* node, const std::vector<std::string>& objects);
+    double compute_value(double t, const IntervalConfig& interval);
+};
+
+} // namespace assignments::three

src/g2_2025_odometry_pkg/test/TestImuSimulator.cpp

@@ -0,0 +1,171 @@
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include "g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.hpp"
+#include <memory>
+
+using namespace assignments::three::data_simulator_node;
+
+class IMUSimulatorTest : public ::testing::Test {
+protected:
+    static void SetUpTestSuite() {
+        rclcpp::init(0, nullptr);
+    }
+
+    static void TearDownTestSuite() {
+        rclcpp::shutdown();
+    }
+
+    void setupBasicIMUParams(rclcpp::Node* node) {
+        node->declare_parameter("max_intervals", 4);
+        node->declare_parameter("publish_rate", 10.0);
+
+        // Setup linear_x
+        node->declare_parameter("linear_x.num_intervals", 1);
+        node->declare_parameter("linear_x.interval_0.type", "constant");
+        node->declare_parameter("linear_x.interval_0.t_start", 0.0);
+        node->declare_parameter("linear_x.interval_0.t_end", 5.0);
+        node->declare_parameter("linear_x.interval_0.y_start", 1.0);
+        node->declare_parameter("linear_x.interval_0.y_end", 0.0);
+        node->declare_parameter("linear_x.interval_0.t_mid", 0.0);
+        node->declare_parameter("linear_x.interval_0.y_mid", 0.0);
+
+        // Setup other axes with no intervals
+        for (const auto& axis : { "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" }) {
+            node->declare_parameter(std::string(axis) + ".num_intervals", 0);
+        }
+    }
+};
+
+// Test node initialization
+TEST_F(IMUSimulatorTest, NodeInitialization) {
+    auto node = std::make_shared<rclcpp::Node>("test_imu_init_node");
+    setupBasicIMUParams(node.get());
+
+    // Should not throw
+    EXPECT_NO_THROW({
+        DataSimulator sim;
+    });
+}
+
+// Test IMU message publishing
+TEST_F(IMUSimulatorTest, MessagePublishing) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_imu_pub_node");
+
+    sensor_msgs::msg::Imu::SharedPtr received_msg;
+    auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
+        "simulated_imu_data",
+        10,
+        [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
+            received_msg = msg;
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin a few times to allow publishing
+    for (int i = 0; i < 5; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    // Check that we received a message
+    ASSERT_NE(received_msg, nullptr);
+}
+
+// Test IMU message frame_id
+TEST_F(IMUSimulatorTest, MessageFrameId) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_imu_frame_node");
+
+    sensor_msgs::msg::Imu::SharedPtr received_msg;
+    auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
+        "simulated_imu_data",
+        10,
+        [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
+            received_msg = msg;
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin a few times to allow publishing
+    for (int i = 0; i < 5; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    ASSERT_NE(received_msg, nullptr);
+    EXPECT_EQ(received_msg->header.frame_id, "imu_link");
+}
+
+// Test linear acceleration values
+TEST_F(IMUSimulatorTest, LinearAccelerationValues) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_imu_linear_node");
+
+    sensor_msgs::msg::Imu::SharedPtr received_msg;
+    auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
+        "simulated_imu_data",
+        10,
+        [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
+            if (!received_msg) {  // Only capture first message
+                received_msg = msg;
+            }
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin until we receive a message
+    for (int i = 0; i < 10 && !received_msg; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    ASSERT_NE(received_msg, nullptr);
+
+    // Values should be set according to configuration
+    // Since we don't control exact timing, just verify structure
+    EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.x));
+    EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.y));
+    EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.z));
+}
+
+// Test angular velocity values
+TEST_F(IMUSimulatorTest, AngularVelocityValues) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_imu_angular_node");
+
+    sensor_msgs::msg::Imu::SharedPtr received_msg;
+    auto subscription = test_node->create_subscription<sensor_msgs::msg::Imu>(
+        "simulated_imu_data",
+        10,
+        [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) {
+            if (!received_msg) {
+                received_msg = msg;
+            }
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin until we receive a message
+    for (int i = 0; i < 10 && !received_msg; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    ASSERT_NE(received_msg, nullptr);
+
+    // Values should be set according to configuration
+    EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.x));
+    EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.y));
+    EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.z));
+}
+
+int main(int argc, char** argv) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

src/g2_2025_odometry_pkg/test/TestSimulator.cpp

@@ -0,0 +1,257 @@
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include "simulator/Simulator.hpp"
+#include <memory>
+
+using namespace assignments::three;
+
+class SimulatorTest : public ::testing::Test {
+protected:
+    static void SetUpTestSuite() {
+        rclcpp::init(0, nullptr);
+    }
+
+    static void TearDownTestSuite() {
+        rclcpp::shutdown();
+    }
+
+    // Helper to create a node with parameter overrides
+    std::shared_ptr<rclcpp::Node> createNodeWithParams(
+        const std::string& name,
+        const std::vector<rclcpp::Parameter>& params)
+    {
+        rclcpp::NodeOptions options;
+        options.parameter_overrides(params);
+        return std::make_shared<rclcpp::Node>(name, options);
+    }
+};
+
+// Test constant interval
+TEST_F(SimulatorTest, ConstantInterval) {
+    auto node = createNodeWithParams("test_constant_node", {
+        rclcpp::Parameter("max_intervals", 4),
+        rclcpp::Parameter("test_channel.num_intervals", 1),
+        rclcpp::Parameter("test_channel.interval_0.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+    Simulator sim(node.get(), channels);    // Test values within interval
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 5.0);
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0);
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 5.0);
+
+    // Test value after interval holds
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 5.0);
+}
+
+// Test linear interval
+TEST_F(SimulatorTest, LinearInterval) {
+    auto node = createNodeWithParams("test_linear_node", {
+        rclcpp::Parameter("max_intervals", 4),
+        rclcpp::Parameter("test_channel.num_intervals", 1),
+        rclcpp::Parameter("test_channel.interval_0.type", "linear"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+    Simulator sim(node.get(), channels);
+
+    // Test linear interpolation
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0);
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0);
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 10.0);
+
+    // Test value after interval holds
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 10.0);
+}
+
+// Test quadratic interval
+TEST_F(SimulatorTest, QuadraticInterval) {
+    auto node = createNodeWithParams("test_quadratic_node", {
+        rclcpp::Parameter("max_intervals", 4),
+        rclcpp::Parameter("test_channel.num_intervals", 1),
+        rclcpp::Parameter("test_channel.interval_0.type", "quadratic"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 5.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 10.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+    Simulator sim(node.get(), channels);
+
+    // Test quadratic interpolation
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0);
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 10.0);
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 0.0);
+
+    // Test value after interval holds
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 0.0);
+}
+
+// Test multiple intervals
+TEST_F(SimulatorTest, MultipleIntervals) {
+    auto node = createNodeWithParams("test_multiple_node", {
+        rclcpp::Parameter("max_intervals", 4),
+        rclcpp::Parameter("test_channel.num_intervals", 2),
+        // First interval: constant 5.0 from t=0 to t=5
+        rclcpp::Parameter("test_channel.interval_0.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 5.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0),
+        // Second interval: constant 10.0 from t=10 to t=15
+        rclcpp::Parameter("test_channel.interval_1.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_1.t_start", 10.0),
+        rclcpp::Parameter("test_channel.interval_1.t_end", 15.0),
+        rclcpp::Parameter("test_channel.interval_1.y_start", 10.0),
+        rclcpp::Parameter("test_channel.interval_1.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+    Simulator sim(node.get(), channels);
+
+    // Test first interval
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 2.5), 5.0);
+
+    // Test gap between intervals - should hold value from first interval
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 7.5), 5.0);
+
+    // Test second interval
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 12.5), 10.0);
+
+    // Test after all intervals - should hold value from last interval
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 20.0), 10.0);
+}
+
+// Test value before any interval
+TEST_F(SimulatorTest, ValueBeforeIntervals) {
+    auto node = createNodeWithParams("test_before_node", {
+        rclcpp::Parameter("max_intervals", 4),
+        rclcpp::Parameter("test_channel.num_intervals", 1),
+        rclcpp::Parameter("test_channel.interval_0.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 20.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+    Simulator sim(node.get(), channels);
+
+    // Before any interval starts, should return 0.0
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 0.0);
+}
+
+// Test non-existent channel
+TEST_F(SimulatorTest, NonExistentChannel) {
+    auto node = createNodeWithParams("test_nonexistent_node", {
+        rclcpp::Parameter("max_intervals", 4),
+        rclcpp::Parameter("test_channel.num_intervals", 1),
+        rclcpp::Parameter("test_channel.interval_0.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+    Simulator sim(node.get(), channels);
+
+    // Query non-existent channel should return 0.0
+    EXPECT_DOUBLE_EQ(sim.get_object_value("nonexistent_channel", 5.0), 0.0);
+}
+
+// Test overlapping intervals detection
+TEST_F(SimulatorTest, OverlappingIntervalsThrowsException) {
+    auto node = createNodeWithParams("test_overlap_node", {
+        rclcpp::Parameter("max_intervals", 4),
+        rclcpp::Parameter("test_channel.num_intervals", 2),
+        // First interval: t=0 to t=10
+        rclcpp::Parameter("test_channel.interval_0.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 5.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0),
+        // Second interval: t=5 to t=15 (overlaps with first)
+        rclcpp::Parameter("test_channel.interval_1.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_1.t_start", 5.0),
+        rclcpp::Parameter("test_channel.interval_1.t_end", 15.0),
+        rclcpp::Parameter("test_channel.interval_1.y_start", 10.0),
+        rclcpp::Parameter("test_channel.interval_1.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+
+    // Should throw exception due to overlapping intervals
+    EXPECT_THROW({
+        Simulator sim(node.get(), channels);
+    }, std::runtime_error);
+}
+
+// Test max_intervals limit
+TEST_F(SimulatorTest, MaxIntervalsLimit) {
+    auto node = createNodeWithParams("test_max_intervals_node", {
+        rclcpp::Parameter("max_intervals", 2),
+        rclcpp::Parameter("test_channel.num_intervals", 3),
+        rclcpp::Parameter("test_channel.interval_0.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_0.t_start", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_end", 10.0),
+        rclcpp::Parameter("test_channel.interval_0.y_start", 1.0),
+        rclcpp::Parameter("test_channel.interval_0.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_1.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_1.t_start", 10.0),
+        rclcpp::Parameter("test_channel.interval_1.t_end", 20.0),
+        rclcpp::Parameter("test_channel.interval_1.y_start", 2.0),
+        rclcpp::Parameter("test_channel.interval_1.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_2.type", "constant"),
+        rclcpp::Parameter("test_channel.interval_2.t_start", 20.0),
+        rclcpp::Parameter("test_channel.interval_2.t_end", 30.0),
+        rclcpp::Parameter("test_channel.interval_2.y_start", 3.0),
+        rclcpp::Parameter("test_channel.interval_2.y_end", 0.0),
+        rclcpp::Parameter("test_channel.interval_2.t_mid", 0.0),
+        rclcpp::Parameter("test_channel.interval_2.y_mid", 0.0)
+    });
+
+    std::vector<std::string> channels = { "test_channel" };
+    Simulator sim(node.get(), channels);
+
+    // Only first 2 intervals should be loaded
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 1.0);  // First interval
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 2.0); // Second interval
+    EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 25.0), 2.0); // Third interval should not exist
+}
+
+int main(int argc, char** argv) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

src/g2_2025_odometry_pkg/test/TestWheelSimulator.cpp

@@ -0,0 +1,168 @@
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include "g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.hpp"
+#include <memory>
+
+using namespace assignments::three::wheel_data_simulator_node;
+
+class WheelSimulatorTest : public ::testing::Test {
+protected:
+    static void SetUpTestSuite() {
+        rclcpp::init(0, nullptr);
+    }
+
+    static void TearDownTestSuite() {
+        rclcpp::shutdown();
+    }
+
+    void setupBasicWheelParams(rclcpp::Node* node) {
+        node->declare_parameter("max_intervals", 4);
+        node->declare_parameter("publish_rate", 10.0);
+
+        // Setup wheel_fl
+        node->declare_parameter("wheel_fl.num_intervals", 1);
+        node->declare_parameter("wheel_fl.interval_0.type", "linear");
+        node->declare_parameter("wheel_fl.interval_0.t_start", 0.0);
+        node->declare_parameter("wheel_fl.interval_0.t_end", 10.0);
+        node->declare_parameter("wheel_fl.interval_0.y_start", 0.0);
+        node->declare_parameter("wheel_fl.interval_0.y_end", 5.0);
+        node->declare_parameter("wheel_fl.interval_0.t_mid", 0.0);
+        node->declare_parameter("wheel_fl.interval_0.y_mid", 0.0);
+
+        // Setup other wheels with no intervals
+        for (const auto& wheel : {"wheel_fr", "wheel_bl", "wheel_br"}) {
+            node->declare_parameter(std::string(wheel) + ".num_intervals", 0);
+        }
+    }
+};
+
+// Test node initialization
+TEST_F(WheelSimulatorTest, NodeInitialization) {
+    auto node = std::make_shared<rclcpp::Node>("test_wheel_init_node");
+    setupBasicWheelParams(node.get());
+
+    // Should not throw
+    EXPECT_NO_THROW({
+        DataSimulator sim;
+    });
+}
+
+// Test wheel message publishing
+TEST_F(WheelSimulatorTest, WheelDataPublishing) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_wheel_pub_node");
+
+    std_msgs::msg::Float64MultiArray::SharedPtr received_msg;
+    auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
+        "simulated_wheel_data",
+        10,
+        [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) {
+            received_msg = msg;
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin a few times to allow publishing
+    for (int i = 0; i < 5; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    // Check that we received a message
+    ASSERT_NE(received_msg, nullptr);
+}
+
+// Test wheel data array size
+TEST_F(WheelSimulatorTest, WheelDataArraySize) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_wheel_size_node");
+
+    std_msgs::msg::Float64MultiArray::SharedPtr received_msg;
+    auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
+        "simulated_wheel_data",
+        10,
+        [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) {
+            if (!received_msg) {
+                received_msg = msg;
+            }
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin until we receive a message
+    for (int i = 0; i < 10 && !received_msg; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    ASSERT_NE(received_msg, nullptr);
+    // Should have 4 wheel values: FL, FR, RL, RR
+    EXPECT_EQ(received_msg->data.size(), 4);
+}
+
+// Test wheel velocity values are finite
+TEST_F(WheelSimulatorTest, ValidVelocityValues) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_wheel_values_node");
+
+    std_msgs::msg::Float64MultiArray::SharedPtr received_msg;
+    auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
+        "simulated_wheel_data",
+        10,
+        [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) {
+            if (!received_msg) {
+                received_msg = msg;
+            }
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin until we receive a message
+    for (int i = 0; i < 10 && !received_msg; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    ASSERT_NE(received_msg, nullptr);
+    ASSERT_EQ(received_msg->data.size(), 4);
+
+    // All values should be finite
+    for (const auto& val : received_msg->data) {
+        EXPECT_TRUE(std::isfinite(val));
+    }
+}
+
+// Test multiple wheel messages received
+TEST_F(WheelSimulatorTest, MultipleMessagesReceived) {
+    auto test_node = std::make_shared<rclcpp::Node>("test_multi_wheel_node");
+
+    int msg_count = 0;
+    auto subscription = test_node->create_subscription<std_msgs::msg::Float64MultiArray>(
+        "simulated_wheel_data",
+        10,
+        [&msg_count](std_msgs::msg::Float64MultiArray::SharedPtr) {
+            msg_count++;
+        }
+    );
+
+    auto sim_node = std::make_shared<DataSimulator>();
+
+    // Spin for a bit to receive messages
+    for (int i = 0; i < 10; i++) {
+        rclcpp::spin_some(sim_node);
+        rclcpp::spin_some(test_node);
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    // Should have received at least one message
+    EXPECT_GT(msg_count, 0);
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}