Implement waypoint generation

[7ayushgupta]

@shubhamkorde @KrishnaAgr
Consider that you've decided you need to go from point A to point C via point B. Now if you input these coordinates to the controller, there is a high chance it would go linearly from one point to the other with jerks. So ideally, you make up a lot of points in between and follow them.

Reading material: Learn about splines, path interpolation, uuvsimulator's codebase has this implemented, contact me.