[navfn_planner]Optimizations for the navfn planner in large map scenarios

[XinyuKhan]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble binaries
• Version or commit hash:
  • 77e4386
• DDS implementation:
  • Fast-RTPS

Additional information

Navfn became slow when I use a large map in the simulation

[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

---

Feature request

Feature description

Implementation considerations