navfn : fix performance issue (#4945)

Signed-off-by: Yancey <135342560+Yancey2023@users.noreply.github.com>

Author: Yancey2023

nav2_navfn_planner/src/navfn_planner.cpp

@@ -293,11 +293,13 @@ NavfnPlanner::makePlan(
       p.position.x = goal.position.x - tolerance;
       while (p.position.x <= goal.position.x + tolerance) {
         potential = getPointPotential(p.position);
-        double sdist = squared_distance(p, goal);
-        if (potential < POT_HIGH && sdist < best_sdist) {
-          best_sdist = sdist;
-          best_pose = p;
-          found_legal = true;
+        if (potential < POT_HIGH) {
+          double sdist = squared_distance(p, goal);
+          if (sdist < best_sdist) {
+            best_sdist = sdist;
+            best_pose = p;
+            found_legal = true;
+          }
         }
         p.position.x += resolution;
       }