Merge pull request #74 from mchhoy/message_serialization_helpers

Message serialization helpers

r2r/Cargo.toml

@@ -34,6 +34,7 @@ futures = "0.3.25"
 tokio = { version = "1.22.0", features = ["rt-multi-thread", "time", "macros"] }
 rand = "0.8.5"
 cdr = "0.2.4"
+criterion = "0.5.1"
 
 [build-dependencies]
 r2r_common = { path = "../r2r_common", version = "0.8.2" }
@@ -51,3 +52,7 @@ doc-only = ["r2r_common/doc-only", "r2r_rcl/doc-only", "r2r_msg_gen/doc-only", "
 
 [package.metadata.docs.rs]
 features = ["doc-only"]
+
+[[bench]]
+name = "deserialization"
+harness = false

r2r/benches/deserialization.rs

@@ -0,0 +1,57 @@
+use criterion::{criterion_group, criterion_main, Criterion};
+use r2r::*;
+use rand::Rng;
+use rand::thread_rng;
+
+const NUM_ELEMENTS: usize = 10_000;
+const NUM_TIMES: usize = 1_000;
+
+fn bench_ros_deserialization() {
+    let mut rng = thread_rng();
+    let mut numbers = Vec::<i32>::with_capacity(NUM_ELEMENTS);
+
+    for _ in 0..NUM_ELEMENTS {
+        numbers.push(rng.gen_range(0..i32::MAX));
+    }
+    let message = std_msgs::msg::Int32MultiArray {
+        layout: std_msgs::msg::MultiArrayLayout::default(),
+        data: numbers
+    };
+
+    let bytes = message.to_serialized_bytes().unwrap();
+
+    for _ in 0..NUM_TIMES {
+        let _ = std_msgs::msg::Int32MultiArray::from_serialized_bytes(&bytes).unwrap();
+    }
+}
+
+fn bench_cdr_deserialization() {
+    let mut rng = thread_rng();
+    let mut numbers = Vec::<i32>::with_capacity(NUM_ELEMENTS);
+
+    for _ in 0..NUM_ELEMENTS {
+        numbers.push(rng.gen_range(0..i32::MAX));
+    }
+    let message = std_msgs::msg::Int32MultiArray {
+        layout: std_msgs::msg::MultiArrayLayout::default(),
+        data: numbers
+    };
+
+    let bytes = message.to_serialized_bytes().unwrap();
+
+    for _ in 0..NUM_TIMES {
+        let _msg1 = cdr::deserialize::<std_msgs::msg::Int32MultiArray>(&bytes).unwrap();
+        // just for testing that we get the same result.
+        // let msg2 = std_msgs::msg::Int32MultiArray::from_serialized_bytes(&bytes).unwrap();
+        // assert_eq!(msg1, msg2);
+        // assert_eq!(msg2, message);
+    }
+}
+
+pub fn criterion_benchmark(c: &mut Criterion) {
+    c.bench_function("ros_deserialization", |b| b.iter(|| bench_ros_deserialization()));
+    c.bench_function("cdr_deserialization", |b| b.iter(|| bench_cdr_deserialization()));
+}
+
+criterion_group!(benches, criterion_benchmark);
+criterion_main!(benches);

r2r/src/msg_types.rs

@@ -2,12 +2,14 @@ use crate::error::*;
 use r2r_msg_gen::*;
 use r2r_rcl::{
     rosidl_action_type_support_t, rosidl_message_type_support_t, rosidl_service_type_support_t,
+    rcl_serialized_message_t,
 };
 use serde::{Deserialize, Serialize};
 use std::boxed::Box;
 use std::convert::TryInto;
 use std::fmt::Debug;
 use std::ops::{Deref, DerefMut};
+use std::cell::RefCell;
 
 pub mod generated_msgs {
     #![allow(clippy::all)]
@@ -32,6 +34,30 @@ pub(crate) fn uuid_msg_to_uuid(msg: &unique_identifier_msgs::msg::UUID) -> uuid:
     uuid::Uuid::from_bytes(bytes)
 }
 
+// TODO where is the best place for this?
+thread_local! {
+    pub static SERIALIZED_MESSAGE_CACHE: std::result::Result<RefCell<rcl_serialized_message_t>, i32> = {
+        use r2r_rcl::*;
+
+        let mut msg_buf: rcl_serialized_message_t  = unsafe { rcutils_get_zero_initialized_uint8_array() };
+
+        let ret = unsafe {
+            rcutils_uint8_array_init(
+                &mut msg_buf as *mut rcl_serialized_message_t,
+                0,
+                &rcutils_get_default_allocator(),
+            )
+        };
+
+        if ret != RCL_RET_OK as i32 {
+            Err(ret)
+        } else {
+            Ok(RefCell::new(msg_buf))
+        }
+
+    };
+}
+
 pub trait WrappedTypesupport:
     Serialize + for<'de> Deserialize<'de> + Default + Debug + Clone
 {
@@ -42,6 +68,77 @@ pub trait WrappedTypesupport:
     fn destroy_msg(msg: *mut Self::CStruct);
     fn from_native(msg: &Self::CStruct) -> Self;
     fn copy_to_native(&self, msg: &mut Self::CStruct);
+
+    /// This serializes the message using ROS2 methods.
+    fn to_serialized_bytes(&self) -> Result<Vec<u8>> {
+        use r2r_rcl::*;
+
+        SERIALIZED_MESSAGE_CACHE.with(|msg_buf| {
+            let msg = Self::create_msg();
+
+            self.copy_to_native(unsafe { msg.as_mut().expect("not null") });
+
+            let msg_buf: &mut rcl_serialized_message_t = &mut *msg_buf.as_ref().map_err(|err| Error::from_rcl_error(*err))?.borrow_mut();
+
+            let result = unsafe { 
+                rmw_serialize(
+                    msg as *const ::std::os::raw::c_void,
+                    Self::get_ts(),
+                    msg_buf as *mut rcl_serialized_message_t,
+                )
+            };
+
+            let data_bytes = unsafe {
+                std::slice::from_raw_parts(msg_buf.buffer, msg_buf.buffer_length).to_vec()
+            };
+
+            Self::destroy_msg(msg);
+
+            if result == RCL_RET_OK as i32 {
+                Ok(data_bytes)
+            } else {
+                Err(Error::from_rcl_error(result))
+            }
+        })
+    }
+
+    /// This deserializes the message using ROS2 methods.
+    fn from_serialized_bytes(data: &[u8]) -> Result<Self> {
+        use r2r_rcl::*;
+
+        let msg = Self::create_msg();
+
+        let msg_buf = rcl_serialized_message_t {
+            buffer: data.as_ptr() as *mut u8,
+            buffer_length: data.len(),
+            buffer_capacity: data.len(),
+            
+            // Since its read only, this should never be used ..
+            allocator: unsafe { rcutils_get_default_allocator() }
+        };
+
+        // Note From the docs of rmw_deserialize, its not clear whether this reuses
+        // any part of msg_buf. However it shouldn't matter since from_native
+        // clones everything again anyway ..
+        let result = unsafe { 
+            rmw_deserialize(
+                &msg_buf as *const rcl_serialized_message_t,
+                Self::get_ts(),
+                msg as *mut std::os::raw::c_void,
+            )
+        };
+        
+        let ret_val = if result == RCL_RET_OK as i32 {
+            Ok(Self::from_native(unsafe{ msg.as_ref().expect("not null") }))
+        } else {
+            Err(Error::from_rcl_error(result))
+        };
+
+        Self::destroy_msg(msg);
+
+        ret_val
+
+    }
 }
 
 pub trait WrappedServiceTypeSupport: Debug + Clone {
@@ -605,6 +702,45 @@ mod tests {
         assert!(native.void_ptr() == borrowed_msg as *mut core::ffi::c_void);
     }
 
+    #[test]
+    fn test_serialization_fixed_size() {
+        let message = std_msgs::msg::Int32 { data: 10};
+        
+        let bytes = message.to_serialized_bytes().unwrap();
+
+        let message_2 = std_msgs::msg::Int32::from_serialized_bytes(&bytes).unwrap();
+
+        assert_eq!(message.data, message_2.data);
+
+        let bytes_2 = message_2.to_serialized_bytes().unwrap();
+        let bytes_3 = message_2.to_serialized_bytes().unwrap();
+
+        assert_eq!(bytes, bytes_2);
+        assert_eq!(bytes, bytes_3);
+
+    }
+
+    #[test]
+    fn test_serialization_dynamic_size() {
+        let message = std_msgs::msg::Int32MultiArray {                     
+            layout: std_msgs::msg::MultiArrayLayout::default(),
+            data: vec![10, 20, 30]
+        };
+        
+        let bytes = message.to_serialized_bytes().unwrap();
+
+        let message_2 = std_msgs::msg::Int32MultiArray::from_serialized_bytes(&bytes).unwrap();
+
+        assert_eq!(message.data, message_2.data);
+
+        let bytes_2 = message_2.to_serialized_bytes().unwrap();
+        let bytes_3 = message_2.to_serialized_bytes().unwrap();
+        
+        assert_eq!(bytes, bytes_2);
+        assert_eq!(bytes, bytes_3);
+
+    }
+
     #[cfg(r2r__test_msgs__msg__Defaults)]
     #[test]
     fn test_untyped_json_default() {

r2r_rcl/build.rs

@@ -63,6 +63,7 @@ fn run_dynlink() {
     println!("cargo:rustc-link-lib=dylib=rcl_yaml_param_parser");
     println!("cargo:rustc-link-lib=dylib=rcutils");
     println!("cargo:rustc-link-lib=dylib=rmw");
+    println!("cargo:rustc-link-lib=dylib=rmw_implementation");
     println!("cargo:rustc-link-lib=dylib=rosidl_typesupport_c");
     println!("cargo:rustc-link-lib=dylib=rosidl_runtime_c");
 }