Added some helpful methods to Dubins library
Added ability to construct UAVOrientation using degrees

not just radians
Added "Optimize angles" method to Wayset
Hierarchical planner uses optimize angles
WaypointPlanner's auto fix uses wayset's optimize angles