ros2 / rclc

ROS Client Library for the C language.
Apache License 2.0
118 stars 42 forks source link

[rclc/rclc_executor]: subscribe loaned message #394

Open Zard-C opened 1 year ago

Zard-C commented 1 year ago

Background

When I was writting example codes that using loaned message with rclc interfaces (pub/sub) , the publish_with_loaned_msg demo works fine, but then I found out that there's no such implementation in rclc_executor which allow executor to take loaned subscription message from rmw. So I added some codes to executor.c:

It works with my test code subscription_with_loaned_message

Discussion

  1. The draft used rcl_take_loaned_message to take loaned msg from rmw, the question is, when and where to return it to rmw? I tried to return in sub callback, unfortunately, the return value was RCL_RET_ERROR This is because I used another librclc.so, a big mistake for me. 😇
  2. The draft is only a attempt to use loaned message with rclc's subcription interface, and a lot of things need to be considered, do you have better ideas? CC @JanStaschulat @fujitatomoya @pablogs9

Thanks in advance!

JanStaschulat commented 1 year ago

@Zard-C thanks for proposing to use loaned messages also in rclc. Out of my head, I have no explanation, why it failed to return the message. We would have to check the implementation of the return-method in RMW layer.

Typically, I would use loaned-messages for pipelined processing, e.g. to pass the message data to multiple callbacks or threads without the need to copy the data (as the scope of the message data is not limited to the subscription callback).

What is your use-case for loaned messages?

Zard-C commented 1 year ago

@JanStaschulat Thanks Jan, before trying to use loaned message with rclc, I took a look at rclcpp::executor's implementation(code here), it shows where and when to take and return loaned message. After checking my codes and steps to generate that error, I found out that the RCL_RET_ERROR was casued by myself, I used the rolling version(not the modifed one) of rclc to do the test. 😇

By the way, my use-case of loaned message is:

Publish and subscribe a large but fixed(bounded) message in different nodes, a fixed picture generated by a camera, which may up to Mbytes. I believed that using loaned message would reduce data copying.

fujitatomoya commented 1 year ago

@Zard-C several things i would like to share with you for what you are trying to do here.

to see those conditions, we have interface to check if can we use loaned interfaces for this publisher or subscription,

https://github.com/ros2/rcl/blob/45fc9520216013007c2d4f3bd69d778af9051aa3/rcl/include/rcl/publisher.h#L684-L691

https://github.com/ros2/rcl/blob/45fc9520216013007c2d4f3bd69d778af9051aa3/rcl/include/rcl/subscription.h#L869-L879

these are called in rclcpp before calling rcl_take_loaned_message to check the condition. and if it fails, it falls back to the default behavior which is RMW returns the copy of the data from own memry data.

be advised that using loaned interfaces are tricky, this is all because of the memory ownership.

https://github.com/ros2/rcl/blob/45fc9520216013007c2d4f3bd69d778af9051aa3/rcl/include/rcl/subscription.h#L643-L644

i want to point out above constraint. this says, once the message is returned via rcl_return_loaned_message, we cannot keep the data reference since the physical memory is owned by the RMW implementation. if the application keeps the pointer somewhere in the application for later use, the data would not be valid because RMW implementation needs to reuse the memory pool it owns. so if the application is using the loaned interface, the above constraints must be applied. instead, data corruption can be seen in the application.

further more, loaned interfaces are the memory management between ROS 2 and RMW implementation endpoints. it does not say it achieve the zero copy between the endpoints underneath implementation. if your final target is to support zero copy data sharing, you need to enable RMW implementation data sharing feature. you can see more details for https://github.com/ros2/rmw_fastrtps#enable-zero-copy-data-sharing.

i hope this helps some, if you have any questions, let me know. I am positive for this feature enabled in rclc as well.