nav2_costmap_2d: add TypeAdapter for Costmap2DStamped to enable copy-free pub/sub

[redddddyyyyy]

Basic Info

Info | Please fill out this column -- | -- Ticket(s) this addresses | #4829 Primary OS tested on | Ubuntu 22.04 (ROS 2 Humble) Robotic platform's tested on | N/A (unit/integration tests only) Does this PR contain AI generated software? | Yes (assisted), reviewed and edited by me Was this PR description generated by AI software? | No

Description of contribution in a few bullet points

• Add ROS 2 TypeAdapter between nav2_costmap_2d::Costmap2DStamped and nav2_msgs::msg::Costmap

• Update costmap publisher/subscriber to use the adapter path to avoid extra message copies

• Update nav2_smoother + nav2_constrained_smoother tests to publish/consume stamped costmap via the adapter

Description of documentation updates required

• None

Description of how this change was tested

• colcon build --symlink-install --packages-select nav2_costmap_2d nav2_smoother nav2_constrained_smoother

• colcon test --packages-select nav2_costmap_2d nav2_smoother nav2_constrained_smoother --event-handlers console_direct+

• colcon test --packages-select nav2_smoother nav2_constrained_smoother --ctest-args -R "cpplint|uncrustify" -V --output-on-failure

Notes

• PR is intentionally targeting main per project policy; Humble backport can be done after merge if needed.

[mini-1235]

@redddddyyyyy, thanks for picking up the issue. Unfortunately, this is not building at the moment. I assume this was tested on the Humble branch?

There are quite a few differences between humble and main. For example, the correct type here should be nav2::LifecycleNode rather than nav2_util::LifecycleNode. These build issues will need to be resolved before we can do a full review :)

Additionally, it would be great to highlight the CPU usage benefits of this change

[SteveMacenski]

Also: does this impact performance? I still see a copy so does this really improve CPU / latency meaningfully?

[roncapat]

Also: does this impact performance? I still see a copy so does this really improve CPU / latency meaningfully?

Hi, my two cents here. If you are referring to the copy inside the Type Adapter from/to_custom_message methods, those ones are called only when intra-process is disabled. Otherwise it is mostly a matter of passing around pointers.

About the choice of the "native" data structure - the costmap data is not "owned" but only a shared pointer is kept - this may be checked so that any subscribing node may NOT edit (write) to the costmap, otherwise that modification directly impacts all other nodes - the publishing one and all other subscribers. It is just something to check and maybe "secure" with some getters returing const pointers and moving the costmap as private member, of stuff like that IMHO.