Bug fixed: now the RouteGenerator finds the path even if the goal point is near the wall