3732 costmap raw update msgs rebased (fixes github issues with PR #3948)

[emsko]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3732
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation

---

Description of contribution in a few bullet points

• Adding new msg type CostmapUpdate.msg,
• Adding CostmapUpdate publisher to Costmap2DPublisher class,
• Re-implementation of the CostmapSubscriber class to support the CostmapUpdate msg.

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

• I see a lot of redundancy in Costmap2DPublisher class while creating the msgs, adding methods that are responsible for populating msgs and return the unique ptr to msgs might improve the code readability

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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: 22 lines in your changes are missing coverage. Please review.

Comparison is base (c5eddb3) 90.32% compared to head (770e2be) 90.08%.
Report is 4 commits behind head on main.

Files	Patch %	Lines
nav2_costmap_2d/src/costmap_2d_publisher.cpp	62.74%	19 Missing ⚠️
nav2_costmap_2d/src/costmap_subscriber.cpp	94.54%	3 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3965      +/-   ##
==========================================
- Coverage   90.32%   90.08%   -0.25%     
==========================================
  Files         415      415              
  Lines       18462    18513      +51     
==========================================
+ Hits        16676    16677       +1     
- Misses       1786     1836      +50     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[emsko]

This is the continuation of work from #3948

[SteveMacenski]

Otherwise, beyond tests, this looks good :-)

[SteveMacenski]

Overall looks good / done to me! The costmap publisher seems to be missing coverage in several entire functions though: https://app.codecov.io/gh/ros-planning/navigation2/pull/3965/blob/nav2_costmap_2d/src/costmap_2d_publisher.cpp

The subscribers looks perfect though - a few lines missing but nothing major and all for error cases

[emsko]

@SteveMacenski the lines that are missing coverage are not related to the functionality implemented in this issue (such as grid publishing and costmap service). I feel that lines were missing the coverage before my changes and the tests should be added in different PR

[SteveMacenski]

OK - still heed the underscore comment above

[SteveMacenski]

@emsko last thing to merge this: Add new feature to the migration guide! https://navigation.ros.org/migration/Iron.html

Also, please do a run of the system and add some prints to make sure its working in production and not just sending the full costmap except on initialization (e.g. update callback is triggered at runtime mostly). Do you see any major computation changes in the runtime performance of the system? I suspect this change may actually be visible in metrics

[SteveMacenski]

Any metrics changes?

[emsko]

@SteveMacenski I have run the simulation with the default map for Turtlebot3 and unfortunately, I could not observe any noticeable change in performance.

Full costmap not navigating. Average CPU time: 28.294999999999995, average memory 1.4000000000000004.

Full costmap navigating. Average CPU time: 49.1, average memory 1.3640000000000003.

Update not navigating. Average CPU time: 29.507, average memory 1.4000000000000004.

Update navigating. Average CPU time: 50.132, average memory 1.3770000000000002. 

Not navigating means that the robot was just standing still, navigating means that the robot was navigating to goals. These are the measurements for a composable node after collecting 100 samples for each case.

Then with help from Alexey, I created a synthetic map of size 1000x1000 px, however, I was not able to collect the measurements because the simulation kept crashing on my laptop, but by just eyeballing the top command I would say that there still were not any observable changes.

[SteveMacenski]

OK! Thanks for testing. Are you sure that the updates callbacks are being triggered in the subscription (and I guess by extension that they're being sent by the publisher)? The behavior server is the easiest testing point for this as that's an external user of the raw costmaps. I just want to make sure that functionally things are working as expected if there's no compute change. A few print statements can illustrate this easily.

Then, happy to merge :-)

[emsko]

I've checked the functionality with the ros2 topic echo command. It works as intended that is the full costmap is latched on the topic and only the updates are sent continuously. I also have implemented the helper node, as you suggested, that is converting the raw msgs into grid msgs to visualize it in rviz - it shows that everything is correct.

From my analysis, I have found that it does not make sense to use the update msg for the local costmap. Due to the change of the origin while the robot is moving, the full costmap is sent and the updates are only sent when the robot is standing still. Also, the size of the full costmap is insignificantly larger than the update (3600 cells vs ~3300 cells). However, in the case of the global costmap, we are dealing with the decrease of the number of cells ~10 times (when running the simulation of the default turtlebot map/world, for larger maps the difference is larger).

[SteveMacenski]

That makes sense for the local vs global - it is more geared towards global. Anything more we should do before merging?

[emsko]

I have nothing to add at the moment. As for me, the changes can be merged at the current state.

[SteveMacenski]

Great! Thanks so much for the contribution! This is really nice to have and probably not something I would have gotten to for a long, long time. Any interest in other projects potentially? We got a whole bunch of new tickets in the last couple of weeks or happy to hear about what you might be interested in working on / learning about to suggest something else 😄

[emsko]

Thank you, Steven, for all the comments and suggestions.
I was thinking about working on the AI Integration Layers that are listed on the nav2 user survey. Could you provide me with more info about that on Slack, or can we discuss it during the working group meeting?