Fix bug in path planner