[FEATURE]: Implement trajectory planner

[evan-palmer]

Feature Type

Adding new functionality to Angler

Problem Description

The current framework utilizes general waypoint planning for task execution. While this may be fine for a baseline system, it doesn't provide an optimal solution and can be challenging to extend when using a planner in energetic conditions.

Feature Description

Implement an online trajectory planner for generating trajectories through a sequence of setpoints planned by a high-level planner. This should be fed to a kinematic controller which should perform interpolation and trajectory tracking.

Alternative Solutions

need food

Additional Context

No response