diff --git a/doc/architecture/nodes/HardwareInterface.md b/doc/architecture/classes/HardwareInterface.md similarity index 100% rename from doc/architecture/nodes/HardwareInterface.md rename to doc/architecture/classes/HardwareInterface.md diff --git a/doc/architecture/classes/Simulator.md b/doc/architecture/classes/Simulator.md new file mode 100644 index 0000000..95e56c7 --- /dev/null +++ b/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& 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& 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 | +| `.num_intervals` | int | Number of intervals for this object | +| `.interval_.type` | string | "constant", "linear", or "quadratic" | +| `.interval_.t_start` | double | Interval start time | +| `.interval_.t_end` | double | Interval end time | +| `.interval_.y_start` | double | Value at start | +| `.interval_.y_end` | double | Value at end | +| `.interval_.t_mid` | double | Mid-point time (quadratic) | +| `.interval_.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) \ No newline at end of file diff --git a/doc/architecture/nodes/IMUDataSimulator.md b/doc/architecture/nodes/IMUDataSimulator.md new file mode 100644 index 0000000..298bfd1 --- /dev/null +++ b/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 | +| `.num_intervals` | int | 0 | Number of intervals for each axis | +| `.interval_.*` | 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 \ No newline at end of file diff --git a/doc/architecture/nodes/WheelDataSimulator.md b/doc/architecture/nodes/WheelDataSimulator.md new file mode 100644 index 0000000..bbce437 --- /dev/null +++ b/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 | +| `.num_intervals` | int | 0 | Number of intervals for each wheel | +| `.interval_.*` | 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 diff --git a/doc/tests/IMUDataSimulator.md b/doc/tests/IMUDataSimulator.md new file mode 100644 index 0000000..bc5ebad --- /dev/null +++ b/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 | diff --git a/doc/tests/Simulator.md b/doc/tests/Simulator.md new file mode 100644 index 0000000..dbffc43 --- /dev/null +++ b/doc/tests/Simulator.md @@ -0,0 +1,98 @@ +# Simulator Unit Tests + +Unit tests for `Simulator` 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 diff --git a/doc/tests/WheelDataSimulator.md b/doc/tests/WheelDataSimulator.md new file mode 100644 index 0000000..852efa5 --- /dev/null +++ b/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) |