Optimizations for the navfn planner in large map scenarios

[XinyuKhan]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #4243)
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of Tally
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

1. Avoid repetitive memory allocation and deallocation;
2. Avoid unnecessary map traversal;
3. Use std::memset or std::fill instead of a for loop.

before change:

[component_container_isolated-6] [INFO] [1712419510.305826643] [bt_navigator]: Begin navigating from current location (1.53, -0.61) to (-2.33, 0.19)
[component_container_isolated-6] [WARN] [1712419510.396399095] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.0735 Hz
[component_container_isolated-6] [INFO] [1712419510.416545751] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [WARN] [1712419511.504568379] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.7862 Hz
[component_container_isolated-6] [INFO] [1712419511.516851913] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419512.612415462] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.6330 Hz
[component_container_isolated-6] [INFO] [1712419512.666831014] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419513.721952562] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.7125 Hz
[component_container_isolated-6] [INFO] [1712419513.766835490] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419514.831260982] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.7334 Hz
[component_container_isolated-6] [INFO] [1712419514.866702982] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419515.942198139] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.6323 Hz
[component_container_isolated-6] [INFO] [1712419515.966687909] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419517.057747510] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.9449 Hz
[component_container_isolated-6] [INFO] [1712419517.116698077] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419518.239859170] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 6.5029 Hz
[component_container_isolated-6] [INFO] [1712419518.266711146] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419519.358471549] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.8443 Hz
[component_container_isolated-6] [INFO] [1712419519.416706815] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419520.480007637] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.6521 Hz
[component_container_isolated-6] [INFO] [1712419520.516705441] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419521.606562860] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.9695 Hz
[component_container_isolated-6] [INFO] [1712419521.666690680] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419522.740269989] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.6144 Hz
[component_container_isolated-6] [INFO] [1712419522.766692174] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419523.852446068] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.5828 Hz
[component_container_isolated-6] [INFO] [1712419523.866674268] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419524.967602968] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.9646 Hz
[component_container_isolated-6] [INFO] [1712419525.016711211] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419526.122193143] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 7.9463 Hz
[component_container_isolated-6] [INFO] [1712419526.166718255] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419527.255724271] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 9.1580 Hz
[component_container_isolated-6] [INFO] [1712419527.266730648] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419528.345932597] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 12.5242 Hz
[component_container_isolated-6] [INFO] [1712419528.366703842] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419529.469556256] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 10.7167 Hz
[component_container_isolated-6] [INFO] [1712419529.516679885] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419530.583000957] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 11.5234 Hz
[component_container_isolated-6] [INFO] [1712419530.616703278] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419531.020662321] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712419531.055845047] [bt_navigator]: Goal succeeded

after change

[component_container_isolated-6] [INFO] [1712419809.126727115] [bt_navigator]: Begin navigating from current location (-2.11, -0.15) to (1.75, -0.65)
[component_container_isolated-6] [WARN] [1712419809.178561268] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 19.4695 Hz
[component_container_isolated-6] [INFO] [1712419809.197766687] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [WARN] [1712419810.260866628] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.7728 Hz
[component_container_isolated-6] [INFO] [1712419810.298255490] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419811.348251044] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419812.448262406] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419813.498260756] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419814.548278205] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419815.647936526] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419816.697943675] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419817.764847469] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 17.2670 Hz
[component_container_isolated-6] [INFO] [1712419817.797905056] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419818.847910680] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419819.947894103] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419820.997894306] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [WARN] [1712419822.053832724] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 17.6348 Hz
[component_container_isolated-6] [INFO] [1712419822.097913229] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419823.147923333] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419824.197926737] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419825.297902760] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419826.347922785] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419827.447926615] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1712419827.652004457] [controller_server]: Reached the goal!
[component_container_isolated-6] [INFO] [1712419827.686569069] [bt_navigator]: Goal succeeded

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• [ ] Check that any new parameters added are updated in navigation.ros.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