Move tryDirectPath method to pathPlanner