Update costmap publisher and subscriber to allow for 'update' messages in raw costmaps

[SteveMacenski]

Currently, all costmap publisher / subscription when using raw messages (e.g. costmap messages containing the raw costmap from 0-255 rather than the occupancy grid message of 0-100 for internal use by the task servers such as behavior and smoother) send the full costmap each time. This is unideal, even though we compose all our nodes into the same process, which does help greatly with compute time.

It would be better if we mirrored the Update methods from the OccupancyGrid publication to minimize the number of times a full, raw, costmap is sent over the network and only publish the updated regions. This would decrease compute resources and latency for the behavior and smoother server (or other servers for client applications using the raw costmaps).

This is a great first issue for a new contributor! Just make sure to look over the code and make sure that all instances of costmap pub/sub are using these classes to make sure that everyone can properly handle the updates (which I believe they all should be, minus some unit tests)

[Relativiteit]

@SteveMacenski I would like to make this one of my first contributions!

[SteveMacenski]

Great! Any questions you might have :-) The file I point to should pretty much have the "core basics" of demonstration of how such a feature could work using the OccupancyGrid update logic. Its mostly mirroring that for the nav2_msgs/Costmap message type and updating the publisher and subscriber to work with it. Then, checking uses of the Costmap message to make sure everyone is using the pub/sub objects to get those updates -- in case there are some raw publishers / subscribers hanging around!

Thanks and I look forward!

[SteveMacenski]

@Relativiteit any update? :-)

[emsko]

I would like to take over this issue.

Just to recap, you think about mimicking this behaviour costmap_updatepub for _costmap_raw_pub__ .

So creating CostmapUpdate message and adding proper publisher and subscriber.

[SteveMacenski]

And the counterpart in the costmap subscriber as well to take those published updates and populate the raw costmap!

Save on that networking traffic overhead :-)

[Relativiteit]

hey guys yes, I got a bit lost on how to do it please @emSko take it over this was a bit above my head. I would still like to try it this weekend. And make a PR but please take point on it.

[SteveMacenski]

@emSko any updates or thoughts? How's it going :smile:

[emsko]

@SteveMacenski I've implemented the publisher but have no good ideas for the subscriber's implementation. I would like to talk it over during tonight's meeting, is that ok?

[SteveMacenski]

OK!

[SteveMacenski]

https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/plugins/static_layer.cpp#L290

https://github.com/ros-visualization/rviz/blob/noetic-devel/src/rviz/default_plugin/map_display.cpp#L570

[SteveMacenski]

Hi @emSko ! How are things going?

[emsko]

Hi @SteveMacenski

I have added the subscriber implementation however, it required adding some mutexes, which might have affected the overall performance. I've checked on the default map in the first run example and the size of the local Costmap update remained the same, but the size of an update of the global Costmap became 10 times smaller compared with the whole map (I am talking about the arrays of data in messages).

I don't have much experience with profiling, but I was able to run Valgrind on some nodes, and probably it would be a good idea to check the performance with Callgrind. I will probably need some assistance with the interpretation of the results.

Now some tests are broken. I struggle to run them with verbose output to analyze them. Also, I could not find the equivalent to the rostest command in ros2. Maybe you can provide some tips and commands to work with tests?

You can look at the (almost) most current version of the implementation here: 3732_costmap_raw_update_msgs I see that I need to update my fork and test if this still works.

[SteveMacenski]

I took a brief look at it and from a first look, the publisher seems good. Some notes, but nothing huge!

but the size of an update of the global Costmap became 10 times smaller compared with the whole map

Assuming you're talking about the update: That's good news! That's exactly what it should be doing. This should be only publishing the part of the map that has changed from new sensor data processing rather than sending the full size every single time. The first map published (if the logic is right) on the main topic should be the full map, and subsequent updates on the _update topic should be much smaller!

This sounds like good news

For testing:

colcon test --packages-select <your package> and to see the results once complete colcon test-result --verbose to see the failed results.

[SteveMacenski]

Merging imminently! Thanks @emSko !