[✨FEAT✨]: Path planning

[csprono]

Is there an existing enhancement request for this?

• I have searched the existing feature requests

Description

Implement optimisation based path planning for a fixed time horizon.

Additional Information

No response

[Tanish29]

Stationary Velocity Pseudocode:
1: create a uniform set of Vx between 0.5 and 3.5
2: create a uniform set of Vx between 1.5 and 2.25 for drifting range
2: create a uniform set of steering angle values between -0.3 to 0.3 for each Vx
3: code up Vy derivative
4: code up angular acceleration derivative
5: using a python ODE solver and get outputs based on the initial conditions
6: code up the values in a msg format for input to trajectory generation function