Path Planner efficiency tweak