Add support for loaned messages (#212)

Author: nnmm

README.md

@@ -17,6 +17,7 @@ Features and limitations
 The current set of features include:
 - Message generation
 - Support for publishers and subscriptions
+- Loaned messages (zero-copy)
 - Tunable QoS settings
 - Clients and services
 

examples/minimal_pub_sub/Cargo.toml

@@ -13,6 +13,14 @@ path = "src/minimal_subscriber.rs"
 name = "minimal_publisher"
 path = "src/minimal_publisher.rs"
 
+[[bin]]
+name = "zero_copy_subscriber"
+path = "src/zero_copy_subscriber.rs"
+
+[[bin]]
+name = "zero_copy_publisher"
+path = "src/zero_copy_publisher.rs"
+
 [dependencies]
 anyhow = {version = "1", features = ["backtrace"]}
 

examples/minimal_pub_sub/src/zero_copy_publisher.rs

@@ -0,0 +1,23 @@
+use anyhow::{Error, Result};
+use std::env;
+
+fn main() -> Result<(), Error> {
+    let context = rclrs::Context::new(env::args())?;
+
+    let node = rclrs::create_node(&context, "minimal_publisher")?;
+
+    let publisher =
+        node.create_publisher::<std_msgs::msg::rmw::UInt32>("topic", rclrs::QOS_PROFILE_DEFAULT)?;
+
+    let mut publish_count: u32 = 1;
+
+    while context.ok() {
+        let mut message = publisher.borrow_loaned_message()?;
+        message.data = publish_count;
+        println!("Publishing: {}", message.data);
+        message.publish()?;
+        publish_count += 1;
+        std::thread::sleep(std::time::Duration::from_millis(500));
+    }
+    Ok(())
+}

examples/minimal_pub_sub/src/zero_copy_subscriber.rs

@@ -0,0 +1,23 @@
+use std::env;
+
+use anyhow::{Error, Result};
+
+fn main() -> Result<(), Error> {
+    let context = rclrs::Context::new(env::args())?;
+
+    let mut node = rclrs::create_node(&context, "minimal_subscriber")?;
+
+    let mut num_messages: usize = 0;
+
+    let _subscription = node.create_subscription::<std_msgs::msg::UInt32, _>(
+        "topic",
+        rclrs::QOS_PROFILE_DEFAULT,
+        move |msg: std_msgs::msg::UInt32| {
+            num_messages += 1;
+            println!("I heard: '{}'", msg.data);
+            println!("(Got {} messages so far)", num_messages);
+        },
+    )?;
+
+    rclrs::spin(&node).map_err(|err| err.into())
+}

rclrs/src/node/publisher.rs

@@ -11,6 +11,9 @@ use std::sync::{Arc, Mutex};
 
 use rosidl_runtime_rs::{Message, RmwMessage};
 
+mod loaned_message;
+pub use loaned_message::*;
+
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
 // they are running in. Therefore, this type can be safely sent to another thread.
 unsafe impl Send for rcl_publisher_t {}
@@ -31,6 +34,9 @@ where
 {
     rcl_publisher_mtx: Mutex<rcl_publisher_t>,
     rcl_node_mtx: Arc<Mutex<rcl_node_t>>,
+    // The data pointed to by type_support_ptr has static lifetime;
+    // it is global data in the type support library.
+    type_support_ptr: *const rosidl_message_type_support_t,
     message: PhantomData<T>,
 }
 
@@ -62,7 +68,7 @@ where
     {
         // SAFETY: Getting a zero-initialized value is always safe.
         let mut rcl_publisher = unsafe { rcl_get_zero_initialized_publisher() };
-        let type_support =
+        let type_support_ptr =
             <T as Message>::RmwMsg::get_type_support() as *const rosidl_message_type_support_t;
         let topic_c_string = CString::new(topic).map_err(|err| RclrsError::StringContainsNul {
             err,
@@ -82,7 +88,7 @@ where
             rcl_publisher_init(
                 &mut rcl_publisher,
                 rcl_node,
-                type_support,
+                type_support_ptr,
                 topic_c_string.as_ptr(),
                 &publisher_options,
             )
@@ -92,6 +98,7 @@ where
         Ok(Self {
             rcl_publisher_mtx: Mutex::new(rcl_publisher),
             rcl_node_mtx: Arc::clone(&node.rcl_node_mtx),
+            type_support_ptr,
             message: PhantomData,
         })
     }
@@ -145,6 +152,57 @@ where
     }
 }
 
+impl<T> Publisher<T>
+where
+    T: RmwMessage,
+{
+    /// Obtains a writable handle to a message owned by the middleware.
+    ///
+    /// This lets the middleware control how and where to allocate memory for the
+    /// message.
+    /// The purpose of this is typically to achieve *zero-copy communication* between publishers and
+    /// subscriptions on the same machine: the message is placed directly in a shared memory region,
+    /// and a reference to the same memory is returned by [`Subscription::take_loaned_message()`][1].
+    /// No copying or serialization/deserialization takes place, which is much more efficient,
+    /// especially as the message size grows.
+    ///
+    /// # Conditions for zero-copy communication
+    /// 1. A middleware with support for shared memory is used, e.g. `CycloneDDS` with `iceoryx`
+    /// 1. Shared memory transport is enabled in the middleware configuration
+    /// 1. Publishers and subscriptions are on the same machine
+    /// 1. The message is a "plain old data" type containing no variable-size members, whether bounded or unbounded
+    /// 1. The publisher's QOS settings are compatible with zero-copy, e.g. the [default QOS][2]
+    /// 1. `Publisher::borrow_loaned_message()` and [`Subscription::take_loaned_message()`][1] are used
+    ///
+    /// This function is only implemented for [`RmwMessage`]s since the "idiomatic" message type
+    /// does not have a typesupport library.
+    ///
+    /// [1]: crate::Subscription::take_loaned_message
+    /// [2]: crate::QOS_PROFILE_DEFAULT
+    //
+    // TODO: Explain more, e.g.
+    // - Zero-copy communication between rclcpp and rclrs possible?
+    // - What errors occur when?
+    // - What happens when only *some* subscribers are local?
+    // - What QOS settings are required exactly? https://cyclonedds.io/docs/cyclonedds/latest/shared_memory.html
+    pub fn borrow_loaned_message(&self) -> Result<LoanedMessage<'_, T>, RclrsError> {
+        let mut msg_ptr = std::ptr::null_mut();
+        unsafe {
+            // SAFETY: msg_ptr contains a null ptr as expected by this function.
+            rcl_borrow_loaned_message(
+                &*self.rcl_publisher_mtx.lock().unwrap(),
+                self.type_support_ptr,
+                &mut msg_ptr,
+            )
+            .ok()?;
+        }
+        Ok(LoanedMessage {
+            publisher: self,
+            msg_ptr: msg_ptr as *mut T,
+        })
+    }
+}
+
 /// Convenience trait for [`Publisher::publish`].
 pub trait MessageCow<'a, T: Message> {
     /// Wrap the owned or borrowed message in a `Cow`.

rclrs/src/node/publisher/loaned_message.rs

@@ -0,0 +1,88 @@
+use crate::rcl_bindings::*;
+use crate::{Publisher, RclrsError, ToResult};
+
+use rosidl_runtime_rs::RmwMessage;
+
+use std::ops::{Deref, DerefMut};
+
+/// A message that is owned by the middleware, loaned for publishing.
+///
+/// It dereferences to a `&mut T`.
+///
+/// This type is returned by [`Publisher::borrow_loaned_message()`], see the documentation of
+/// that function for more information.
+///
+/// The loan is returned by dropping the message or [publishing it][1].
+///
+/// [1]: LoanedMessage::publish
+pub struct LoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    pub(super) msg_ptr: *mut T,
+    pub(super) publisher: &'a Publisher<T>,
+}
+
+impl<'a, T> Deref for LoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    type Target = T;
+    fn deref(&self) -> &Self::Target {
+        // SAFETY: msg_ptr is a valid pointer, obtained through rcl_borrow_loaned_message.
+        unsafe { &*self.msg_ptr }
+    }
+}
+
+impl<'a, T> DerefMut for LoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        // SAFETY: msg_ptr is a valid pointer, obtained through rcl_borrow_loaned_message.
+        unsafe { &mut *self.msg_ptr }
+    }
+}
+
+impl<'a, T> Drop for LoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    fn drop(&mut self) {
+        // Check whether the loan was already returned with
+        // rcl_publish_loaned_message()
+        if !self.msg_ptr.is_null() {
+            unsafe {
+                // SAFETY: These two pointers are valid, and the msg_ptr is not used afterwards.
+                rcl_return_loaned_message_from_publisher(
+                    &*self.publisher.rcl_publisher_mtx.lock().unwrap(),
+                    self.msg_ptr as *mut _,
+                )
+                .ok()
+                .unwrap()
+            }
+        }
+    }
+}
+
+impl<'a, T> LoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    /// Publishes the loaned message, falling back to regular publishing if needed.
+    pub fn publish(mut self) -> Result<(), RclrsError> {
+        unsafe {
+            // SAFETY: These two pointers are valid, and the msg_ptr is not used afterwards.
+            rcl_publish_loaned_message(
+                &*self.publisher.rcl_publisher_mtx.lock().unwrap(),
+                self.msg_ptr as *mut _,
+                std::ptr::null_mut(),
+            )
+            .ok()?;
+        }
+        // Set the msg_ptr to null, as a signal to the drop impl that this
+        // loan was already returned.
+        self.msg_ptr = std::ptr::null_mut();
+        Ok(())
+    }
+}

rclrs/src/node/subscription.rs

@@ -12,6 +12,9 @@ use std::sync::{Arc, Mutex, MutexGuard};
 
 use rosidl_runtime_rs::{Message, RmwMessage};
 
+mod readonly_loaned_message;
+pub use readonly_loaned_message::*;
+
 // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread
 // they are running in. Therefore, this type can be safely sent to another thread.
 unsafe impl Send for rcl_subscription_t {}
@@ -187,6 +190,40 @@ where
     }
 }
 
+impl<T> Subscription<T>
+where
+    T: RmwMessage,
+{
+    /// Obtains a read-only handle to a message owned by the middleware.
+    ///
+    /// When there is no new message, this will return a
+    /// [`SubscriptionTakeFailed`][1].
+    ///
+    /// This is the counterpart to [`Publisher::borrow_loaned_message()`][2]. See its documentation
+    /// for more information.
+    ///
+    /// [1]: crate::RclrsError
+    /// [2]: crate::Publisher::borrow_loaned_message
+    pub fn take_loaned_message(&self) -> Result<ReadOnlyLoanedMessage<'_, T>, RclrsError> {
+        let mut msg_ptr = std::ptr::null_mut();
+        unsafe {
+            // SAFETY: The third argument (message_info) and fourth argument (allocation) may be null.
+            // The second argument (loaned_message) contains a null ptr as expected.
+            rcl_take_loaned_message(
+                &*self.handle.lock(),
+                &mut msg_ptr,
+                std::ptr::null_mut(),
+                std::ptr::null_mut(),
+            )
+            .ok()?;
+        }
+        Ok(ReadOnlyLoanedMessage {
+            msg_ptr: msg_ptr as *const T,
+            subscription: self,
+        })
+    }
+}
+
 impl<T> SubscriptionBase for Subscription<T>
 where
     T: Message,

rclrs/src/node/subscription/readonly_loaned_message.rs

@@ -0,0 +1,47 @@
+use crate::rcl_bindings::*;
+use crate::{Subscription, ToResult};
+
+use rosidl_runtime_rs::RmwMessage;
+
+use std::ops::Deref;
+
+/// A message that is owned by the middleware, loaned out for reading.
+///
+/// It dereferences to a `&T`.
+///
+/// This type is returned by [`Subscription::take_loaned_message()`].
+///
+/// The loan is returned by dropping the `ReadOnlyLoanedMessage`.
+pub struct ReadOnlyLoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    pub(super) msg_ptr: *const T,
+    pub(super) subscription: &'a Subscription<T>,
+}
+
+impl<'a, T> Deref for ReadOnlyLoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    type Target = T;
+    fn deref(&self) -> &Self::Target {
+        unsafe { &*self.msg_ptr }
+    }
+}
+
+impl<'a, T> Drop for ReadOnlyLoanedMessage<'a, T>
+where
+    T: RmwMessage,
+{
+    fn drop(&mut self) {
+        unsafe {
+            rcl_return_loaned_message_from_subscription(
+                &*self.subscription.handle.lock(),
+                self.msg_ptr as *mut _,
+            )
+            .ok()
+            .unwrap();
+        }
+    }
+}

rosidl_runtime_rs/src/traits.rs

@@ -37,7 +37,7 @@ pub trait SequenceAlloc: Sized {
 /// used by user code.
 ///
 /// User code never needs to call this trait's method, much less implement this trait.
-pub trait RmwMessage: Clone + Debug + Default + Send + Sync {
+pub trait RmwMessage: Clone + Debug + Default + Send + Sync + Message {
     /// Get a pointer to the correct `rosidl_message_type_support_t` structure.
     fn get_type_support() -> libc::uintptr_t;
 }