Fixed a bug in the BreadthFirstPlanner