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

@@ -10,17 +10,70 @@ find_package(ament_cmake REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
+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_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+  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

@@ -11,6 +11,7 @@
 
   <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>

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