diff --git a/Cargo.toml b/Cargo.toml index 3d23d79..93b323f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -35,7 +35,7 @@ rosrust = { version = "0.9.11", optional = true } r2r = { version = "0.8.4", optional = true } rayon = { version = "1", optional = true } nalgebra = { version = "0.32.5", optional = true, default-features = false } -rpcl2-derive = { version = "0.2.0", optional = true, path = "../rpcl2-derive" } +rpcl2-derive = { version = "0.2.0", optional = true } memoffset = { version = "0.9", optional = true } sensor_msgs = { version = "*", optional = true } diff --git a/src/iterator.rs b/src/iterator.rs index 7e6b7c7..cf7cc60 100644 --- a/src/iterator.rs +++ b/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, } } @@ -495,4 +496,4 @@ mod test { let first_right = right.next(); assert!(first_right.is_none()); } -} +} \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index d4dfa2e..bfee774 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -3,8 +3,8 @@ //! The library provides the [`PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators. //! //! Vector conversions are optimized for direct copy. They are useful when you just move similar data around. They are usually a good default. -//! - [`try_from_vec`](PointCloud2Msg::try_from_vec) requires `derive` feature (enabled by default) -//! - [`try_into_vec`](PointCloud2Msg::try_into_vec) requires `derive` feature (enabled by default) +//! - [`try_from_vec`](PointCloud2Msg::try_from_vec) +//! - [`try_into_vec`](PointCloud2Msg::try_into_vec) //! //! You can use the iterator functions for more control over the conversion process. //! - [`try_from_iter`](PointCloud2Msg::try_from_iter) @@ -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,30 +94,28 @@ //! } //! } //! -//! // 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 PointConvertible<4> 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), +//! ]) +//! } +//! } //! //! 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)]; @@ -114,21 +123,9 @@ //! 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")] +#![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.0-rc.2")] #![warn(clippy::print_stderr)] #![warn(clippy::print_stdout)] #![warn(clippy::unwrap_used)] @@ -139,14 +136,6 @@ #![cfg_attr(not(feature = "std"), no_std)] // Setup an allocator with #[global_allocator] // see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html -#![warn(clippy::unwrap_used)] -#![warn(clippy::cargo)] -#![warn(clippy::std_instead_of_core)] -#![warn(clippy::alloc_instead_of_core)] -#![warn(clippy::std_instead_of_alloc)] -#![cfg_attr(not(feature = "std"), no_std)] -// Setup an allocator with #[global_allocator] -// see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html pub mod points; pub mod prelude; @@ -157,6 +146,9 @@ pub mod iterator; use crate::ros::{HeaderMsg, PointFieldMsg}; #[cfg(feature = "derive")] +#[doc(hidden)] +pub use memoffset; + use core::str::FromStr; #[macro_use] @@ -230,7 +222,6 @@ impl std::error::Error for MsgConversionError { } } -#[cfg(feature = "derive")] fn system_endian() -> Endian { if cfg!(target_endian = "big") { Endian::Big @@ -241,6 +232,43 @@ fn system_endian() -> Endian { } } +/// Description of the memory layout of a type with named fields. +#[derive(Clone, Debug)] +pub struct LayoutDescription(Vec); + +impl LayoutDescription { + pub fn new(fields: &[LayoutField]) -> Self { + Self(fields.into()) + } +} + +/// Enum to describe the field type and size in a padded or unpadded layout. +#[derive(Clone, Debug)] +pub enum LayoutField { + Field { + name: alloc::borrow::Cow<'static, str>, + ty: alloc::borrow::Cow<'static, str>, + size: usize, + }, + Padding { + size: usize, + }, +} + +impl LayoutField { + pub fn new(name: &'static str, ty: &'static str, size: usize) -> Self { + LayoutField::Field { + name: name.into(), + ty: ty.into(), + size, + } + } + + pub fn padding(size: usize) -> Self { + LayoutField::Padding { size } + } +} + /// The intermediate point cloud type for ROS integrations. /// /// To assert consistency, the type should be build with the [`PointCloud2MsgBuilder`]. @@ -273,7 +301,6 @@ pub enum Denseness { Sparse, } -#[cfg(feature = "derive")] enum ByteSimilarity { Equal, Overlapping, @@ -428,8 +455,32 @@ pub struct CloudDimensions { pub height: u32, } +fn ordered_field_names>() -> Vec { + C::layout() + .0 + .iter() + .filter(|field| { + matches!( + field, + LayoutField::Field { + name: _, + ty: _, + size: _, + } + ) + }) + .map(|field| match field { + LayoutField::Field { + name, + ty: _, + size: _, + } => name.to_string(), + _ => unreachable!("Fields must be filtered before."), + }) + .collect() +} + impl PointCloud2Msg { - #[cfg(feature = "derive")] #[inline] fn byte_similarity(&self) -> Result where @@ -438,26 +489,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 = TypeLayoutInfo::try_from(C::type_layout())?; - debug_assert!(field_names.len() == layout.fields.len()); + let layout = KnownLayoutInfo::try_from(C::layout())?; + 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 @@ -503,7 +554,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; @@ -519,7 +570,7 @@ impl PointCloud2Msg { let _ = FieldDatatype::try_from(datatype_code)?; *field_val = PointFieldMsg { - name: field_name.into(), + name: field_name, offset: pdata_offsets_acc, datatype: datatype_code, count: 1, @@ -557,8 +608,8 @@ impl PointCloud2Msg { } /// Create a PointCloud2Msg from a parallel iterator. Requires the `rayon` and `derive` feature to be enabled. - #[cfg(all(feature = "rayon", feature = "derive"))] - #[cfg_attr(docsrs, doc(cfg(all(feature = "rayon", feature = "derive"))))] + #[cfg(feature = "rayon")] + #[cfg_attr(docsrs, doc(cfg(feature = "rayon")))] pub fn try_from_par_iter( iterable: impl rayon::iter::ParallelIterator, ) -> Result @@ -585,8 +636,6 @@ impl PointCloud2Msg { /// /// # Errors /// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies. - #[cfg(feature = "derive")] - #[cfg_attr(docsrs, doc(cfg(feature = "derive")))] pub fn try_from_vec(vec: Vec) -> Result where C: PointConvertible, @@ -597,17 +646,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 = TypeLayoutInfo::try_from(C::type_layout())?; - debug_assert!(field_names.len() == layout.fields.len()); + let layout = KnownLayoutInfo::try_from(C::layout())?; + 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, @@ -615,7 +662,7 @@ impl PointCloud2Msg { count, } => { fields.push(PointFieldMsg { - name: f_translated, + name: field_names[fields.len()].clone(), offset, datatype, ..Default::default() @@ -674,8 +721,6 @@ impl PointCloud2Msg { /// /// # Errors /// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies. - #[cfg(feature = "derive")] - #[cfg_attr(docsrs, doc(cfg(feature = "derive")))] pub fn try_into_vec(self) -> Result, MsgConversionError> where C: PointConvertible, @@ -778,14 +823,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; @@ -802,157 +839,97 @@ impl From<[PointData; N]> for RPCL2Point { /// Trait to enable point conversions on the fly. /// -/// # Example +/// Implement this trait for your custom point you want to read or write in the message. +/// 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. +/// +/// 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, +/// 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 PointConvertible<4> for MyPointXYZI {} -/// ``` -#[cfg_attr(docsrs, doc(cfg(not(feature = "derive"))))] -#[cfg(not(feature = "derive"))] -pub trait PointConvertible: - From> + Into> + Fields + Clone + Default -{ -} - -/// 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. -/// -/// # Derive Example -/// ``` -/// use ros_pointcloud2::prelude::*; -/// -/// #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)] -/// pub struct MyPointXYZI { -/// pub x: f32, -/// pub y: f32, -/// pub z: f32, -/// pub intensity: f32, -/// } -/// ``` -/// -/// # Manual Example -/// ``` -/// use ros_pointcloud2::prelude::*; -/// -/// #[derive(Clone, Debug, PartialEq, Copy, Default, TypeLayout)] -/// pub struct MyPointXYZI { -/// pub x: f32, -/// pub y: f32, -/// pub z: f32, -/// pub intensity: f32, -/// } -/// -/// impl From for RPCL2Point<4> { -/// fn from(point: MyPointXYZI) -> Self { -/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into() +/// 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("l", "u8", 1), +/// LayoutField::padding(3), +/// ]) /// } /// } -/// -/// impl From> for MyPointXYZI { -/// fn from(point: RPCL2Point<4>) -> Self { -/// Self { -/// x: point[0].get(), -/// y: point[1].get(), -/// z: point[2].get(), -/// intensity: point[3].get(), -/// } -/// } -/// } -/// -/// impl Fields<4> for MyPointXYZI { -/// fn field_names_ordered() -> [&'static str; 4] { -/// ["x", "y", "z", "intensity"] -/// } -/// } -/// -/// impl PointConvertible<4> for MyPointXYZI {} /// ``` -#[cfg_attr(docsrs, doc(cfg(feature = "derive")))] -#[cfg(feature = "derive")] pub trait PointConvertible: - type_layout::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]; -} - -#[cfg(feature = "derive")] +#[derive(Debug)] enum PointField { Padding(u32), Field { size: u32, datatype: u8, count: u32 }, } -#[cfg(feature = "derive")] -struct TypeLayoutInfo { +#[derive(Debug)] +struct KnownLayoutInfo { fields: Vec, } -#[cfg(feature = "derive")] -impl TryFrom for PointField { +impl TryFrom for PointField { type Error = MsgConversionError; - fn try_from(f: type_layout::Field) -> Result { + fn try_from(f: LayoutField) -> Result { match f { - type_layout::Field::Field { name: _, ty, size } => { + LayoutField::Field { name: _, ty, size } => { let typename: String = ty.into_owned().to_lowercase(); let datatype = FieldDatatype::from_str(typename.as_str())?; Ok(Self::Field { @@ -961,21 +938,19 @@ impl TryFrom for PointField { count: 1, }) } - type_layout::Field::Padding { size } => Ok(Self::Padding(size.try_into()?)), + LayoutField::Padding { size } => Ok(Self::Padding(size.try_into()?)), } } } -#[cfg(feature = "derive")] -impl TryFrom for TypeLayoutInfo { +impl TryFrom for KnownLayoutInfo { type Error = MsgConversionError; - fn try_from(t: type_layout::TypeLayoutInfo) -> Result { - let fields: Vec = t - .fields - .into_iter() - .map(PointField::try_from) - .collect::, _>>()?; + fn try_from(t: LayoutDescription) -> Result { + let fields: Vec = + t.0.into_iter() + .map(PointField::try_from) + .collect::, _>>()?; Ok(Self { fields }) } } @@ -1470,29 +1445,4 @@ impl FromBytes for u8 { fn from_le_bytes(bytes: PointDataBuffer) -> Self { Self::from_le_bytes([bytes[0]]) } -} - -#[cfg(test)] -#[cfg(feature = "derive")] -mod tests { - use super::Fields; - use rpcl2_derive::Fields; - - use alloc::string::String; - - #[allow(dead_code)] - #[derive(Fields)] - struct TestStruct { - field1: String, - #[rpcl2(rename("renamed_field"))] - field2: i32, - field3: f64, - field4: bool, - } - - #[test] - fn test_struct_names() { - let names = TestStruct::field_names_ordered(); - assert_eq!(names, ["field1", "renamed_field", "field3", "field4"]); - } -} +} \ No newline at end of file diff --git a/src/points.rs b/src/points.rs index 9c8d0af..87def59 100644 --- a/src/points.rs +++ b/src/points.rs @@ -1,18 +1,12 @@ //! Predefined point types commonly used in ROS. -use crate::{Fields, PointConvertible, RPCL2Point}; - -#[cfg(feature = "derive")] -use type_layout::TypeLayout; - -#[cfg(feature = "derive")] -use alloc::vec::Vec; +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 {} @@ -112,8 +106,7 @@ 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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZ { pub x: f32, pub y: f32, @@ -155,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()) @@ -173,13 +160,21 @@ impl From for RPCL2Point<3> { } } -impl PointConvertible<3> for PointXYZ {} +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::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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZI { pub x: f32, pub y: f32, @@ -203,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( @@ -232,13 +221,21 @@ impl From for RPCL2Point<4> { } } -impl PointConvertible<4> for PointXYZI {} +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("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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZL { pub x: f32, pub y: f32, @@ -262,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( @@ -291,13 +282,21 @@ impl From for RPCL2Point<4> { } } -impl PointConvertible<4> for PointXYZL {} +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("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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZRGB { pub x: f32, pub y: f32, @@ -338,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 { @@ -367,14 +360,22 @@ impl From for RPCL2Point<4> { } } -impl PointConvertible<4> for PointXYZRGB {} +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), + ]) + } +} /// 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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZRGBA { pub x: f32, pub y: f32, @@ -416,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 { @@ -447,13 +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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZRGBNormal { pub x: f32, pub y: f32, @@ -512,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 { @@ -547,13 +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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZINormal { pub x: f32, pub y: f32, @@ -597,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( @@ -632,13 +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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZRGBL { pub x: f32, pub y: f32, @@ -686,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 { @@ -717,13 +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)] -#[cfg_attr(feature = "derive", derive(TypeLayout))] -#[repr(C)] +#[repr(C, align(16))] pub struct PointXYZNormal { pub x: f32, pub y: f32, @@ -757,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( @@ -790,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), + ]) + } +} \ No newline at end of file diff --git a/src/prelude.rs b/src/prelude.rs index fd746fa..11cf41f 100644 --- a/src/prelude.rs +++ b/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, MsgConversionError, PointCloud2Msg, - PointConvertible, PointDataBuffer, RPCL2Point, + FieldDatatype, FromBytes, GetFieldDatatype, LayoutDescription, LayoutField, MsgConversionError, + PointCloud2Msg, PointConvertible, PointDataBuffer, RPCL2Point, }; pub use crate::points::*; @@ -11,7 +11,4 @@ pub use crate::ros::*; pub use rayon::prelude::*; #[cfg(feature = "derive")] -pub use type_layout::TypeLayout; - -#[cfg(feature = "derive")] -pub use rpcl2_derive::*; +pub use rpcl2_derive::*; \ No newline at end of file diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index 69fb781..81d2c9b 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -9,7 +9,6 @@ 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); @@ -30,7 +29,6 @@ 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()); @@ -59,7 +57,6 @@ fn write_cloud() { } #[test] -#[cfg(feature = "derive")] fn write_cloud_from_vec() { let cloud = vec![ PointXYZ::new(0.0, 1.0, 5.0), @@ -73,7 +70,6 @@ fn write_cloud_from_vec() { } #[test] -#[cfg(feature = "derive")] fn write_empty_cloud_vec() { let cloud: Vec = vec![]; let msg = PointCloud2Msg::try_from_vec(cloud); @@ -90,7 +86,7 @@ fn write_empty_cloud_iter() { } #[test] -#[cfg(all(feature = "derive", feature = "rayon"))] +#[cfg(feature = "rayon")] fn conv_cloud_par_iter() { let cloud = vec![ PointXYZ::new(0.0, 1.0, 5.0), @@ -110,7 +106,7 @@ fn conv_cloud_par_iter() { } #[test] -#[cfg(all(feature = "derive", feature = "rayon"))] +#[cfg(feature = "rayon")] fn conv_cloud_par_par_iter() { let cloud = vec![ PointXYZ::new(0.0, 1.0, 5.0), @@ -133,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, @@ -141,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![ @@ -164,7 +186,6 @@ fn custom_xyz_f32() { } #[test] -#[cfg(feature = "derive")] fn custom_xyzi_f32() { let cloud: Vec = vec![ CustomPointXYZI { @@ -193,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, @@ -202,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, @@ -220,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, @@ -375,7 +478,6 @@ fn converterxyzrgb() { } #[test] -#[cfg(feature = "derive")] fn converterxyzrgb_from_vec() { convert_from_into_vec!( PointXYZRGB, @@ -433,7 +535,6 @@ fn write_xyzi_read_xyz() { } #[test] -#[cfg(feature = "derive")] fn write_xyzi_read_xyz_vec() { let write_cloud = vec![ PointXYZI::new(0.0, 1.0, 5.0, 0.0), @@ -453,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, @@ -481,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, @@ -532,37 +634,4 @@ fn write_less_than_available() { ]; convert_from_into_in_out_cloud!(write_cloud, CustomPoint, read_cloud, CustomPoint); -} - -#[test] -#[cfg(feature = "derive")] -#[allow(unused_variables)] -fn readme() { - use ros_pointcloud2::prelude::*; - - // PointXYZ (and many others) are provided by the crate. - let cloud_points = vec![ - PointXYZI::new(91.486, -4.1, 42.0001, 0.1), - PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MIN), - ]; - - let out_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap(); - - // Convert the ROS crate message type, we will use r2r here. - // let msg: r2r::sensor_msgs::msg::PointCloud2 = out_msg.into(); - // Publish ... - // ... now incoming from a topic. - // let in_msg: PointCloud2Msg = msg.into(); - let in_msg = out_msg; - - let processed_cloud = in_msg - .try_into_iter() - .unwrap() - .map(|point: PointXYZ| { - // Define the info you want to have from the Msg. - // Some logic here ... - - point - }) - .collect::>(); -} +} \ No newline at end of file