diff --git a/rpcl2/examples/custom_enum_field_filter.rs b/rpcl2/examples/custom_enum_field_filter.rs index 8dc5216..14af4c3 100644 --- a/rpcl2/examples/custom_enum_field_filter.rs +++ b/rpcl2/examples/custom_enum_field_filter.rs @@ -90,29 +90,20 @@ impl From> for CustomPoint { } } -// Define how we want to name the fields in the message. -impl Fields<5> for CustomPoint { - fn field_names_ordered() -> [&'static str; 5] { - ["x", "y", "z", "intensity", "my_custom_label"] - } -} - // C representation of the struct hardcoded without using the derive feature. -impl TypeLayout for CustomPoint { +impl PointConvertible<5> for CustomPoint { fn layout() -> LayoutDescription { LayoutDescription::new(&[ LayoutField::new("x", "f32", 4), LayoutField::new("y", "f32", 4), LayoutField::new("z", "f32", 4), LayoutField::new("intensity", "f32", 4), + LayoutField::new("my_custom_label", "u8", 1), + LayoutField::padding(3), ]) } } -// We implemented everything that is needed for PointConvertible so we declare it as a done. -#[cfg(not(feature = "derive"))] -impl PointConvertible<5> for CustomPoint {} - // Now we tell the library how to encode and decode the label. // You don't need to do this if your CustomPoint has a field that is already supported by PointCloud2. impl GetFieldDatatype for Label { diff --git a/rpcl2/src/iterator.rs b/rpcl2/src/iterator.rs index 7e6b7c7..587661b 100644 --- a/rpcl2/src/iterator.rs +++ b/rpcl2/src/iterator.rs @@ -1,6 +1,6 @@ //! Iterator implementations for [`PointCloud2Msg`] including a parallel iterator for rayon. use crate::{ - Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, + Endian, FieldDatatype, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, RPCL2Point, }; @@ -27,12 +27,12 @@ use alloc::vec::Vec; /// pub struct PointCloudIterator where - C: Fields, + C: PointConvertible, { iteration: usize, iteration_back: usize, data: ByteBufferView, - phantom_c: core::marker::PhantomData, // internally used for pdata names array + _phantom: core::marker::PhantomData, } #[cfg(feature = "rayon")] @@ -250,7 +250,7 @@ impl ByteBufferView { impl TryFrom for PointCloudIterator where - C: Fields, + C: PointConvertible, { type Error = MsgConversionError; @@ -262,8 +262,10 @@ where fn try_from(cloud: PointCloud2Msg) -> Result { let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N]; - let not_found_fieldnames = C::field_names_ordered() - .into_iter() + let fields_only = crate::ordered_field_names::(); + + let not_found_fieldnames = fields_only + .iter() .map(|name| { let found = cloud.fields.iter().any(|field| field.name == *name); (name, found) @@ -279,9 +281,8 @@ where return Err(MsgConversionError::FieldsNotFound(names_not_found)); } - let ordered_fieldnames = C::field_names_ordered(); for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) { - if ordered_fieldnames.contains(&field.name.as_str()) { + if fields_only.contains(&field.name) { *with_offset = ( field.name.clone(), field.datatype.try_into()?, @@ -342,14 +343,14 @@ where iteration: 0, iteration_back: cloud_length - 1, data, - phantom_c: core::marker::PhantomData, + _phantom: core::marker::PhantomData, }) } } impl PointCloudIterator where - C: Fields, + C: PointConvertible, { #[inline] #[must_use] @@ -358,7 +359,7 @@ where iteration: 0, iteration_back: data.len() - 1, data, - phantom_c: core::marker::PhantomData, + _phantom: core::marker::PhantomData, } } diff --git a/rpcl2/src/lib.rs b/rpcl2/src/lib.rs index d4a5ee4..fbfb0fe 100644 --- a/rpcl2/src/lib.rs +++ b/rpcl2/src/lib.rs @@ -64,16 +64,27 @@ //! # Custom Points //! Implement [`PointConvertible`] for your point with the `derive` feature or manually. //! +//! ## Derive (recommended) +//! ```ignore +//! #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible)] +//! pub struct MyPointXYZI { +//! pub x: f32, +//! pub y: f32, +//! pub z: f32, +//! #[rpcl2(rename("i"))] +//! pub intensity: f32, +//! } +//! ``` +//! +//! ## Manual //! ``` //! use ros_pointcloud2::prelude::*; //! -//! #[cfg_attr(feature = "derive", derive(PointConvertible, TypeLayout))] //! #[derive(Clone, Debug, PartialEq, Copy, Default)] //! pub struct MyPointXYZI { //! pub x: f32, //! pub y: f32, //! pub z: f32, -//! #[cfg_attr(feature = "derive", rpcl2(rename("i")))] //! pub intensity: f32, //! } //! @@ -83,61 +94,35 @@ //! } //! } //! -//! // Manual implementation of PointConvertible without derive. -//! #[cfg(not(feature = "derive"))] -//! impl Fields<4> for MyPointXYZI { -//! fn field_names_ordered() -> [&'static str; 4] { -//! ["x", "y", "z", "i"] // Note the different field name for intensity. -//! } -//! } -//! -//! #[cfg(not(feature = "derive"))] //! impl From> for MyPointXYZI { //! fn from(point: RPCL2Point<4>) -> Self { //! Self::new(point[0].get(), point[1].get(), point[2].get(), point[3].get()) //! } //! } //! -//! #[cfg(not(feature = "derive"))] //! impl From for RPCL2Point<4> { //! fn from(point: MyPointXYZI) -> Self { //! [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into() //! } //! } //! -//! #[cfg(not(feature = "derive"))] -//! impl TypeLayout for MyPointXYZI { +//! impl PointConvertible<4> for MyPointXYZI { //! fn layout() -> LayoutDescription { //! LayoutDescription::new(&[ //! LayoutField::new("x", "f32", 4), //! LayoutField::new("y", "f32", 4), //! LayoutField::new("z", "f32", 4), -//! LayoutField::new("intensity", "f32", 4), // Use the original name. +//! LayoutField::new("intensity", "f32", 4), //! ]) //! } //! } //! -//! #[cfg(not(feature = "derive"))] -//! impl PointConvertible<4> for MyPointXYZI {} -//! //! let first_p = MyPointXYZI::new(1.0, 2.0, 3.0, 0.5); //! let cloud_points = vec![first_p, MyPointXYZI::new(4.0, 5.0, 6.0, 0.5)]; //! let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap(); //! let cloud_points_out: Vec = msg_out.try_into_iter().unwrap().collect(); //! assert_eq!(first_p, *cloud_points_out.first().unwrap()); //! ``` -//! -//! An example without `#[cfg_attr]` looks like this: -//! ```ignore -//! #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)] -//! pub struct MyPointXYZI { -//! pub x: f32, -//! pub y: f32, -//! pub z: f32, -//! #[rpcl2(rename("i"))] -//! pub intensity: f32, -//! } -//! ``` #![crate_type = "lib"] #![cfg_attr(docsrs, feature(doc_cfg))] #![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.0-rc.1")] @@ -253,7 +238,7 @@ pub struct LayoutDescription(Vec); impl LayoutDescription { pub fn new(fields: &[LayoutField]) -> Self { - Self(fields.to_vec()) + Self(fields.into()) } } @@ -470,6 +455,29 @@ pub struct CloudDimensions { pub height: u32, } +fn ordered_field_names>() -> Vec { + C::layout() + .0 + .iter() + .filter(|field| match field { + LayoutField::Field { + name: _, + ty: _, + size: _, + } => true, + _ => false, + }) + .map(|field| match field { + LayoutField::Field { + name, + ty: _, + size: _, + } => name.to_string(), + _ => unreachable!("Fields must be filtered before."), + }) + .collect() +} + impl PointCloud2Msg { #[inline] fn byte_similarity(&self) -> Result @@ -479,26 +487,26 @@ impl PointCloud2Msg { let point: RPCL2Point = C::default().into(); debug_assert!(point.fields.len() == N); - let field_names = C::field_names_ordered(); + let field_names = ordered_field_names::(); debug_assert!(field_names.len() == N); let layout = KnownLayoutInfo::try_from(C::layout())?; - debug_assert!(field_names.len() == layout.fields.len()); + debug_assert!(field_names.len() <= layout.fields.len()); let mut offset: u32 = 0; let mut field_counter = 0; - for (f, msg_f) in layout.fields.iter().zip(self.fields.iter()) { + for f in layout.fields.iter() { match f { PointField::Field { datatype, size, count, } => { - let f_translated = String::from_str(field_names[field_counter]) - .expect("Field name is not a valid string."); + let msg_f = &self.fields[field_counter]; + let f_translated = &field_names[field_counter]; field_counter += 1; - if msg_f.name != f_translated + if msg_f.name != *f_translated || msg_f.offset != offset || msg_f.datatype != *datatype || msg_f.count != 1 @@ -544,7 +552,7 @@ impl PointCloud2Msg { let point: RPCL2Point = C::default().into(); debug_assert!(point.fields.len() == N); - let field_names = C::field_names_ordered(); + let field_names = crate::ordered_field_names::(); debug_assert!(field_names.len() == N); let mut pdata_offsets_acc: u32 = 0; @@ -636,17 +644,15 @@ impl PointCloud2Msg { let point: RPCL2Point = C::default().into(); debug_assert!(point.fields.len() == N); - let field_names = C::field_names_ordered(); + let field_names = crate::ordered_field_names::(); debug_assert!(field_names.len() == N); let layout = KnownLayoutInfo::try_from(C::layout())?; - debug_assert!(field_names.len() == layout.fields.len()); + debug_assert!(field_names.len() <= layout.fields.len()); let mut offset = 0; - let mut fields: Vec = Vec::with_capacity(layout.fields.len()); + let mut fields: Vec = Vec::with_capacity(field_names.len()); for f in layout.fields.into_iter() { - let f_translated = String::from_str(field_names[fields.len()]) - .expect("Field name is not a valid string."); match f { PointField::Field { datatype, @@ -654,7 +660,7 @@ impl PointCloud2Msg { count, } => { fields.push(PointFieldMsg { - name: f_translated, + name: field_names[fields.len()].clone(), offset, datatype, ..Default::default() @@ -815,14 +821,6 @@ pub struct RPCL2Point { fields: [PointData; N], } -impl Default for RPCL2Point { - fn default() -> Self { - Self { - fields: [PointData::default(); N], - } - } -} - impl core::ops::Index for RPCL2Point { type Output = PointData; @@ -840,115 +838,90 @@ impl From<[PointData; N]> for RPCL2Point { /// Trait to enable point conversions on the fly. /// /// Implement this trait for your custom point you want to read or write in the message. -/// For a more convenient way to implement this trait, enable the `derive` feature and use the `#[derive(PointConvertible, TypeLayout)]` macro. +/// It is strongly recommended to enable the `derive` feature and use the `#[derive(PointConvertible)]` macro. +/// This prevents common errors when implementing this trait by hand. /// -/// # Derive Example +/// Be aware that Rust does not guarantee the memory layout of structs. Learn more [here](https://doc.rust-lang.org/reference/type-layout.html). +/// To make layouting more predictable and thus faster for C++ node interactions, use the `#[repr(C)]` attribute on your struct. +/// An example for diverging point layouts with padding can be seen in the source code of [this](points::PointXYZRGBA::layout) implementation. +/// +/// The generic parameter `N` is the number of fields in the point type. There can be more (hidden) fields that pad the layout but they do not count for the N. +/// For +/// +/// # Derive +/// ```ignore +/// use ros_pointcloud2::prelude::*; +/// +/// #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible)] +/// #[repr(C, align(4))] +/// pub struct MyPointXYZL { +/// pub x: f32, +/// pub y: f32, +/// pub z: f32, +/// #[rpcl2(rename("l"))] +/// pub label: u8, +/// } +/// ``` +/// +/// # Manual /// ``` /// use ros_pointcloud2::prelude::*; /// /// #[derive(Clone, Debug, PartialEq, Copy, Default)] -/// pub struct MyPointXYZI { +/// #[repr(C, align(4))] +/// pub struct MyPointXYZL { /// pub x: f32, /// pub y: f32, /// pub z: f32, -/// pub intensity: f32, -/// } -/// ``` -/// -/// # Manual Example -/// ``` -/// use ros_pointcloud2::prelude::*; -/// -/// #[derive(Clone, Debug, PartialEq, Copy, Default)] -/// pub struct MyPointXYZI { -/// pub x: f32, -/// pub y: f32, -/// pub z: f32, -/// pub intensity: f32, +/// pub label: u8, /// } /// -/// impl From for RPCL2Point<4> { -/// fn from(point: MyPointXYZI) -> Self { -/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into() +/// impl From for RPCL2Point<4> { +/// fn from(point: MyPointXYZL) -> Self { +/// [point.x.into(), point.y.into(), point.z.into(), point.label.into()].into() /// } /// } /// -/// impl From> for MyPointXYZI { +/// impl From> for MyPointXYZL { /// fn from(point: RPCL2Point<4>) -> Self { /// Self { /// x: point[0].get(), /// y: point[1].get(), /// z: point[2].get(), -/// intensity: point[3].get(), +/// label: point[3].get(), /// } /// } /// } /// -/// impl Fields<4> for MyPointXYZI { -/// fn field_names_ordered() -> [&'static str; 4] { -/// ["x", "y", "z", "intensity"] -/// } -/// } -/// -/// impl TypeLayout for MyPointXYZI { +/// impl PointConvertible<4> for MyPointXYZL { /// fn layout() -> LayoutDescription { /// LayoutDescription::new(&[ /// LayoutField::new("x", "f32", 4), /// LayoutField::new("y", "f32", 4), /// LayoutField::new("z", "f32", 4), -/// LayoutField::new("intensity", "f32", 4), +/// LayoutField::new("l", "u8", 1), +/// LayoutField::padding(3), /// ]) /// } /// } -/// -/// impl PointConvertible<4> for MyPointXYZI {} /// ``` pub trait PointConvertible: - TypeLayout + From> + Into> + Fields + Default + From> + Into> + Default + Sized { + fn layout() -> LayoutDescription; } -/// Matching field names from each data point. -/// Always make sure to use the same order as in your conversion implementation to have a correct mapping. -/// -/// This trait is needed to implement the `PointConvertible` trait. -/// -/// # Example -/// ``` -/// use ros_pointcloud2::prelude::*; -/// -/// #[derive(Clone, Debug, PartialEq, Copy)] -/// pub struct MyPointXYZI { -/// pub x: f32, -/// pub y: f32, -/// pub z: f32, -/// pub intensity: f32, -/// } -/// -/// impl Fields<4> for MyPointXYZI { -/// fn field_names_ordered() -> [&'static str; 4] { -/// ["x", "y", "z", "intensity"] -/// } -/// } -/// ``` -pub trait Fields { - fn field_names_ordered() -> [&'static str; N]; -} - +#[derive(Debug)] enum PointField { Padding(u32), Field { size: u32, datatype: u8, count: u32 }, } +#[derive(Debug)] struct KnownLayoutInfo { fields: Vec, } -/// Trait to enable the layout description for the point type for direct copy operations. -pub trait TypeLayout { - fn layout() -> LayoutDescription; -} - impl TryFrom for PointField { type Error = MsgConversionError; @@ -1472,6 +1445,7 @@ impl FromBytes for u8 { } } +/* TODO test in test crate #[cfg(test)] #[cfg(feature = "derive")] mod tests { @@ -1495,4 +1469,4 @@ mod tests { let names = TestStruct::field_names_ordered(); assert_eq!(names, ["field1", "renamed_field", "field3", "field4"]); } -} +}*/ diff --git a/rpcl2/src/points.rs b/rpcl2/src/points.rs index be2d134..11f93c8 100644 --- a/rpcl2/src/points.rs +++ b/rpcl2/src/points.rs @@ -1,12 +1,12 @@ //! Predefined point types commonly used in ROS. -use crate::{Fields, LayoutDescription, LayoutField, PointConvertible, RPCL2Point, TypeLayout}; +use crate::{LayoutDescription, LayoutField, PointConvertible, RPCL2Point}; /// A packed RGB color encoding as used in ROS tools. #[derive(Clone, Copy)] -#[repr(C)] +#[repr(C, align(4))] pub union RGB { packed: f32, - unpacked: [u8; 4], // 1 byte padding + unpacked: [u8; 4], // Padding } unsafe impl Send for RGB {} @@ -106,23 +106,13 @@ impl From for RGB { /// 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)] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZ { pub x: f32, pub y: f32, pub z: f32, } -impl TypeLayout for PointXYZ { - fn layout() -> LayoutDescription { - LayoutDescription::new(&[ - LayoutField::new("x", "f32", 4), - LayoutField::new("y", "f32", 4), - LayoutField::new("z", "f32", 4), - ]) - } -} - #[cfg(feature = "nalgebra")] impl From> for PointXYZ { fn from(point: nalgebra::Point3) -> Self { @@ -158,12 +148,6 @@ impl PointXYZ { unsafe impl Send for PointXYZ {} unsafe impl Sync for PointXYZ {} -impl Fields<3> for PointXYZ { - fn field_names_ordered() -> [&'static str; 3] { - ["x", "y", "z"] - } -} - impl From> for PointXYZ { fn from(point: RPCL2Point<3>) -> Self { Self::new(point[0].get(), point[1].get(), point[2].get()) @@ -176,30 +160,28 @@ impl From for RPCL2Point<3> { } } -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)] -#[repr(C)] -pub struct PointXYZI { - pub x: f32, - pub y: f32, - pub z: f32, - pub intensity: f32, -} - -impl TypeLayout for PointXYZI { +impl PointConvertible<3> for PointXYZ { fn layout() -> LayoutDescription { LayoutDescription::new(&[ LayoutField::new("x", "f32", 4), LayoutField::new("y", "f32", 4), LayoutField::new("z", "f32", 4), - LayoutField::new("intensity", "f32", 4), + LayoutField::padding(4), ]) } } +/// 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)] +#[repr(C, align(16))] +pub struct PointXYZI { + pub x: f32, + pub y: f32, + pub z: f32, + pub intensity: f32, +} + impl PointXYZI { pub fn new(x: f32, y: f32, z: f32, intensity: f32) -> Self { Self { x, y, z, intensity } @@ -216,12 +198,6 @@ impl PointXYZI { unsafe impl Send for PointXYZI {} unsafe impl Sync for PointXYZI {} -impl Fields<4> for PointXYZI { - fn field_names_ordered() -> [&'static str; 4] { - ["x", "y", "z", "intensity"] - } -} - impl From> for PointXYZI { fn from(point: RPCL2Point<4>) -> Self { Self::new( @@ -245,30 +221,28 @@ impl From for RPCL2Point<4> { } } -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)] -#[repr(C)] -pub struct PointXYZL { - pub x: f32, - pub y: f32, - pub z: f32, - pub label: u32, -} - -impl TypeLayout for PointXYZL { +impl PointConvertible<4> for PointXYZI { fn layout() -> LayoutDescription { LayoutDescription::new(&[ LayoutField::new("x", "f32", 4), LayoutField::new("y", "f32", 4), LayoutField::new("z", "f32", 4), - LayoutField::new("label", "u32", 4), + LayoutField::new("intensity", "f32", 4), ]) } } +/// 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)] +#[repr(C, align(16))] +pub struct PointXYZL { + pub x: f32, + pub y: f32, + pub z: f32, + pub label: u32, +} + impl PointXYZL { pub fn new(x: f32, y: f32, z: f32, label: u32) -> Self { Self { x, y, z, label } @@ -285,12 +259,6 @@ impl PointXYZL { unsafe impl Send for PointXYZL {} unsafe impl Sync for PointXYZL {} -impl Fields<4> for PointXYZL { - fn field_names_ordered() -> [&'static str; 4] { - ["x", "y", "z", "label"] - } -} - impl From> for PointXYZL { fn from(point: RPCL2Point<4>) -> Self { Self::new( @@ -314,30 +282,28 @@ impl From for RPCL2Point<4> { } } -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)] -#[repr(C)] -pub struct PointXYZRGB { - pub x: f32, - pub y: f32, - pub z: f32, - pub rgb: RGB, -} - -impl TypeLayout for PointXYZRGB { +impl PointConvertible<4> for PointXYZL { fn layout() -> LayoutDescription { LayoutDescription::new(&[ LayoutField::new("x", "f32", 4), LayoutField::new("y", "f32", 4), LayoutField::new("z", "f32", 4), - LayoutField::new("rgb", "RGB", 4), + LayoutField::new("label", "u32", 4), ]) } } +/// 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)] +#[repr(C, align(16))] +pub struct PointXYZRGB { + pub x: f32, + pub y: f32, + pub z: f32, + pub rgb: RGB, +} + impl PointXYZRGB { #[must_use] pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8) -> Self { @@ -371,12 +337,6 @@ impl PointXYZRGB { unsafe impl Send for PointXYZRGB {} unsafe impl Sync for PointXYZRGB {} -impl Fields<4> for PointXYZRGB { - fn field_names_ordered() -> [&'static str; 4] { - ["x", "y", "z", "rgb"] - } -} - impl From> for PointXYZRGB { fn from(point: RPCL2Point<4>) -> Self { Self { @@ -400,34 +360,30 @@ impl From for RPCL2Point<4> { } } -impl PointConvertible<4> for PointXYZRGB {} - -/// Predefined point type commonly used in ROS with PCL. -/// 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)] -#[repr(C)] -pub struct PointXYZRGBA { - pub x: f32, - pub y: f32, - pub z: f32, - pub rgb: RGB, - pub a: u8, -} - -impl TypeLayout for PointXYZRGBA { +impl PointConvertible<4> for PointXYZRGB { fn layout() -> LayoutDescription { LayoutDescription::new(&[ LayoutField::new("x", "f32", 4), LayoutField::new("y", "f32", 4), LayoutField::new("z", "f32", 4), LayoutField::new("rgb", "RGB", 4), - LayoutField::new("a", "u8", 1), - LayoutField::padding(3), ]) } } +/// Predefined point type commonly used in ROS with PCL. +/// 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)] +#[repr(C, align(16))] +pub struct PointXYZRGBA { + pub x: f32, + pub y: f32, + pub z: f32, + pub rgb: RGB, + pub a: u8, +} + impl PointXYZRGBA { #[must_use] pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, a: u8) -> Self { @@ -461,12 +417,6 @@ impl PointXYZRGBA { unsafe impl Send for PointXYZRGBA {} unsafe impl Sync for PointXYZRGBA {} -impl Fields<5> for PointXYZRGBA { - fn field_names_ordered() -> [&'static str; 5] { - ["x", "y", "z", "rgb", "a"] - } -} - impl From> for PointXYZRGBA { fn from(point: RPCL2Point<5>) -> Self { Self { @@ -492,12 +442,23 @@ impl From for RPCL2Point<5> { } } -impl PointConvertible<5> for PointXYZRGBA {} +impl PointConvertible<5> for PointXYZRGBA { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + LayoutField::new("rgb", "RGB", 4), + LayoutField::new("a", "u8", 1), + LayoutField::padding(15), + ]) + } +} /// 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)] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZRGBNormal { pub x: f32, pub y: f32, @@ -508,20 +469,6 @@ pub struct PointXYZRGBNormal { pub normal_z: f32, } -impl TypeLayout for PointXYZRGBNormal { - fn layout() -> LayoutDescription { - LayoutDescription::new(&[ - LayoutField::new("x", "f32", 4), - LayoutField::new("y", "f32", 4), - LayoutField::new("z", "f32", 4), - LayoutField::new("rgb", "RGB", 4), - LayoutField::new("normal_x", "f32", 4), - LayoutField::new("normal_y", "f32", 4), - LayoutField::new("normal_z", "f32", 4), - ]) - } -} - impl PointXYZRGBNormal { #[must_use] pub fn new( @@ -570,12 +517,6 @@ impl PointXYZRGBNormal { unsafe impl Send for PointXYZRGBNormal {} unsafe impl Sync for PointXYZRGBNormal {} -impl Fields<7> for PointXYZRGBNormal { - fn field_names_ordered() -> [&'static str; 7] { - ["x", "y", "z", "rgb", "normal_x", "normal_y", "normal_z"] - } -} - impl From> for PointXYZRGBNormal { fn from(point: RPCL2Point<7>) -> Self { Self { @@ -605,12 +546,25 @@ impl From for RPCL2Point<7> { } } -impl PointConvertible<7> for PointXYZRGBNormal {} +impl PointConvertible<7> for PointXYZRGBNormal { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + LayoutField::new("rgb", "RGB", 4), + LayoutField::new("normal_x", "f32", 4), + LayoutField::new("normal_y", "f32", 4), + LayoutField::new("normal_z", "f32", 4), + LayoutField::padding(4), + ]) + } +} /// 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)] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZINormal { pub x: f32, pub y: f32, @@ -621,20 +575,6 @@ pub struct PointXYZINormal { pub normal_z: f32, } -impl TypeLayout for PointXYZINormal { - fn layout() -> LayoutDescription { - LayoutDescription::new(&[ - LayoutField::new("x", "f32", 4), - LayoutField::new("y", "f32", 4), - LayoutField::new("z", "f32", 4), - LayoutField::new("intensity", "f32", 4), - LayoutField::new("normal_x", "f32", 4), - LayoutField::new("normal_y", "f32", 4), - LayoutField::new("normal_z", "f32", 4), - ]) - } -} - impl PointXYZINormal { #[must_use] pub fn new( @@ -668,12 +608,6 @@ impl PointXYZINormal { unsafe impl Send for PointXYZINormal {} unsafe impl Sync for PointXYZINormal {} -impl Fields<7> for PointXYZINormal { - fn field_names_ordered() -> [&'static str; 7] { - ["x", "y", "z", "i", "normal_x", "normal_y", "normal_z"] - } -} - impl From> for PointXYZINormal { fn from(point: RPCL2Point<7>) -> Self { Self::new( @@ -703,12 +637,25 @@ impl From for RPCL2Point<7> { } } -impl PointConvertible<7> for PointXYZINormal {} +impl PointConvertible<7> for PointXYZINormal { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + LayoutField::new("intensity", "f32", 4), + LayoutField::new("normal_x", "f32", 4), + LayoutField::new("normal_y", "f32", 4), + LayoutField::new("normal_z", "f32", 4), + LayoutField::padding(4), + ]) + } +} /// 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)] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZRGBL { pub x: f32, pub y: f32, @@ -717,18 +664,6 @@ pub struct PointXYZRGBL { pub label: u32, } -impl TypeLayout for PointXYZRGBL { - fn layout() -> LayoutDescription { - LayoutDescription::new(&[ - LayoutField::new("x", "f32", 4), - LayoutField::new("y", "f32", 4), - LayoutField::new("z", "f32", 4), - LayoutField::new("rgb", "RGB", 4), - LayoutField::new("label", "u32", 4), - ]) - } -} - unsafe impl Send for PointXYZRGBL {} unsafe impl Sync for PointXYZRGBL {} @@ -768,12 +703,6 @@ impl PointXYZRGBL { } } -impl Fields<5> for PointXYZRGBL { - fn field_names_ordered() -> [&'static str; 5] { - ["x", "y", "z", "rgb", "label"] - } -} - impl From> for PointXYZRGBL { fn from(point: RPCL2Point<5>) -> Self { Self { @@ -799,12 +728,23 @@ impl From for RPCL2Point<5> { } } -impl PointConvertible<5> for PointXYZRGBL {} +impl PointConvertible<5> for PointXYZRGBL { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + LayoutField::new("rgb", "RGB", 4), + LayoutField::new("label", "u32", 4), + LayoutField::padding(12), + ]) + } +} /// 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)] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZNormal { pub x: f32, pub y: f32, @@ -814,19 +754,6 @@ pub struct PointXYZNormal { pub normal_z: f32, } -impl TypeLayout for PointXYZNormal { - fn layout() -> LayoutDescription { - LayoutDescription::new(&[ - LayoutField::new("x", "f32", 4), - LayoutField::new("y", "f32", 4), - LayoutField::new("z", "f32", 4), - LayoutField::new("normal_x", "f32", 4), - LayoutField::new("normal_y", "f32", 4), - LayoutField::new("normal_z", "f32", 4), - ]) - } -} - impl PointXYZNormal { #[must_use] pub fn new(x: f32, y: f32, z: f32, normal_x: f32, normal_y: f32, normal_z: f32) -> Self { @@ -851,12 +778,6 @@ impl PointXYZNormal { unsafe impl Send for PointXYZNormal {} unsafe impl Sync for PointXYZNormal {} -impl Fields<6> for PointXYZNormal { - fn field_names_ordered() -> [&'static str; 6] { - ["x", "y", "z", "normal_x", "normal_y", "normal_z"] - } -} - impl From> for PointXYZNormal { fn from(point: RPCL2Point<6>) -> Self { Self::new( @@ -884,4 +805,16 @@ impl From for RPCL2Point<6> { } } -impl PointConvertible<6> for PointXYZNormal {} +impl PointConvertible<6> for PointXYZNormal { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + LayoutField::new("normal_x", "f32", 4), + LayoutField::new("normal_y", "f32", 4), + LayoutField::new("normal_z", "f32", 4), + LayoutField::padding(8), + ]) + } +} diff --git a/rpcl2/src/prelude.rs b/rpcl2/src/prelude.rs index a1c3903..b591716 100644 --- a/rpcl2/src/prelude.rs +++ b/rpcl2/src/prelude.rs @@ -1,7 +1,7 @@ //! Commonly used types and traits for predefined and custom point conversions. pub use crate::{ - FieldDatatype, Fields, FromBytes, GetFieldDatatype, LayoutDescription, LayoutField, - MsgConversionError, PointCloud2Msg, PointConvertible, PointDataBuffer, RPCL2Point, TypeLayout, + FieldDatatype, FromBytes, GetFieldDatatype, LayoutDescription, LayoutField, MsgConversionError, + PointCloud2Msg, PointConvertible, PointDataBuffer, RPCL2Point, }; pub use crate::points::*; diff --git a/rpcl2/tests/e2e_test.rs b/rpcl2/tests/e2e_test.rs index ab91996..817fa13 100644 --- a/rpcl2/tests/e2e_test.rs +++ b/rpcl2/tests/e2e_test.rs @@ -129,7 +129,7 @@ fn conv_cloud_par_par_iter() { #[test] #[cfg(feature = "derive")] fn custom_xyz_f32() { - #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)] + #[derive(Debug, PartialEq, Clone, Default)] #[repr(C)] struct CustomPoint { x: f32, @@ -137,6 +137,32 @@ fn custom_xyz_f32() { z: f32, } + impl From> for CustomPoint { + fn from(point: RPCL2Point<3>) -> Self { + Self { + x: point[0].get(), + y: point[1].get(), + z: point[2].get(), + } + } + } + + impl From for RPCL2Point<3> { + fn from(point: CustomPoint) -> Self { + [point.x.into(), point.y.into(), point.z.into()].into() + } + } + + impl PointConvertible<3> for CustomPoint { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + ]) + } + } + convert_from_into!( CustomPoint, vec![ @@ -160,7 +186,6 @@ fn custom_xyz_f32() { } #[test] -#[cfg(feature = "derive")] fn custom_xyzi_f32() { let cloud: Vec = vec![ CustomPointXYZI { @@ -189,7 +214,7 @@ fn custom_xyzi_f32() { }, ]; - #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)] + #[derive(Debug, PartialEq, Clone, Default)] #[repr(C)] struct CustomPointXYZI { x: f32, @@ -198,13 +223,48 @@ fn custom_xyzi_f32() { i: u8, } + impl From> for CustomPointXYZI { + fn from(point: RPCL2Point<4>) -> Self { + Self { + x: point[0].get(), + y: point[1].get(), + z: point[2].get(), + i: point[3].get(), + } + } + } + + impl From for RPCL2Point<4> { + fn from(point: CustomPointXYZI) -> Self { + [ + point.x.into(), + point.y.into(), + point.z.into(), + point.i.into(), + ] + .into() + } + } + + impl PointConvertible<4> for CustomPointXYZI { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + LayoutField::new("i", "u8", 1), + LayoutField::padding(3), + ]) + } + } + convert_from_into!(CustomPointXYZI, cloud); } #[test] #[cfg(feature = "derive")] fn custom_rgba_f32() { - #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)] + #[derive(Debug, PartialEq, Clone, Default)] #[repr(C)] struct CustomPoint { x: f32, @@ -216,6 +276,53 @@ fn custom_rgba_f32() { a: u8, } + impl From> for CustomPoint { + fn from(point: RPCL2Point<7>) -> Self { + Self { + x: point[0].get(), + y: point[1].get(), + z: point[2].get(), + r: point[3].get(), + g: point[4].get(), + b: point[5].get(), + a: point[6].get(), + } + } + } + + impl From for RPCL2Point<7> { + fn from(point: CustomPoint) -> Self { + [ + point.x.into(), + point.y.into(), + point.z.into(), + point.r.into(), + point.g.into(), + point.b.into(), + point.a.into(), + ] + .into() + } + } + + impl PointConvertible<7> for CustomPoint { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + LayoutField::new("r", "u8", 1), + LayoutField::padding(3), + LayoutField::new("g", "u8", 1), + LayoutField::padding(3), + LayoutField::new("b", "u8", 1), + LayoutField::padding(3), + LayoutField::new("a", "u8", 1), + LayoutField::padding(3), + ]) + } + } + let cloud = vec![ CustomPoint { x: 0.0, @@ -447,9 +554,8 @@ fn write_xyzi_read_xyz_vec() { } #[test] -#[cfg(feature = "derive")] fn write_less_than_available() { - #[derive(Debug, PartialEq, Clone, Default, TypeLayout)] + #[derive(Debug, PartialEq, Clone, Default)] #[repr(C)] struct CustomPoint { x: f32, @@ -475,14 +581,16 @@ fn write_less_than_available() { } } - impl Fields<3> for CustomPoint { - fn field_names_ordered() -> [&'static str; 3] { - ["x", "y", "z"] + impl PointConvertible<3> for CustomPoint { + fn layout() -> LayoutDescription { + LayoutDescription::new(&[ + LayoutField::new("x", "f32", 4), + LayoutField::new("y", "f32", 4), + LayoutField::new("z", "f32", 4), + ]) } } - impl PointConvertible<3> for CustomPoint {} - let write_cloud = vec![ CustomPoint { x: 1.0,