Support stamped footprints in costmap_2d

[Sushant-Chavan]

---

Basic Info

Info	Please fill out this column
Primary OS tested on	(Ubuntu)
Robotic platform tested on	Propreitary
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• This PR adds a parameter to choose between Polygon/PolygonStamped subscriber for receiving the footprint in the Costmap2DROS.
• By default Costmap2DROS subscribes to Polygon footprints
• PolygonStamped footprints can be subscribed to by setting the subscribe_to_stamped_footprint parameter to True
• Also includes some cleanup, tiny optimizations, and minor refactoring to parts of code in the vicinity of the above changes

Description of documentation updates required from your changes

None

Description of how this change was tested

Tested that the Costmap2DROS can receive and properly process PolygonStamped footprint messages on a proprietary robot

---

Future work that may be required in bullet points

N/A

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
• Should this be backported to current distributions? If so, tag with backport-*.

[codecov]

Codecov Report

Attention: Patch coverage is 80.00000% with 4 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_costmap_2d/src/costmap_2d_ros.cpp	55.55%	4 Missing ⚠️

Files with missing lines	Coverage Δ
...tmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp	100.00% <ø> (ø)
nav2_costmap_2d/src/footprint.cpp	97.53% <100.00%> (+0.06%)	⬆️
nav2_costmap_2d/src/footprint_subscriber.cpp	100.00% <100.00%> (ø)
nav2_costmap_2d/src/costmap_2d_ros.cpp	86.54% <55.55%> (-1.10%)	⬇️

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

No issue here. In the future though, maybe we should just make a separate topic and just subscribe to both. For Costmap2D though I'm not all that sensitive to changes like this, we need a major refactor either way.