navfn : fix performance issue

[Yancey2023]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4945
Primary OS tested on	Ubuntu
Robotic platform tested on	reality
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

squared_distance(p, goal) should be calculate when potential < POT_HIGH.

Description of documentation updates required from your changes

none.

Description of how this change was tested

test in reality.

---

Future work that may be required in bullet points

none

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 3 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_navfn_planner/src/navfn_planner.cpp	0.00%	3 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_navfn_planner/src/navfn_planner.cpp	90.15% <0.00%> (-0.47%)	⬇️

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

• ❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[Yancey2023]

I think this PR should be ok. I don't know how to handle the blocking of this merging.

[SteveMacenski]

@Yancey2023 what performance change do you measure with this change? Moving one distance calculation I am surprised to hear has meaningful performance changes. Please provide some metrics to support this change if the goal is improving performance.

Sorry for the delay, I was on vacation and getting back up to speed

[Yancey2023]

@SteveMacenski I'm sorry for causing you confusion. Actually, this won't have a significant performance improvement, but it is indeed a small optimization. I need to track object, so I need to navigate to the nearest area to the destination when the destination is obstacle, which is implemented in Navfn. In the original method, for grids that are obstacles, the distance to the destination is also calculated, which is unnecessary. My improvement is to only calculate the distance when the destination is not obstacle.

[SteveMacenski]

Got it - what is the speed up that you actually see from this change in your application?