From 2c637c40ed207294ea5c5eee59db81db6bef39f8 Mon Sep 17 00:00:00 2001 From: stelzo Date: Tue, 7 May 2024 10:01:17 +0200 Subject: [PATCH] derive as feat only --- Cargo.toml | 2 +- benches/roundtrip.rs | 2 + examples/custom_label_filter.rs | 103 ++++++++++++----------- rpcl2_derive/src/lib.rs | 35 ++++---- src/lib.rs | 140 +++++++++++++++++++++++++++----- src/pcl_utils.rs | 14 +++- tests/e2e_test.rs | 19 ++++- 7 files changed, 221 insertions(+), 94 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index a897f32..8c4dadd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -37,4 +37,4 @@ r2r_msg = ["dep:r2r"] rayon = ["dep:rayon"] derive = ["dep:rpcl2_derive"] -default = ["derive"] +default = ["derive"] \ No newline at end of file diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs index 7f3d888..1b631f9 100644 --- a/benches/roundtrip.rs +++ b/benches/roundtrip.rs @@ -1,3 +1,5 @@ +#![cfg(feature = "derive")] + use criterion::{black_box, criterion_group, criterion_main, Criterion}; use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg}; diff --git a/examples/custom_label_filter.rs b/examples/custom_label_filter.rs index 83d7e4b..a27404d 100644 --- a/examples/custom_label_filter.rs +++ b/examples/custom_label_filter.rs @@ -2,7 +2,10 @@ // The use case is a segmentation point cloud where each point holds a label // with a custom enum type we want to filter. -use ros_pointcloud2::{Fields, Point, PointCloud2Msg, PointConvertible}; +use ros_pointcloud2::{Fields, Point}; + +#[cfg(not(feature = "derive"))] +use ros_pointcloud2::{PointCloud2Msg, PointConvertible}; #[derive(Debug, PartialEq, Clone, Default)] enum Label { @@ -83,54 +86,58 @@ impl Fields<5> for CustomPoint { } // We implemented everything that is needed so we declare it as a PointConvertible +#[cfg(not(feature = "derive"))] impl PointConvertible<5> for CustomPoint {} fn main() { - let cloud = vec![ - CustomPoint { - x: 1.0, - y: 2.0, - z: 3.0, - intensity: 4.0, - my_custom_label: Label::Deer, - }, - CustomPoint { - x: 4.0, - y: 5.0, - z: 6.0, - intensity: 7.0, - my_custom_label: Label::Car, - }, - CustomPoint { - x: 7.0, - y: 8.0, - z: 9.0, - intensity: 10.0, - my_custom_label: Label::Human, - }, - ]; - - println!("Original cloud: {:?}", cloud); - - let msg = PointCloud2Msg::try_from_iter(cloud.clone().into_iter()).unwrap(); - - println!("filtering by label == Deer"); - let out = msg - .try_into_iter() - .unwrap() - .filter(|point: &CustomPoint| point.my_custom_label == Label::Deer) - .collect::>(); - - println!("Filtered cloud: {:?}", out); - - assert_eq!( - vec![CustomPoint { - x: 1.0, - y: 2.0, - z: 3.0, - intensity: 4.0, - my_custom_label: Label::Deer, - },], - out - ); + #[cfg(not(feature = "derive"))] + { + let cloud = vec![ + CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + intensity: 4.0, + my_custom_label: Label::Deer, + }, + CustomPoint { + x: 4.0, + y: 5.0, + z: 6.0, + intensity: 7.0, + my_custom_label: Label::Car, + }, + CustomPoint { + x: 7.0, + y: 8.0, + z: 9.0, + intensity: 10.0, + my_custom_label: Label::Human, + }, + ]; + + println!("Original cloud: {:?}", cloud); + + let msg = PointCloud2Msg::try_from_iter(cloud.clone().into_iter()).unwrap(); + + println!("filtering by label == Deer"); + let out = msg + .try_into_iter() + .unwrap() + .filter(|point: &CustomPoint| point.my_custom_label == Label::Deer) + .collect::>(); + + println!("Filtered cloud: {:?}", out); + + assert_eq!( + vec![CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + intensity: 4.0, + my_custom_label: Label::Deer, + },], + out + ); + } } diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs index f90b64e..b1ce4a5 100644 --- a/rpcl2_derive/src/lib.rs +++ b/rpcl2_derive/src/lib.rs @@ -1,11 +1,24 @@ extern crate proc_macro; -use std::{any::Any, collections::HashMap, fmt::Debug}; +use std::collections::HashMap; use proc_macro::TokenStream; use quote::{quote, ToTokens}; use syn::{parse_macro_input, DeriveInput}; +fn get_allowed_types() -> HashMap<&'static str, usize> { + let mut allowed_datatypes = HashMap::<&'static str, usize>::new(); + allowed_datatypes.insert("f32", 4); + allowed_datatypes.insert("f64", 8); + allowed_datatypes.insert("i32", 4); + allowed_datatypes.insert("u8", 1); + allowed_datatypes.insert("u16", 2); + allowed_datatypes.insert("u32", 4); + allowed_datatypes.insert("i8", 1); + allowed_datatypes.insert("i16", 2); + allowed_datatypes +} + /// Derive macro for the `Fields` trait. /// /// Given the ordering from the source code of your struct, this macro will generate an array of field names. @@ -19,15 +32,7 @@ pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream { _ => return syn::Error::new_spanned(input, "Only structs are supported").to_compile_error().into(), }; - let mut allowed_datatypes = HashMap::<&'static str, usize>::new(); - allowed_datatypes.insert("f32", 4); - allowed_datatypes.insert("f64", 8); - allowed_datatypes.insert("i32", 4); - allowed_datatypes.insert("u8", 1); - allowed_datatypes.insert("u16", 2); - allowed_datatypes.insert("u32", 4); - allowed_datatypes.insert("i8", 1); - allowed_datatypes.insert("i16", 2); + let allowed_datatypes = get_allowed_types(); if fields.is_empty() { return syn::Error::new_spanned(input, "No fields found").to_compile_error().into(); @@ -75,15 +80,7 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream { _ => return syn::Error::new_spanned(input, "Only structs are supported").to_compile_error().into(), }; - let mut allowed_datatypes = HashMap::<&'static str, usize>::new(); - allowed_datatypes.insert("f32", 4); - allowed_datatypes.insert("f64", 8); - allowed_datatypes.insert("i32", 4); - allowed_datatypes.insert("u8", 1); - allowed_datatypes.insert("u16", 2); - allowed_datatypes.insert("u32", 4); - allowed_datatypes.insert("i8", 1); - allowed_datatypes.insert("i16", 2); + let allowed_datatypes = get_allowed_types(); if fields.is_empty() { return syn::Error::new_spanned(input, "No fields found").to_compile_error().into(); diff --git a/src/lib.rs b/src/lib.rs index d5793c6..c764b3c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -63,11 +63,12 @@ use crate::ros_types::{HeaderMsg, PointFieldMsg}; use convert::Endianness; pub use convert::Fields; -use type_layout::TypeLayout; - #[cfg(feature = "derive")] pub use rpcl2_derive::*; +#[cfg(feature = "derive")] +pub use type_layout::TypeLayout; + /// All errors that can occur while converting to or from the PointCloud2 message. #[derive(Debug)] pub enum ConversionError { @@ -123,6 +124,7 @@ impl Default for PointCloud2Msg { } impl PointCloud2Msg { + #[cfg(feature = "derive")] fn prepare_direct_copy() -> Result where C: PointConvertible, @@ -134,7 +136,7 @@ impl PointCloud2Msg { debug_assert!(meta_names.len() == N); let mut offset: u32 = 0; - let layout = C::layout(); + let layout = TypeLayoutInfo::try_from(C::type_layout())?; let mut fields: Vec = Vec::with_capacity(layout.fields.len()); for f in layout.fields.into_iter() { match f { @@ -164,6 +166,53 @@ impl PointCloud2Msg { }) } + #[cfg(feature = "derive")] + fn assert_byte_similarity(&self) -> Result + where + C: PointConvertible, + { + let point: Point = C::default().into(); + debug_assert!(point.fields.len() == N); + + let meta_names = C::field_names_ordered(); + debug_assert!(meta_names.len() == N); + + let mut offset: u32 = 0; + let layout = TypeLayoutInfo::try_from(C::type_layout())?; + for (f, msg_f) in layout.fields.into_iter().zip(self.fields.iter()) { + match f { + PointField::Field { + name, + datatype, + size, + } => { + if msg_f.name != name { + return Err(ConversionError::FieldNotFound(vec![name.clone()])); + } + + if msg_f.datatype != datatype { + return Err(ConversionError::InvalidFieldFormat); + } + + if msg_f.offset != offset { + return Err(ConversionError::DataLengthMismatch); + } + + if msg_f.count != 1 { + return Err(ConversionError::UnsupportedFieldType); + } + + offset += size; // assume field_count 1 + } + PointField::Padding(size) => { + offset += size; // assume field_count 1 + } + } + } + + Ok(true) + } + #[inline(always)] fn prepare() -> Result where @@ -250,27 +299,73 @@ impl PointCloud2Msg { Ok(cloud) } + #[cfg(feature = "derive")] pub fn try_from_vec(vec: Vec) -> Result where C: PointConvertible, { - let mut cloud = Self::prepare_direct_copy::()?; + let endianness = if cfg!(target_endian = "big") { + Endianness::Big + } else if cfg!(target_endian = "little") { + Endianness::Little + } else { + panic!("Unsupported endianness"); + }; + + match endianness { + Endianness::Big => Self::try_from_iter(vec.into_iter()), + Endianness::Little => { + let mut cloud = Self::prepare_direct_copy::()?; + + let bytes_total = vec.len() * cloud.point_step as usize; + cloud.data.resize(bytes_total, u8::default()); + let raw_data: *mut C = cloud.data.as_ptr() as *mut C; + unsafe { + std::ptr::copy_nonoverlapping( + vec.as_ptr() as *const u8, + raw_data as *mut u8, + bytes_total, + ); + } - let bytes_total = vec.len() * cloud.point_step as usize; - cloud.data.resize(bytes_total, u8::default()); - let raw_data: *mut C = cloud.data.as_ptr() as *mut C; - unsafe { - std::ptr::copy_nonoverlapping( - vec.as_ptr() as *const u8, - raw_data as *mut u8, - bytes_total, - ); + cloud.width = vec.len() as u32; + cloud.row_step = cloud.width * cloud.point_step; + + Ok(cloud) + } } + } - cloud.width = vec.len() as u32; - cloud.row_step = cloud.width * cloud.point_step; + #[cfg(feature = "derive")] + pub fn try_into_vec(self) -> Result, ConversionError> + where + C: PointConvertible, + { + let endianness = if cfg!(target_endian = "big") { + Endianness::Big + } else if cfg!(target_endian = "little") { + Endianness::Little + } else { + panic!("Unsupported endianness"); + }; + + self.assert_byte_similarity::()?; + + match endianness { + Endianness::Big => Ok(self.try_into_iter()?.collect()), + Endianness::Little => { + let mut vec = Vec::with_capacity(self.width as usize); + let raw_data: *const C = self.data.as_ptr() as *const C; + unsafe { + for i in 0..self.width { + let point = raw_data.add(i as usize).read(); + vec.push(point); + } + } - Ok(cloud) + Ok(vec) + } + } } pub fn try_into_iter( @@ -347,8 +442,15 @@ pub struct Point { /// /// impl PointConvertible for MyPointXYZI {} /// ``` +#[cfg(not(feature = "derive"))] pub trait PointConvertible: - KnownLayout + From> + Into> + Fields + Clone + 'static + Default + From> + Into> + Fields + Clone + 'static + Default +{ +} + +#[cfg(feature = "derive")] +pub trait PointConvertible: + type_layout::TypeLayout + From> + Into> + Fields + Clone + 'static + Default { } @@ -397,10 +499,6 @@ impl TryFrom for TypeLayoutInfo { } } -trait KnownLayout { - fn layout() -> TypeLayoutInfo; -} - /// Metadata representation for a point. /// /// This struct is used to store meta data in a fixed size byte buffer along the with the diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs index ce4458b..1a13c46 100644 --- a/src/pcl_utils.rs +++ b/src/pcl_utils.rs @@ -1,4 +1,7 @@ -use crate::{Fields, Point, PointConvertible}; +use crate::{ConversionError, Fields, Point, PointConvertible}; + +#[cfg(feature = "derive")] +use crate::TypeLayout; /// Pack an RGB color into a single f32 value as used in ROS with PCL for RViz usage. #[inline] @@ -20,6 +23,7 @@ fn unpack_rgb(rgb: f32) -> [u8; 3] { /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZ { pub x: f32, @@ -56,6 +60,7 @@ impl PointConvertible<3> for PointXYZ {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an intensity value. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZI { pub x: f32, @@ -99,6 +104,7 @@ impl PointConvertible<4> for PointXYZI {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a label. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZL { pub x: f32, @@ -142,6 +148,7 @@ impl PointConvertible<4> for PointXYZL {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an RGB color value. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZRGB { pub x: f32, @@ -188,6 +195,7 @@ impl PointConvertible<4> for PointXYZRGB {} /// This is a 3D point with x, y, z coordinates and an RGBA color value. /// The alpha channel is commonly used as padding but this crate uses every channel and no padding. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZRGBA { pub x: f32, @@ -241,6 +249,7 @@ impl PointConvertible<5> for PointXYZRGBA {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates, an RGB color value and a normal vector. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZRGBNormal { pub x: f32, @@ -300,6 +309,7 @@ impl PointConvertible<7> for PointXYZRGBNormal {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates, an intensity value and a normal vector. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZINormal { pub x: f32, @@ -352,6 +362,7 @@ impl PointConvertible<7> for PointXYZINormal {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a label. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZRGBL { pub x: f32, @@ -405,6 +416,7 @@ impl PointConvertible<5> for PointXYZRGBL {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a normal vector. #[derive(Clone, Debug, PartialEq, Copy, Default)] +#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C)] pub struct PointXYZNormal { pub x: f32, diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index 652e344..c325bed 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -9,6 +9,7 @@ macro_rules! convert_from_into { }; } +#[cfg(feature = "derive")] macro_rules! convert_from_into_vec { ($point:ty, $cloud:expr) => { convert_from_into_in_out_cloud_vec!($cloud, $point, $cloud, $point); @@ -29,6 +30,7 @@ macro_rules! convert_from_into_in_out_cloud { }; } +#[cfg(feature = "derive")] macro_rules! convert_from_into_in_out_cloud_vec { ($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => { let msg = PointCloud2Msg::try_from_vec($in_cloud.clone()); @@ -73,6 +75,7 @@ fn write_cloud() { } #[test] +#[cfg(feature = "derive")] fn write_cloud_from_vec() { let cloud = vec![ PointXYZ { @@ -105,8 +108,10 @@ fn write_cloud_from_vec() { } #[test] +#[cfg(feature = "derive")] fn custom_xyz_f32() { - #[derive(Debug, PartialEq, Clone, Default, RosFull)] + #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)] + #[repr(C)] struct CustomPoint { x: f32, y: f32, @@ -136,6 +141,7 @@ fn custom_xyz_f32() { } #[test] +#[cfg(feature = "derive")] fn custom_xyzi_f32() { let cloud: Vec = vec![ CustomPointXYZI { @@ -164,7 +170,8 @@ fn custom_xyzi_f32() { }, ]; - #[derive(Debug, PartialEq, Clone, Default, RosFull)] + #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)] + #[repr(C)] struct CustomPointXYZI { x: f32, y: f32, @@ -176,8 +183,10 @@ fn custom_xyzi_f32() { } #[test] +#[cfg(feature = "derive")] fn custom_rgba_f32() { - #[derive(Debug, PartialEq, Clone, Default, RosFull)] + #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)] + #[repr(C)] struct CustomPoint { x: f32, y: f32, @@ -528,6 +537,7 @@ fn converterxyzrgb() { } #[test] +#[cfg(feature = "derive")] fn converterxyzrgb_from_vec() { convert_from_into_vec!( PointXYZRGB, @@ -691,7 +701,8 @@ fn write_xyzi_read_xyz() { #[test] fn write_less_than_available() { - #[derive(Debug, PartialEq, Clone, Default)] + #[derive(Debug, PartialEq, Clone, Default, TypeLayout)] + #[repr(C)] struct CustomPoint { x: f32, y: f32,