feat(params & docs): added wheelposaprox params to yaml & node arch docs

doc/architecture/nodes/WheelPositionApproximator.md

@@ -0,0 +1,151 @@
+# WheelPositionApproximator (`assignments::three::wheel_position_approximator_node`)
+
+## Overview
+The `WheelPositionApproximator` is a ROS2 node that computes 2D odometry for a mecanum-wheeled robot by integrating wheel encoder velocities over time. It subscribes to simulated or real wheel angular velocity data, applies kinematic equations for mecanum drive systems, and publishes estimated position and orientation as standard ROS odometry messages. This node is essential for robot localization, navigation, and trajectory tracking in wheeled mobile robot applications.
+
+## Implementation Details
+
+### Parameters
+
+- **`wheel_radius`** (double, default: 0.2): Radius of the mecanum wheels in meters. This converts angular velocity (rad/s) to linear velocity (m/s).
+- **`lx`** (double, default: 0.3): Half-distance along the x-axis from the robot center to the wheel in meters. Used for angular velocity calculations.
+- **`ly`** (double, default: 0.6): Half-distance along the y-axis from the robot center to the wheel in meters. Used for angular velocity calculations.
+- **`initial_x`** (double, default: 0.0): Initial x-position of the robot in the global frame (meters).
+- **`initial_y`** (double, default: 0.0): Initial y-position of the robot in the global frame (meters).
+- **`initial_theta`** (double, default: 0.0): Initial orientation angle of the robot in radians.
+- **`time_elapsed`** (double, default: 1.0): Integration time step in seconds for position updates.
+
+### Constructor
+```cpp
+WheelPositionApproximator()
+```
+- Initializes ROS2 node with name `wheel_position_approximator`
+- Declares and reads all configuration parameters
+- Initializes state variables (wheel velocities, position, orientation) to zero or configured values
+- Creates a subscriber to `simulated_wheel_data` topic for receiving wheel angular velocities
+- Creates a publisher to `car_position` topic for publishing odometry messages
+- Sets up a 100ms timer (10 Hz) for periodic odometry computation and publishing
+- Logs initialization status and parameter values
+
+### Core Functions
+
+#### Velocity Calculation Methods
+
+**`double calculate_V_x()`**
+- Computes the robot's forward velocity in the robot's local x-axis
+- **Mecanum kinematics formula**:
+  ```
+  V_x = r * (ω_fl + ω_fr + ω_rl + ω_rr) / 4
+  ```
+  Where `ω` represents angular velocity of each wheel in rad/s
+- Returns linear velocity in m/s
+
+**`double calculate_V_y()`**
+- Computes the robot's lateral (sideways) velocity in the robot's local y-axis
+- **Mecanum kinematics formula**:
+  ```
+  V_y = r * (-ω_fl + ω_fr + ω_rl - ω_rr) / 4
+  ```
+- Returns linear velocity in m/s
+- Enables omnidirectional movement characteristic of mecanum wheels
+
+**`double calculate_V_theta()`**
+- Computes the robot's angular velocity (rotation rate)
+- **Mecanum kinematics formula**:
+  ```
+  ω_theta = r * (-ω_fl + ω_fr - ω_rl + ω_rr) / (4 * (lx + ly))
+  ```
+- Returns angular velocity in rad/s
+- Depends on robot geometry (lx and ly parameters)
+
+#### Position Integration
+
+**`double normalize_angle(double angle)`**
+- Normalizes an angle to the range [-π, π]
+- Prevents angle accumulation from growing unbounded
+- Uses iterative wrapping:
+  ```cpp
+  while (angle > M_PI) angle -= 2.0 * M_PI;
+  while (angle < -M_PI) angle += 2.0 * M_PI;
+  ```
+
+**`void calculate_position()`**
+- Core odometry integration function, called every timer cycle
+- **Algorithm**:
+  1. Calculates current velocities (vx, vy, vtheta) from wheel encoder data
+  2. Updates orientation: `theta += vtheta * dt`
+  3. Normalizes theta to [-π, π] range
+  4. Transforms local velocities to global frame using rotation matrix:
+     ```
+     delta_x = (vx * cos(theta) - vy * sin(theta)) * dt
+     delta_y = (vx * sin(theta) + vy * cos(theta)) * dt
+     ```
+  5. Updates global position: `x += delta_x`, `y += delta_y`
+- Logs current position and orientation (in both radians and degrees)
+
+#### Publishing
+
+**`void publish_odometry()`**
+- Timer callback function (triggered at 10 Hz)
+- Calls `calculate_position()` to update internal state
+- Creates `nav_msgs::msg::Odometry` message with:
+  - **Header**: Current timestamp and frame IDs (`odom` → `base_link`)
+  - **Position**: Current (x, y, z=0) coordinates in meters
+  - **Orientation**: Converted from theta (yaw angle) to quaternion representation:
+    ```
+    q_z = sin(theta / 2)
+    q_w = cos(theta / 2)
+    ```
+    This represents 2D rotation around the z-axis
+- Publishes odometry message to `car_position` topic
+
+## Visualization
+
+### PlotJuggler
+Visualize 2D trajectory and orientation:
+```bash
+ros2 run plotjuggler plotjuggler
+# In PlotJuggler:
+# 1. Streaming → ROS2 Topic Subscriber
+# 2. Select /car_position
+# 3. Use XY Plot tab:
+#    - X: pose.pose.position.x
+#    - Y: pose.pose.position.y
+```
+
+## Usage Example
+
+### Basic Usage
+```bash
+# Launch the wheel position approximator
+ros2 run g2_2025_odometry_pkg wheel_position_approximator_node
+
+# In another terminal, check published odometry
+ros2 topic echo /car_position
+
+# Monitor update rate
+ros2 topic hz /car_position
+```
+
+### With Custom Parameters
+```bash
+ros2 run g2_2025_odometry_pkg wheel_position_approximator_node \
+  --ros-args \
+  -p wheel_radius:=0.15 \
+  -p lx:=0.25 \
+  -p ly:=0.5 \
+  -p time_elapsed:=0.1 \
+  -p initial_x:=1.0 \
+  -p initial_y:=2.0 \
+  -p initial_theta:=1.57
+```
+
+### With Wheel Data Simulator
+```bash
+# Launch complete odometry system
+ros2 launch g2_2025_odometry_pkg wheel_sim.launch.xml
+
+# The launch file starts:
+# - wheel_data_simulator_node (publishes simulated wheel velocities)
+# - wheel_position_approximator_node (computes odometry)
+```

src/g2_2025_odometry_pkg/config/opt.yaml

@@ -72,3 +72,13 @@ wheel_data_simulator:
         t_end: 10.0
         y_start: 0.0
         y_end: 10.0
+
+wheel_position_approximator:
+  ros__parameters:
+    wheel_radius: 0.2
+    lx: 0.3
+    ly: 0.6
+    time_elapsed: 1.0
+    initial_x: 0.0
+    initial_y: 0.0
+    initial_theta: 0.0

src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml

@@ -5,4 +5,9 @@
     <node pkg="g2_2025_odometry_pkg" exec="wheel_data_simulator_node" name="wheel_data_simulator" output="screen">
         <param from="$(var params_file)"/>
     </node>
+
+    <node pkg="g2_2025_odometry_pkg" exec="wheel_position_approximator_node" name="wheel_position_approximator" output="screen">
+        <param from="$(var params_file)"/>
+    </node>
+
 </launch>