Basic Info

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-*.