[AMCL] 9x initialisation (and reset) speed increase

[doisyg]

---

Basic Info

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

---

Description of contribution in a few bullet points

On VERY big maps, the method map_update_cspace of AMCL takes ages. Which makes AMCL long to start, or reset (when for instance changing a dynamic parameter needing reinit_laser = true)
3 tricks by order of impact:

• using std::queue instead of std::priority_queue. But this is why this is a draft, I cannot understand why std::priority_queue was needed in the first place (and which criteria it used for ordering), so I am not sure.
• avoiding one CellData copy with emplace instead of push.
• computing MAP_INDEX(map, i, j) once instead of twice.

On a big map, AMCL activation times drops from 22s to 2.4s on my machine.

Description of documentation updates required from your changes

Description of how this change was tested

---

Future work that may be required in bullet points

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

All modified and coverable lines are covered by tests ✅

Files with missing lines	Coverage Δ
nav2_amcl/src/map/map_cspace.cpp	85.24% <100.00%> (-5.94%)	⬇️

... and 2 files with indirect coverage changes