Optimize costmap2d publisher performance

[mini-1235]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Related ticket is #4914
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo sim
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

Motivated by the ticket, I did a quick profiling and tried to optimize the performance, for those loops using translation maps, I suggest to use std::transform, it is a 2-5x speed up, for the update ones, I suggest to get the costmap pointer instead of calling getcost(x,y), also a copy_n is used in the inner loop, which is around 10x speed up

Description of documentation updates required from your changes

Description of how this change was tested

Run on the unit test and my simulation

---

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

Attention: Patch coverage is 38.46154% with 8 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_costmap_2d/src/costmap_2d_publisher.cpp	38.46%	8 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_costmap_2d/src/costmap_2d_publisher.cpp	60.66% <38.46%> (-0.88%)	⬇️

... and 1 file with indirect coverage changes