TrajectoryPlanner improvements

Description of the task

A placeholder ticket for a set of improvements that can be made to the WIP trajectory planner.

[ ] TrajectoryPlanner::findTrajectory should be called with a shrinked (by robot radius) field boundary to avoid the robots from trying to path plan outside of the field
[ ] We should support dynamic obstacle avoidance. Currently we use AABB tree for fast collision detection, however, the downside of this method is that AABB trees do not nicely support moving obstacles without regenerating the tree.
- [ ] In the meantime, the performance of trajectory planner may improve if we only generate the AABB tree if the direct trajectory has collisions. i.e. No need for generating the entire tree if we're just returning the direct trajectory.
- [ ] #2986 is needed for enemy obstacles
[ ] TrajectoryPlanner::getTrajectoryWithCost can be cleaned up by delegating the task of either using a cached collision value or a new one to a separate class.
[ ] Consider updating TrajectoryPlanner::getTrajectoryWithCost to return early if trajectory cost is over some threshold
[x] #3105
[ ] Remove duplicated obstacle protos from being visualized in Play::get
[ ] MovePrimitive should return a StopPrimitive if there's an unavoidable collision with a high velocity. StopPrimitives are able to have the robot break to a halt faster than MovePrimitive setting the destination as the current position since the max motor deceleration that we allow is higher than the max overall robot deceleration.
[ ] Consider making sub_destination sticky between consecutive runs. Though this may be difficult since we do not store the state of the trajectory planner.
[ ] If there's a dangerous collision ahead, we should consider returning a STOP primitive instead of a move primitive
[ ] Oscillation between two sub destinations causing the robot to not make any progress towards its destination.

UBC-Thunderbots / Software

TrajectoryPlanner improvements #3104

Description of the task