(Hopefully) fixes path planning stuck issues