Fix and optimize findPathFurthestReachedPoint#5698

tonynajjar wants to merge 1 commit intoros-navigation:mainros-navigation/navigation2:mainfrom

tonynajjar:furthest-point-optitonynajjar/navigation2:furthest-point-optiCopy head branch name to clipboard