Add tests for waypoint guard
Add tests for waypoint guard

jhu-asco/aerial_autonomy,jhu-asco/aerial_autonomy,jhu-asco/aerial_autonomy,jhu-asco/aerial_autonomy,jhu-asco/aerial_autonomy,jhu-asco/aerial_autonomy