Use ROS 2 Type Masquerading to publish costmap messages to Behavior, Smoother, Docking servers without copies

[SteveMacenski]

Right now, the costmap publisher, subscriber model copies the costmap into a message, publishes it, and copies the contents back into a costmap for use. If we use type masquerading like cv2::Mat -> Image and pcl::PointCloudXYZ -> PointCloud2, then we can skip copies in conversions

[glitchhopcore]

Hi @SteveMacenski, I'd like to take up this issue,
From what I understand the idea is to optimize the costmap publisher-subscriber model by leveraging type masquerading to avoid redundant memory copies during serialization and deserialization of costmap data by directly mapping the costmap's data to the message buffer?

Currently, the publisher-subscriber model for costmaps involves multiple deep copies of the costmap

• The publisher copies data from the internal unsigned char * buffer into message structures (e.g., prepareGrid() and prepareCostmap() in the Costmap2DPublisher).
• The subscriber copies the message data back into a Costmap2D structure for processing and use (e.g., costmapCallback() in the CostmapSubscriber).

Here I can potentially directly use the costmap’s internal buffer (unsigned char * data) as the data source for publishing and subscribing, eliminating the redundant copies.

Please let me know if my understanding of the issue is correct and if my approach seems fine, or if there anything that I should reconsider?

[SteveMacenski]

Yes, that is correct! I think those are the major points to consider :-) That would only make 1 copy for the middleware.

It may be worth doing a profiling run on Nav2 and look at other hot-spots in the middleware comms that it may be valuable to do similar things. This is the largest internal-data source I'm aware of, but perhaps we could also subscribe to Cv2::Mats and Pcl::Pointclouds in the sensor processing layers in the costmap / collision monitor as another option for external users to hook into batteries included.

If you're thinking about similar things, the #4042 ticket might also be up your alley

[glitchhopcore]

Thanks, @SteveMacenski! I’ll proceed with implementing these changes for the costmap publisher-subscriber model. Performing a profiling run is a great idea, and extending type masquerading to include cv2::Mat and pcl::PointCloud in the sensor processing layer sounds like a good next step.

I’m also very interested in taking up #4042 after making progress on this issue. Thank you so much for the suggestion~

[SteveMacenski]

Fantastic! Do you have a sense of when you're going to be able to work on it and have some preliminary results :-)

[roncapat]

Hi, I did something similar at https://github.com/roncapat/ros2_native_adapters if you need to give a look at (or use & contribute) a relatively simple solution :)