Merge pull request '[PR] Implement IMU location approximator' (#15) from 3-odometry/imu-position-approximator into 3-odometry/master

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

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/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/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/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/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) |

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();
+}