Fix bug where start/goal swap left goal on open set