Need to check obstacle queue when adding/removing obstacles
Need to check obstacle queue when adding/removing obstacles