From b155f416d1c65a8d6d607675935cd159164f8b00 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 30 Apr 2024 18:06:02 +0200
Subject: [PATCH 01/10] reduce to functions

---
 Cargo.toml                      |   8 +
 README.md                       |  50 ++--
 benches/roundtrip.rs            |  55 +++++
 examples/custom_label_filter.rs | 133 ++++++++++
 examples/distance_filter.rs     |  49 ++++
 src/convert.rs                  |   2 +-
 src/iterator.rs                 | 238 ++++++++++++++++++
 src/lib.rs                      | 271 +++++++++++++-------
 src/reader.rs                   | 422 --------------------------------
 src/ros_types.rs                |  13 -
 src/writer.rs                   | 346 --------------------------
 tests/e2e_test.rs               | 254 ++++++-------------
 tests/r2r_msg_test.rs           |   8 +-
 tests/rosrust_msg_test.rs       |   8 +-
 14 files changed, 765 insertions(+), 1092 deletions(-)
 create mode 100644 benches/roundtrip.rs
 create mode 100644 examples/custom_label_filter.rs
 create mode 100644 examples/distance_filter.rs
 create mode 100644 src/iterator.rs
 delete mode 100644 src/reader.rs
 delete mode 100644 src/writer.rs

diff --git a/Cargo.toml b/Cargo.toml
index a70ded8..cb7734d 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -18,6 +18,14 @@ rosrust_msg = { version = "0.1", optional = true }
 rosrust = { version = "0.9.11", optional = true }
 r2r = { version = "0.8.4", optional = true }
 
+[dev-dependencies]
+rand = "0.8"
+criterion = { version = "0.4", features = ["html_reports"] }
+
+[[bench]]
+name = "roundtrip"
+harness = false
+
 [features]
 rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
 r2r_msg = ["dep:r2r"]
\ No newline at end of file
diff --git a/README.md b/README.md
index 4bcd698..96eb8cc 100644
--- a/README.md
+++ b/README.md
@@ -7,48 +7,34 @@
 
 Providing an easy to use, generics defined, point-wise iterator abstraction over the byte buffer in `PointCloud2` to minimize iterations in your processing pipeline.
 
-To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `PointCloud2Msg`.
+To keep the crate a general purpose library for the problem, it uses its own type for the message `PointCloud2Msg`. ROS1 and ROS2 support is added with feature flags.
 
-## Examples
+## Quickstart
 ```rust
-use ros_pointcloud2::{
-  pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-};
+use ros_pointcloud2::{PointCloud2Msg, pcl_utils::PointXYZ};
 
-// Your points (here using the predefined type PointXYZ).
+// Your points (here using a predefined type PointXYZ).
 let cloud_points = vec![
   PointXYZ {x: 91.486, y: -4.1, z: 42.0001,},
   PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,},
 ];
 
-// For equality test later
-let cloud_copy = cloud_points.clone();
-
-// Vector -> Writer -> Message.
-// You can also just give the Vec or anything that implements `IntoIterator`.
-let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
-  .try_into() // iterating points here O(n)
-  .unwrap();
-
-// Convert to your ROS crate message type, we will use r2r here.
-// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
+// Give the Vec or anything that implements `IntoIterator`.
+let in_msg = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
 
+// Convert the ROS crate message type, we will use r2r here.
+// let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
 // Publish ...
-
 // ... now incoming from a topic.
-// let internal_msg: PointCloud2Msg = msg.into();
+// let in_msg: PointCloud2Msg = msg.into();
 
-// Message -> Reader -> your pipeline. The Reader implements the Iterator trait.
-let reader = ReaderXYZ::try_from(internal_msg).unwrap();
-let new_cloud_points = reader
-  .map(|point: PointXYZ| {
-    // Some logic here
+let new_pcl = in_msg.try_into_iterable().unwrap()
+  .map(|point: PointXYZ| { // Define the type of point here.
+    // Some logic here ...
 
-    point
-  })
-  .collect::<Vec<PointXYZ>>(); // iterating points here O(n)
-
-assert_eq!(new_cloud_points, cloud_copy);
+      point
+    })
+    .collect::<Vec<_>>();
 ```
 
 ## Integrations
@@ -87,11 +73,5 @@ Also, indicate the following dependencies to your linker inside the `package.xml
 
 Please open an issue or PR if you want to see support for other crates.
 
-## Future Work
-- Benchmark vs PCL
-- Add more predefined types
-- Optional derive macros for custom point implementations
-
-
 ## License
 [MIT](https://choosealicense.com/licenses/mit/)
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
new file mode 100644
index 0000000..4ebc753
--- /dev/null
+++ b/benches/roundtrip.rs
@@ -0,0 +1,55 @@
+use criterion::{black_box, criterion_group, criterion_main, Criterion};
+use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
+
+use rand::Rng;
+
+pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
+    let mut rng = rand::thread_rng();
+    let mut pointcloud = Vec::with_capacity(num_points);
+    for _ in 0..num_points {
+        let point = PointXYZ {
+            x: rng.gen_range(min..max),
+            y: rng.gen_range(min..max),
+            z: rng.gen_range(min..max),
+        };
+        pointcloud.push(point);
+    }
+    pointcloud
+}
+
+fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let total = internal_msg
+        .try_into_iter()
+        .unwrap()
+        .collect::<Vec<PointXYZ>>();
+    orig_len == total.len()
+}
+
+fn roundtrip_benchmark(c: &mut Criterion) {
+    let cloud_points_10k = generate_random_pointcloud(10_000, f32::MIN / 2.0, f32::MAX / 2.0);
+    let cloud_points_100k = generate_random_pointcloud(100_000, f32::MIN / 2.0, f32::MAX / 2.0);
+    let cloud_points_1m = generate_random_pointcloud(1_000_000, f32::MIN / 2.0, f32::MAX / 2.0);
+
+    c.bench_function("roundtrip 10k", |b| {
+        b.iter(|| {
+            black_box(roundtrip(cloud_points_10k.clone()));
+        })
+    });
+
+    c.bench_function("roundtrip 100k", |b| {
+        b.iter(|| {
+            black_box(roundtrip(cloud_points_100k.clone()));
+        })
+    });
+
+    c.bench_function("roundtrip 1m", |b| {
+        b.iter(|| {
+            black_box(roundtrip(cloud_points_1m.clone()));
+        })
+    });
+}
+
+criterion_group!(benches, roundtrip_benchmark);
+criterion_main!(benches);
diff --git a/examples/custom_label_filter.rs b/examples/custom_label_filter.rs
new file mode 100644
index 0000000..19b92ed
--- /dev/null
+++ b/examples/custom_label_filter.rs
@@ -0,0 +1,133 @@
+// This example demonstrates how to use custom point types with custom metadata.
+// 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::{size_of, MetaNames, Point, PointCloud2Msg, PointConvertible};
+
+#[derive(Debug, PartialEq, Clone)]
+enum Label {
+    Human,
+    Deer,
+    Car,
+}
+
+impl From<Label> for u8 {
+    fn from(label: Label) -> Self {
+        match label {
+            Label::Human => 0,
+            Label::Deer => 1,
+            Label::Car => 2,
+        }
+    }
+}
+
+impl From<u8> for Label {
+    fn from(label: u8) -> Self {
+        match label {
+            0 => Label::Human,
+            1 => Label::Deer,
+            2 => Label::Car,
+            _ => panic!("Invalid label"),
+        }
+    }
+}
+
+// Custom point type with custom metadata
+#[derive(Debug, PartialEq, Clone)]
+struct CustomPoint {
+    x: f32,
+    y: f32,
+    z: f32,
+    intensity: f32,
+    my_custom_label: Label,
+}
+
+// We implement the PointConvertible trait
+
+// A ros_pointcloud2::Point takes as generic arguments:
+// - coordinate type
+// - dimension (xyz = 3)
+// - metadata dimension (intensity + my_custom_label = 2)
+impl From<CustomPoint> for Point<f32, 3, 2> {
+    fn from(point: CustomPoint) -> Self {
+        Point {
+            coords: [point.x, point.y, point.z],
+            meta: [
+                point.intensity.into(),
+                u8::from(point.my_custom_label).into(), // encoding the label as u8
+            ],
+        }
+    }
+}
+
+impl From<Point<f32, 3, 2>> for CustomPoint {
+    fn from(point: Point<f32, 3, 2>) -> Self {
+        let label_raw: u8 = point.meta[1].get();
+        Self {
+            x: point.coords[0],
+            y: point.coords[1],
+            z: point.coords[2],
+            intensity: point.meta[0].get(),
+            my_custom_label: Label::from(label_raw), // decoding the label from u8
+        }
+    }
+}
+
+impl MetaNames<2> for CustomPoint {
+    fn meta_names() -> [&'static str; 2] {
+        ["intensity", "my_custom_label"] // how we want to name the metadata in the message
+    }
+}
+
+// We implemented everything that is needed so we declare it as a PointConvertible
+impl PointConvertible<f32, { size_of!(f32) }, 3, 2> 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_iterable(cloud.clone()).unwrap();
+
+    println!("filtering by label == Deer");
+    let out = msg
+        .try_into_iter()
+        .unwrap()
+        .filter(|point: &CustomPoint| point.my_custom_label == Label::Deer)
+        .collect::<Vec<_>>();
+
+    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/examples/distance_filter.rs b/examples/distance_filter.rs
new file mode 100644
index 0000000..23dc4fc
--- /dev/null
+++ b/examples/distance_filter.rs
@@ -0,0 +1,49 @@
+// This example demonstrates a very simple distance filter with predefined point types.
+// Note that this example is a simplified version of the custom_filter.rs example.
+// Also, it effectively demonstrates a typesafe byte-to-byte buffer filter with a single iteration.
+
+use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
+
+fn main() {
+    let cloud = vec![
+        PointXYZ {
+            x: 1.0,
+            y: 1.0,
+            z: 1.0,
+        },
+        PointXYZ {
+            x: 2.0,
+            y: 2.0,
+            z: 2.0,
+        },
+        PointXYZ {
+            x: 3.0,
+            y: 3.0,
+            z: 3.0,
+        },
+    ];
+
+    println!("Original cloud: {:?}", cloud);
+
+    let msg = PointCloud2Msg::try_from_iterable(cloud.clone()).unwrap();
+
+    println!("filtering by distance < 1.9m");
+    let out = msg
+        .try_into_iter()
+        .unwrap()
+        .filter(|point: &PointXYZ| {
+            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+        })
+        .collect::<Vec<_>>();
+
+    println!("Filtered cloud: {:?}", out);
+
+    assert_eq!(
+        vec![PointXYZ {
+            x: 1.0,
+            y: 1.0,
+            z: 1.0,
+        },],
+        out
+    );
+}
diff --git a/src/convert.rs b/src/convert.rs
index c5d055f..c8c7e3a 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -137,7 +137,7 @@ pub(crate) fn check_coord(
 ///
 /// This trait is needed to implement the `PointConvertible` trait.
 ///
-/// # Example for full point conversion.
+/// # Example
 /// ```
 /// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
 ///
diff --git a/src/iterator.rs b/src/iterator.rs
new file mode 100644
index 0000000..4fb07ad
--- /dev/null
+++ b/src/iterator.rs
@@ -0,0 +1,238 @@
+use crate::{
+    convert::{check_coord, load_loadable, Endianness, FieldDatatype, FromBytes},
+    ConversionError, MetaNames, Point, PointCloud2Msg, PointConvertible, PointMeta,
+};
+
+/// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg.
+///
+/// The iterator is defined at compile time, so the point has to be described via template arguments.
+///
+/// When using within a ROS node, the PointCloud2 (created by the ROS crate) must be converted first.
+/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
+///
+/// ROS1 with rosrust:
+/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
+/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
+///
+/// ROS2 with r2r:
+/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
+/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
+///
+/// ros_pointcloud2 supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags.
+///
+pub struct PointCloudIterator<
+    T: FromBytes,
+    const SIZE: usize,
+    const DIM: usize,
+    const METADIM: usize,
+    C,
+> where
+    T: FromBytes,
+    C: MetaNames<METADIM>,
+{
+    iteration: usize,
+    data: Vec<u8>,
+    point_step_size: usize,
+    cloud_length: usize,
+    offsets: Vec<usize>,
+    meta: Vec<(String, FieldDatatype)>,
+    endianness: Endianness,
+    phantom_t: std::marker::PhantomData<T>, // internally used for byte and datatype conversions
+    phantom_c: std::marker::PhantomData<C>, // internally used for meta names array
+}
+
+/// Implementation of the iterator trait.
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> Iterator
+    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes,
+    C: PointConvertible<T, SIZE, DIM, METADIM>,
+{
+    type Item = C;
+
+    /// The size_hint is the length of the remaining elements and the maximum length of the iterator.
+    ///
+    /// PointCloud2 messages contain the length of the cloud, so we can prepare coming operations.
+    /// This hint is used inside common iterator functions like `collect<Vec<_>>`, for example.
+    fn size_hint(&self) -> (usize, Option<usize>) {
+        (self.cloud_length - self.iteration, Some(self.cloud_length))
+    }
+
+    /// Get the data from the byte buffer and convert it to the predefined point.
+    /// It must keep track of the current iteration and the length of the cloud so it has to mutate self.
+    ///
+    /// The current point is then converted into the custom type. If the conversion fails, an error is returned.
+    fn next(&mut self) -> Option<Self::Item> {
+        if self.iteration >= self.cloud_length {
+            return None; // iteration finished
+        }
+
+        let mut xyz = [T::default(); DIM];
+        xyz.iter_mut()
+            .zip(self.offsets.iter())
+            .for_each(|(p_xyz, in_point_offset)| {
+                *p_xyz = load_loadable::<T, SIZE>(
+                    (self.iteration * self.point_step_size) + in_point_offset,
+                    &self.data,
+                    &self.endianness,
+                );
+            });
+
+        debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
+        debug_assert!(
+            self.offsets.len() == DIM + METADIM,
+            "Offset length mismatch"
+        );
+
+        let mut meta = [PointMeta::default(); METADIM];
+        meta.iter_mut()
+            .zip(self.offsets.iter().skip(DIM))
+            .zip(self.meta.iter())
+            .for_each(|((p_meta, in_point_offset), (_, meta_type))| {
+                let start = (self.iteration * self.point_step_size) + in_point_offset;
+                *p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
+            });
+
+        self.iteration += 1;
+
+        Some(C::from(Point { coords: xyz, meta }))
+    }
+}
+
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryFrom<PointCloud2Msg>
+    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes,
+    C: MetaNames<METADIM>,
+{
+    type Error = ConversionError;
+
+    /// Convert a PointCloud2Msg into an iterator.
+    /// Converting a PointCloud2Msg into an iterator is a fallible operation since the message can contain only a subset of the required fields.
+    ///
+    /// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
+    /// It therefore has a constant time complexity O(1) for practical purposes.
+    fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
+        if cloud.fields.len() < DIM {
+            return Err(ConversionError::NotEnoughFields);
+        }
+
+        let xyz_field_type = T::field_datatype();
+
+        let mut has_x: Option<usize> = None;
+        let mut has_y: Option<usize> = None;
+        let mut has_z: Option<usize> = None;
+
+        let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); METADIM];
+
+        let lower_meta_names = C::meta_names()
+            .iter()
+            .map(|x| x.to_lowercase())
+            .collect::<Vec<String>>();
+
+        for (idx, field) in cloud.fields.iter().enumerate() {
+            let lower_field_name = field.name.to_lowercase();
+            match lower_field_name.as_str() {
+                "x" => has_x = Some(idx),
+                "y" => has_y = Some(idx),
+                "z" => has_z = Some(idx),
+                _ => {
+                    if lower_meta_names.contains(&lower_field_name) {
+                        let meta_idx = idx - DIM;
+                        debug_assert!(
+                            meta_idx < meta_with_offsets.len(),
+                            "Meta data length mismatch"
+                        );
+                        meta_with_offsets[meta_idx].0.clone_from(&field.name);
+                        meta_with_offsets[meta_idx].1 = field.datatype.try_into()?;
+                        meta_with_offsets[meta_idx].2 = field.offset as usize;
+                    }
+                }
+            }
+        }
+
+        meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
+
+        debug_assert!(
+            meta_with_offsets.len() == METADIM,
+            "Meta data length mismatch"
+        );
+
+        let mut meta_offsets = [usize::default(); METADIM];
+        let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM];
+
+        meta_with_offsets
+            .into_iter()
+            .zip(meta.iter_mut())
+            .zip(meta_offsets.iter_mut())
+            .for_each(|(((name, datatype, offset), meta), meta_offset)| {
+                *meta = (name, datatype);
+                *meta_offset = offset;
+            });
+
+        let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
+        let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?;
+
+        let mut offsets = vec![x_field.offset as usize, y_field.offset as usize];
+
+        let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type);
+        match z_field {
+            Ok(z_field) => {
+                offsets.push(z_field.offset as usize);
+            }
+            Err(err) => match err {
+                ConversionError::NotEnoughFields => {
+                    if DIM == 3 {
+                        return Err(ConversionError::NotEnoughFields);
+                    }
+                }
+                _ => return Err(err),
+            },
+        }
+
+        let endian = if cloud.is_bigendian {
+            Endianness::Big
+        } else {
+            Endianness::Little
+        };
+
+        if offsets.len() != DIM {
+            return Err(ConversionError::NotEnoughFields);
+        }
+
+        offsets.extend(meta_offsets);
+
+        if offsets.len() != DIM + METADIM {
+            return Err(ConversionError::NotEnoughFields);
+        }
+
+        let point_step_size = cloud.point_step as usize;
+        let cloud_length = cloud.width as usize * cloud.height as usize;
+        if point_step_size * cloud_length != cloud.data.len() {
+            return Err(ConversionError::DataLengthMismatch);
+        }
+
+        let last_offset = offsets.last().expect("Dimensionality is 0.");
+
+        if let Some(last_meta) = meta.last() {
+            let size_with_last_meta = last_offset + last_meta.1.size();
+            if size_with_last_meta > point_step_size {
+                return Err(ConversionError::DataLengthMismatch);
+            }
+        } else if last_offset + xyz_field_type.size() > point_step_size {
+            return Err(ConversionError::DataLengthMismatch);
+        }
+
+        Ok(Self {
+            iteration: 0,
+            data: cloud.data,
+            point_step_size,
+            cloud_length: cloud.width as usize * cloud.height as usize,
+            offsets,
+            meta,
+            endianness: endian,
+            phantom_t: std::marker::PhantomData,
+            phantom_c: std::marker::PhantomData,
+        })
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index dd868a1..c4d5d93 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,27 +1,29 @@
 //! A library to work with the PointCloud2 message type from ROS.
 //!
-//! ros_pointcloud2 mainly provides iterator-based [`ros_pointcloud2::reader`] and [`ros_pointcloud2::writer`] to interact
-//! with the internal message type [`ros_pointcloud2::PointCloud2Msg`] without forcing an iteration.
+//! ros_pointcloud2 mainly provides a [`ros_pointcloud2::PointCloud2Msg`] type that implements
+//! functions for conversion to and from iterators.
+//!
+//! - [`ros_pointcloud2::PointCloud2Msg::try_from_iterable`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_into_iter`]
 //!
 //! For ROS interoperability, the message type generated by the respective crate must be converted to
 //! the [`ros_pointcloud2::PointCloud2Msg`] using the `From` trait,
-//! which is mostly a ownership transfer without copying the point data.
+//! which mostly does ownership transfers without copying the point data.
 //!
 //! There are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust_msg` message types
 //! available in the feature flags. If you miss a message type, please open an issue or a PR.
 //! See the [`ros_pointcloud2::ros_types`] module for more information.
 //!
-//! Common point types like `PointXYZ` are provided in the [`ros_pointcloud2::pcl_utils`] module. You can implement any custom point type
+//! Common point types like [`ros_pointcloud2::pcl_utils::PointXYZ`] or
+//! [`ros_pointcloud2::pcl_utils::PointXYZI`] are provided in the
+//! [`ros_pointcloud2::pcl_utils`] module. You can implement any custom point type
 //! that can be described by the specification.
-//! See the [`ros_pointcloud2::Reader`] or [`ros_pointcloud2::Writer`] struct documentation for an example.
 //!
-//! # Example
+//! # Example PointXYZ
 //! ```
-//! use ros_pointcloud2::{
-//!   pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-//! };
+//! use ros_pointcloud2::{PointCloud2Msg, pcl_utils::PointXYZ};
 //!
-//! // Your points (here using the predefined type PointXYZ).
+//! // PointXYZ is predefined
 //! let cloud_points = vec![
 //!   PointXYZ {x: 9.0006, y: 42.0, z: -6.2,},
 //!   PointXYZ {x: f32::MAX, y: f32::MIN,z: f32::MAX,},
@@ -30,31 +32,24 @@
 //! // For equality test later
 //! let cloud_copy = cloud_points.clone();
 //!
-//! // Vector -> Writer -> Message.
-//! // You can also just give the Vec or anything that implements `IntoIterator`.
-//! let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
-//!     .try_into() // iterating points here O(n)
-//!     .unwrap();
+//! // Give the Vec or anything that implements `IntoIterator`.
+//! let in_msg = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
 //!
 //! // Convert to your ROS crate message type, we will use r2r here.
-//! // let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
-//!
+//! // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
 //! // Publish ...
-//!
 //! // ... now incoming from a topic.
-//! // let internal_msg: PointCloud2Msg = msg.into();
+//! // let in_msg: PointCloud2Msg = msg.into();
 //!
-//! // Message -> Reader -> your pipeline. The Reader implements the Iterator trait.
-//! let reader = ReaderXYZ::try_from(internal_msg).unwrap();
-//! let new_cloud_points = reader
-//!     .map(|point: PointXYZ| {
+//! let new_pcl = in_msg.try_into_iter().unwrap()
+//!     .map(|point: PointXYZ| { // Define the data you want from the point
 //!         // Some logic here
 //!
 //!         point
 //!     })
-//!     .collect::<Vec<PointXYZ>>(); // iterating points here O(n)
+//!     .collect::<Vec<_>>(); // iterating points here O(n)
 //!
-//! assert_eq!(new_cloud_points, cloud_copy);
+//! assert_eq!(new_pcl, cloud_copy);
 //! ```
 
 pub mod convert;
@@ -62,15 +57,7 @@ pub mod pcl_utils;
 pub mod ros_types;
 
 /// Macro to get the size of a type at compile time. This is a convenience macro to avoid writing out the full std::mem::size_of::<T>().
-/// Use it for your custom Reader and Writer implementations.
-/// # Example
-/// ```
-/// use ros_pointcloud2::{
-///     writer::Writer, size_of, pcl_utils::PointXYZ,
-/// };
-///
-/// type MyWriterXYZ = Writer<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
-/// ```
+/// Use it for your custom point conversion implementations with the [`ros_pointcloud2::PointConvertible`] trait.
 #[macro_export]
 macro_rules! size_of {
     ($t:ty) => {
@@ -78,20 +65,12 @@ macro_rules! size_of {
     };
 }
 
-pub mod reader;
-pub mod writer;
-
+pub mod iterator;
+use crate::convert::{FieldDatatype, FromBytes};
+use crate::ros_types::{HeaderMsg, PointFieldMsg};
 pub use convert::MetaNames;
-pub use ros_types::PointCloud2Msg;
-
-use crate::convert::{
-    FieldDatatype,
-    FromBytes,
-};
 
-use crate::ros_types::PointFieldMsg;
-
-/// All errors that can occur for creating Reader and Writer.
+/// All errors that can occur while converting to or from the PointCloud2 message.
 #[derive(Debug)]
 pub enum ConversionError {
     /// The coordinate field does not match the expected datatype.
@@ -113,50 +92,176 @@ pub enum ConversionError {
     DataLengthMismatch,
 }
 
+#[inline(always)]
+fn add_point_to_byte_buffer<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
+    coords: C,
+    cloud: &mut PointCloud2Msg,
+) -> Result<bool, ConversionError>
+where
+    C: PointConvertible<T, SIZE, DIM, METADIM>,
+    T: FromBytes,
+{
+    let point: Point<T, DIM, METADIM> = coords.into();
+
+    // (x, y, z...)
+    point
+        .coords
+        .iter()
+        .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
+
+    // meta data description
+    point.meta.iter().for_each(|meta| {
+        let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
+        cloud.data.extend_from_slice(truncated_bytes);
+    });
+
+    cloud.width += 1;
+
+    Ok(true)
+}
+
+#[derive(Clone, Debug, Default)]
+pub struct PointCloud2Msg {
+    pub header: HeaderMsg,
+    pub height: u32,
+    pub width: u32,
+    pub fields: Vec<PointFieldMsg>,
+    pub is_bigendian: bool,
+    pub point_step: u32,
+    pub row_step: u32,
+    pub data: Vec<u8>,
+    pub is_dense: bool,
+}
+
+impl PointCloud2Msg {
+    /// Create a PointCloud2Msg from any iterable type.
+    ///
+    /// The operation is O(n) in time complexity where n is the number of points in the point cloud.
+    ///
+    /// # Example
+    /// ```
+    /// use ros_pointcloud2::{
+    ///     pcl_utils::PointXYZ, PointCloud2Msg,
+    /// };
+    ///
+    /// let cloud_points: Vec<PointXYZ> = vec![
+    ///     PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
+    ///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
+    /// ];
+    ///
+    // let msg_out = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
+    /// ```
+    pub fn try_from_iterable<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
+        iterable: impl IntoIterator<Item = C>,
+    ) -> Result<Self, ConversionError>
+    where
+        C: PointConvertible<T, SIZE, DIM, METADIM>,
+        T: FromBytes,
+    {
+        if DIM > 3 {
+            return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time?
+        }
+
+        let mut iter = iterable.into_iter();
+        let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack
+
+        if DIM > 1 {
+            fields.push(PointFieldMsg {
+                name: "x".into(),
+                offset: 0,
+                datatype: T::field_datatype().into(),
+                count: 1,
+            });
+
+            fields.push(PointFieldMsg {
+                name: "y".into(),
+                offset: SIZE as u32,
+                datatype: T::field_datatype().into(),
+                count: 1,
+            });
+        }
+
+        if DIM == 3 {
+            fields.push(PointFieldMsg {
+                name: "z".into(),
+                offset: 2 * SIZE as u32,
+                datatype: T::field_datatype().into(),
+                count: 1,
+            });
+        }
+
+        let first_point = iter.next().ok_or(ConversionError::NoPoints)?;
+        let point: Point<T, DIM, METADIM> = first_point.clone().into();
+        let meta_names = C::meta_names();
+
+        let mut meta_offsets_acc = DIM as u32 * SIZE as u32;
+        for (meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
+            let datatype_code = meta_value.datatype.into();
+            if FieldDatatype::try_from(datatype_code).is_err() {
+                return Err(ConversionError::UnsupportedFieldType);
+            }
+
+            fields.push(PointFieldMsg {
+                name: meta_name.into(),
+                offset: meta_offsets_acc,
+                datatype: datatype_code,
+                count: 1,
+            });
+            meta_offsets_acc += meta_value.datatype.size() as u32
+        }
+
+        let mut cloud = PointCloud2Msg {
+            point_step: fields.iter().fold(Default::default(), |acc, field| {
+                let field_type: FieldDatatype = field
+                    .datatype
+                    .try_into()
+                    .expect("Unsupported field but checked before.");
+                let field_size = field_type.size();
+                acc + field.count * field_size as u32
+            }),
+            ..Default::default()
+        };
+
+        // actual point -> byte conversion -- O(n)
+        add_point_to_byte_buffer(first_point, &mut cloud)?;
+        for coords in iter {
+            add_point_to_byte_buffer(coords, &mut cloud)?;
+        }
+
+        cloud.fields = fields;
+        cloud.height = 1; // everything is in one row (unstructured)
+        cloud.is_bigendian = false; // ROS default
+        cloud.is_dense = true; // ROS default
+        cloud.row_step = cloud.width * cloud.point_step; // Note: redundant but defined in PointCloud2 message
+
+        Ok(cloud)
+    }
+
+    pub fn try_into_iter<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
+        self,
+    ) -> Result<impl Iterator<Item = C>, ConversionError>
+    where
+        C: PointConvertible<T, SIZE, DIM, METADIM>,
+        T: FromBytes,
+    {
+        iterator::PointCloudIterator::try_from(self)
+    }
+}
+
 /// Internal point representation. It is used to store the coordinates and meta data of a point.
-/// In each iteration, the Reader will convert the internal representation to the desired point type.
-/// Implement the `From` traits for your point type to use the conversion as part of the [`PointConvertible`] trait.
-///
-/// # Example
-/// ```
-/// use ros_pointcloud2::Point;
+/// In each iteration, an internal point representation is converted to the desired point type.
+/// Implement the `From` traits for your point type to use the conversion.
 ///
-/// #[derive(Clone, Debug, PartialEq, Copy)]
-/// pub struct MyPointXYZI {
-///     pub x: f32,
-///     pub y: f32,
-///     pub z: f32,
-///     pub intensity: f32,
-/// }
-///
-/// impl From<MyPointXYZI> for Point<f32, 3, 1> {
-///     fn from(point: MyPointXYZI) -> Self {
-///         Point {
-///             coords: [point.x, point.y, point.z],
-///             meta: [point.intensity.into()],
-///         }
-///     }
-/// }
-///
-/// impl From<Point<f32, 3, 1>> for MyPointXYZI {
-///     fn from(point: Point<f32, 3, 1>) -> Self {
-///         Self {
-///             x: point.coords[0],
-///             y: point.coords[1],
-///             z: point.coords[2],
-///             intensity: point.meta[0].get(),
-///         }
-///     }
-/// }
-/// ```
+/// See the [`ros_pointcloud2::PointConvertible`] trait for more information.
 pub struct Point<T, const DIM: usize, const METADIM: usize> {
     pub coords: [T; DIM],
     pub meta: [PointMeta; METADIM],
 }
 
-/// Trait to convert a point to a tuple of coordinates and meta data.
-/// Implement this trait for your point type to use the conversion.
-/// 
+/// Trait to enable point conversions on the fly while iterating.
+///
+/// Implement this trait for your custom point you want to read or write in the message.
+///
 /// # Example
 /// ```
 /// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
diff --git a/src/reader.rs b/src/reader.rs
deleted file mode 100644
index 61aa956..0000000
--- a/src/reader.rs
+++ /dev/null
@@ -1,422 +0,0 @@
-use crate::{
-    pcl_utils::*,
-    Point,
-    PointCloud2Msg,
-    PointConvertible,
-    ConversionError,
-    MetaNames,
-    PointMeta,
-    convert::{
-        FromBytes,
-        FieldDatatype,
-        load_loadable,
-        Endianness,
-        check_coord,
-    },
-};
-
-/// Convenience type for a Reader that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderF32<const DIM: usize, const METADIM: usize, C> =
-    Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Reader that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderF64<const DIM: usize, const METADIM: usize, C> =
-    Reader<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Reader that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderI8<const DIM: usize, const METADIM: usize, C> =
-    Reader<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Reader that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderI16<const DIM: usize, const METADIM: usize, C> =
-    Reader<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Reader that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderI32<const DIM: usize, const METADIM: usize, C> =
-    Reader<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Reader that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderU8<const DIM: usize, const METADIM: usize, C> =
-    Reader<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Reader that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderU16<const DIM: usize, const METADIM: usize, C> =
-    Reader<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Reader that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type ReaderU32<const DIM: usize, const METADIM: usize, C> =
-    Reader<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
-
-/// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`).
-/// Every additional meta data is ignored.
-pub type ReaderXYZ = ReaderF32<3, 0, PointXYZ>;
-
-/// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`).
-/// Every additional meta data is ignored.
-pub type ReaderXYZI = ReaderF32<3, 1, PointXYZI>;
-
-/// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`).
-/// Every additional meta data is ignored.
-pub type ReaderXYZNormal = ReaderF32<3, 3, PointXYZNormal>;
-
-/// Provides the message as an Iterator over xyz coordinates and unpacked rgb (see `PointXYZRGB`).
-/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
-/// Every additional meta data is ignored.
-pub type ReaderXYZRGB = ReaderF32<3, 1, PointXYZRGB>;
-
-/// Provides the message as an Iterator over xyz coordinates and unpacked rgb and intensity (see `PointXYZRGBL`).
-/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
-/// Every additional meta data is ignored.
-pub type ReaderXYZRGBL = ReaderF32<3, 2, PointXYZRGBL>;
-
-/// Provides the message as an Iterator over xyz coordinates and unpacked rgb with additional alpha channel (see `PointXYZRGBA`).
-/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
-/// Every additional meta data is ignored.
-pub type ReaderXYZRGBA = ReaderF32<3, 2, PointXYZRGBA>;
-
-/// Provides the message as an Iterator over xyz coordinates and normal and unpacked rgb (see `PointXYZRGBNormal`).
-/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
-/// Every additional meta data is ignored.
-pub type ReaderXYZRGBNormal = ReaderF32<3, 4, PointXYZRGBNormal>;
-
-/// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
-/// Every additional meta data is ignored.
-pub type ReaderXYZINormal = ReaderF32<3, 4, PointXYZINormal>;
-
-/// Provides the message as an Iterator over xyz coordinates and intensity with additional label (see `PointXYZL`).
-/// Every additional meta data is ignored.
-/// The label is typically used for semantic segmentation.
-pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>;
-
-/// The Reader provides a an iterator abstraction of the PointCloud2Msg.
-///
-/// The iterator is defined at compile time, so the point has to be described via template arguments.
-///
-/// # Predefined Readers
-/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
-///
-/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
-/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
-///
-/// ROS1 with rosrust:
-/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
-/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
-///
-/// ROS2 with r2r:
-/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
-/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
-///
-/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
-///
-/// ## Example
-/// ```
-/// use ros_pointcloud2::{
-///     pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-/// };
-///
-///
-/// let cloud_points: Vec<PointXYZ> = vec![
-///     PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
-///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
-/// ];
-///
-/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
-/// let convert = ReaderXYZ::try_from(msg).unwrap();
-/// //            ^^^^^^^^^ conversion from PointCloud2Msg to Reader that implements Iterator
-///
-/// convert.for_each(|point: PointXYZ| {
-///     // do something with the point
-/// });
-/// ```
-/// # Fully Custom Reader
-/// When the predefined types are not enough (like sensor specific metadata), you can describe your Reader with the following template arguments:
-/// - T: The coordinate type, e.g. f32
-/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
-/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
-/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
-/// Afterwards, implement the PointConvertible trait for your custom point type.
-///
-/// ## Example
-/// ```
-/// use ros_pointcloud2::{
-///     reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
-/// };
-///
-/// type Xyz = f32; // coordinate type
-/// const XYZ_S: usize = size_of!(Xyz);
-/// const DIM: usize = 3; // helper constant for dimensionality
-/// const METADIM: usize = 1; // helper constant for meta data fields
-///
-/// #[derive(Debug, PartialEq, Clone)]
-/// struct CustomPoint {
-///     x: f32,
-///     y: f32,
-///     z: f32,
-///     i: u8,
-/// }
-///
-/// impl From<Point<f32, 3, 1>> for CustomPoint {
-///     fn from(point: Point<f32, 3, 1>) -> Self {
-///         Self {
-///             x: point.coords[0],
-///             y: point.coords[1],
-///             z: point.coords[2],
-///             i: point.meta[0].get(),
-///         }
-///     }
-/// }
-///
-///impl From<CustomPoint> for Point<f32, 3, 1> {
-///    fn from(point: CustomPoint) -> Self {
-///        Point {
-///            coords: [point.x, point.y, point.z],
-///            meta: [
-///                point.i.into(),
-///            ],
-///        }
-///    }
-///}
-///
-/// impl MetaNames<METADIM> for CustomPoint {
-///     fn meta_names() -> [&'static str; METADIM] {
-///         ["intensity"]
-///     }
-/// }
-///
-/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
-///
-/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
-/// //   ^^^^^^^^ your custom Reader
-/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
-/// ```
-pub struct Reader<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
-where
-    T: FromBytes,
-    C: MetaNames<METADIM>,
-{
-    iteration: usize,
-    data: Vec<u8>,
-    point_step_size: usize,
-    cloud_length: usize,
-    offsets: Vec<usize>,
-    meta: Vec<(String, FieldDatatype)>,
-    endianness: Endianness,
-    phantom_t: std::marker::PhantomData<T>, // internally used for byte and datatype conversions
-    phantom_c: std::marker::PhantomData<C>, // internally used for meta names array
-}
-
-/// The iterator implementation for the Reader struct.
-/// The iterator is fallible because the data is read from a byte buffer inside the PointCloud2 message, which is inherently unsafe.
-///
-/// See ConversionError for possible errors that can occur during iteration.
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> Iterator
-    for Reader<T, SIZE, DIM, METADIM, C>
-where
-    T: FromBytes,
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
-{
-    type Item = C;
-
-    /// The size_hint is the length of the remaining elements and the maximum length of the iterator.
-    ///
-    /// PointCloud2 messages contain the length of the cloud, so we can prepare coming operations.
-    /// This hint is used inside common iterator functions like `collect<Vec<_>>`, for example.
-    fn size_hint(&self) -> (usize, Option<usize>) {
-        (self.cloud_length - self.iteration, Some(self.cloud_length))
-    }
-
-    /// Get the data from the byte buffer and convert it to the predefined point.
-    /// It must keep track of the current iteration and the length of the cloud so it has to mutate self.
-    ///
-    /// The current point is then converted into the custom type. If the conversion fails, an error is returned.
-    fn next(&mut self) -> Option<Self::Item> {
-        if self.iteration >= self.cloud_length {
-            return None; // iteration finished
-        }
-
-        let mut xyz = [T::default(); DIM];
-        xyz.iter_mut()
-            .zip(self.offsets.iter())
-            .for_each(|(p_xyz, in_point_offset)| {
-                *p_xyz = load_loadable::<T, SIZE>(
-                    (self.iteration * self.point_step_size) + in_point_offset,
-                    &self.data,
-                    &self.endianness,
-                );
-            });
-
-        debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
-        debug_assert!(
-            self.offsets.len() == DIM + METADIM,
-            "Offset length mismatch"
-        );
-
-        let mut meta = [PointMeta::default(); METADIM];
-        meta.iter_mut()
-            .zip(self.offsets.iter().skip(DIM))
-            .zip(self.meta.iter())
-            .for_each(|((p_meta, in_point_offset), (_, meta_type))| {
-                let start = (self.iteration * self.point_step_size) + in_point_offset;
-                *p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
-            });
-
-        self.iteration += 1;
-
-        Some(C::from(Point { coords: xyz, meta }))
-    }
-}
-
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryFrom<PointCloud2Msg>
-    for Reader<T, SIZE, DIM, METADIM, C>
-where
-    T: FromBytes,
-    C: MetaNames<METADIM>,
-{
-    type Error = ConversionError;
-
-    /// Convert a PointCloud2Msg into a Reader.
-    /// Converting a PointCloud2Msg into a Reader is a fallible operation since the message can contain only a subset of the required fields.
-    ///
-    /// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
-    /// It therefore has effectively a constant time complexity O(1) for practical purposes.
-    ///
-    /// # Example
-    /// ```
-    /// use ros_pointcloud2::{
-    ///     pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-    /// };
-    ///
-    /// let cloud_points: Vec<PointXYZ> = vec![
-    ///     PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
-    ///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
-    /// ];
-    ///
-    /// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
-    ///
-    /// let convert = ReaderXYZ::try_from(msg).unwrap();
-    /// //                       ^^^^^^^^ conversion from PointCloud2Msg to Reader
-    /// ```
-    fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
-        if cloud.fields.len() < DIM {
-            return Err(ConversionError::NotEnoughFields);
-        }
-
-        let xyz_field_type = T::field_datatype();
-
-        let mut has_x: Option<usize> = None;
-        let mut has_y: Option<usize> = None;
-        let mut has_z: Option<usize> = None;
-
-        let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); METADIM];
-
-        let lower_meta_names = C::meta_names()
-            .iter()
-            .map(|x| x.to_lowercase())
-            .collect::<Vec<String>>();
-
-        for (idx, field) in cloud.fields.iter().enumerate() {
-            let lower_field_name = field.name.to_lowercase();
-            match lower_field_name.as_str() {
-                "x" => has_x = Some(idx),
-                "y" => has_y = Some(idx),
-                "z" => has_z = Some(idx),
-                _ => {
-                    if lower_meta_names.contains(&lower_field_name) {
-                        let meta_idx = idx - DIM;
-                        debug_assert!(
-                            meta_idx < meta_with_offsets.len(),
-                            "Meta data length mismatch"
-                        );
-                        meta_with_offsets[meta_idx].0 = field.name.clone();
-                        meta_with_offsets[meta_idx].1 = field.datatype.try_into()?;
-                        meta_with_offsets[meta_idx].2 = field.offset as usize;
-                    }
-                }
-            }
-        }
-
-        meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
-
-        debug_assert!(
-            meta_with_offsets.len() == METADIM,
-            "Meta data length mismatch"
-        );
-
-        let mut meta_offsets = [usize::default(); METADIM];
-        let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM];
-
-        meta_with_offsets
-            .into_iter()
-            .zip(meta.iter_mut())
-            .zip(meta_offsets.iter_mut())
-            .for_each(|(((name, datatype, offset), meta), meta_offset)| {
-                *meta = (name, datatype);
-                *meta_offset = offset;
-            });
-
-        let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
-        let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?;
-
-        let mut offsets = vec![x_field.offset as usize, y_field.offset as usize];
-
-        let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type);
-        match z_field {
-            Ok(z_field) => {
-                offsets.push(z_field.offset as usize);
-            }
-            Err(err) => match err {
-                ConversionError::NotEnoughFields => {
-                    if DIM == 3 {
-                        return Err(ConversionError::NotEnoughFields);
-                    }
-                }
-                _ => return Err(err),
-            },
-        }
-
-        let endian = if cloud.is_bigendian {
-            Endianness::Big
-        } else {
-            Endianness::Little
-        };
-
-        if offsets.len() != DIM {
-            return Err(ConversionError::NotEnoughFields);
-        }
-
-        offsets.extend(meta_offsets);
-
-        if offsets.len() != DIM + METADIM {
-            return Err(ConversionError::NotEnoughFields);
-        }
-
-        let point_step_size = cloud.point_step as usize;
-        let cloud_length = cloud.width as usize * cloud.height as usize;
-        if point_step_size * cloud_length != cloud.data.len() {
-            return Err(ConversionError::DataLengthMismatch);
-        }
-
-        let last_offset = offsets.last().expect("Dimensionality is 0.");
-
-        if let Some(last_meta) = meta.last() {
-            let size_with_last_meta = last_offset + last_meta.1.size();
-            if size_with_last_meta > point_step_size {
-                return Err(ConversionError::DataLengthMismatch);
-            }
-        } else if last_offset + xyz_field_type.size() > point_step_size {
-            return Err(ConversionError::DataLengthMismatch);
-        }
-
-        Ok(Self {
-            iteration: 0,
-            data: cloud.data,
-            point_step_size,
-            cloud_length: cloud.width as usize * cloud.height as usize,
-            offsets,
-            meta,
-            endianness: endian,
-            phantom_t: std::marker::PhantomData,
-            phantom_c: std::marker::PhantomData,
-        })
-    }
-}
diff --git a/src/ros_types.rs b/src/ros_types.rs
index 0e2bbfc..c124fb0 100644
--- a/src/ros_types.rs
+++ b/src/ros_types.rs
@@ -23,19 +23,6 @@ pub struct PointFieldMsg {
     pub count: u32,
 }
 
-#[derive(Clone, Debug, Default)]
-pub struct PointCloud2Msg {
-    pub header: HeaderMsg,
-    pub height: u32,
-    pub width: u32,
-    pub fields: Vec<PointFieldMsg>,
-    pub is_bigendian: bool,
-    pub point_step: u32,
-    pub row_step: u32,
-    pub data: Vec<u8>,
-    pub is_dense: bool,
-}
-
 #[cfg(feature = "r2r_msg")]
 impl From<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
     fn from(msg: r2r::sensor_msgs::msg::PointCloud2) -> Self {
diff --git a/src/writer.rs b/src/writer.rs
deleted file mode 100644
index 3e2ddbe..0000000
--- a/src/writer.rs
+++ /dev/null
@@ -1,346 +0,0 @@
-use crate::{
-    pcl_utils::*,
-    Point,
-    PointCloud2Msg,
-    PointConvertible,
-    ConversionError,
-    ros_types::PointFieldMsg,
-    convert::{
-        FromBytes,
-        FieldDatatype,
-    },
-};
-
-/// Convenience type for a Writer that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterF32<const DIM: usize, const METADIM: usize, C> =
-    Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Writer that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterF64<const DIM: usize, const METADIM: usize, C> =
-    Writer<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Writer that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterI8<const DIM: usize, const METADIM: usize, C> =
-    Writer<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Writer that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterI16<const DIM: usize, const METADIM: usize, C> =
-    Writer<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Writer that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterI32<const DIM: usize, const METADIM: usize, C> =
-    Writer<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Writer that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterU8<const DIM: usize, const METADIM: usize, C> =
-    Writer<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Writer that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterU16<const DIM: usize, const METADIM: usize, C> =
-    Writer<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
-
-/// Convenience type for a Writer that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
-pub type WriterU32<const DIM: usize, const METADIM: usize, C> =
-    Writer<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`).
-pub type WriterXYZ = WriterF32<3, 0, PointXYZ>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`).
-pub type WriterXYZI = WriterF32<3, 1, PointXYZI>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`).
-pub type WriterXYZNormal = WriterF32<3, 3, PointXYZNormal>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`).
-pub type WriterXYZRGB = WriterF32<3, 1, PointXYZRGB>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`).
-pub type WriterXYZRGBL = WriterF32<3, 2, PointXYZRGBL>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels and alpha channel (see `PointXYZRGBA`).
-pub type WriterXYZRGBA = WriterF32<3, 2, PointXYZRGBA>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`).
-pub type WriterXYZRGBNormal = WriterF32<3, 4, PointXYZRGBNormal>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
-pub type WriterXYZINormal = WriterF32<3, 4, PointXYZINormal>;
-
-/// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`).
-pub type WriterXYZL = WriterF32<3, 1, PointXYZL>;
-
-/// The Write creates a PointCloud2Msg out of your point iterator.
-///
-/// The iterator is defined at compile time, so the Point has to be described via template arguments.
-///
-/// # Predefined Readers
-/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
-///
-/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
-/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
-///
-/// ROS1 with rosrust:
-/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
-/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
-///
-/// ROS2 with r2r:
-/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
-/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
-///
-/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
-///
-/// ## Example
-/// ```
-/// use ros_pointcloud2::{
-///     pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
-/// };
-///
-///
-/// let cloud_points: Vec<PointXYZ> = vec![
-///     PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
-///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
-/// ];
-///
-/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
-/// //                        ^^^^^^^^^ creating PointCloud2Msg from an iterator
-/// ```
-/// # Fully Custom Writer
-/// When the predefined types are not enough (like sensor specific metadata), you can describe your Writer with the following template arguments:
-/// - T: The coordinate type, e.g. f32
-/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
-/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
-/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
-/// Afterwards, implement the PointConvertible trait for your custom point type.
-///
-/// ## Example
-/// ```
-/// use ros_pointcloud2::{
-///     reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
-/// };
-///
-/// type Xyz = f32; // coordinate type
-/// const XYZ_S: usize = size_of!(Xyz);
-/// const DIM: usize = 3; // helper constant for dimensionality
-/// const METADIM: usize = 1; // helper constant for meta data fields
-///
-/// #[derive(Debug, PartialEq, Clone)]
-/// struct CustomPoint {
-///     x: f32,
-///     y: f32,
-///     z: f32,
-///     i: u8,
-/// }
-///
-/// impl From<Point<f32, 3, 1>> for CustomPoint {
-///     fn from(point: Point<f32, 3, 1>) -> Self {
-///         Self {
-///             x: point.coords[0],
-///             y: point.coords[1],
-///             z: point.coords[2],
-///             i: point.meta[0].get(),
-///         }
-///     }
-/// }
-///
-///impl From<CustomPoint> for Point<f32, 3, 1> {
-///    fn from(point: CustomPoint) -> Self {
-///        Point {
-///            coords: [point.x, point.y, point.z],
-///            meta: [
-///                point.i.into(),
-///            ],
-///        }
-///    }
-///}
-///
-/// impl MetaNames<METADIM> for CustomPoint {
-///     fn meta_names() -> [&'static str; METADIM] {
-///         ["intensity"]
-///     }
-/// }
-/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
-///
-/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
-/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
-/// //   ^^^^^^^^ your custom Writer
-/// ```
-pub struct Writer<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
-where
-    T: FromBytes,
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
-{
-    coordinates: Box<dyn Iterator<Item = C>>,
-    phantom_t: std::marker::PhantomData<T>,
-}
-
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryInto<PointCloud2Msg>
-    for Writer<T, SIZE, DIM, METADIM, C>
-where
-    T: FromBytes,
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
-{
-    type Error = ConversionError;
-
-    /// Writes the points to a PointCloud2Msg.
-    ///
-    /// First use the `from` method for initializing the Writer.
-    /// Then use the `try_into` method to do the actual conversion.
-    ///
-    /// The operation is O(n) in time complexity where n is the number of points in the point cloud.
-    ///
-    /// # Example
-    /// ```
-    /// use ros_pointcloud2::{
-    ///     pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
-    /// };
-    ///
-    /// let cloud_points: Vec<PointXYZ> = vec![
-    ///     PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
-    ///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
-    /// ];
-    /// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
-    /// //                                                          ^^^^^^^^ ROS message conversion
-    /// ```
-    fn try_into(mut self) -> Result<PointCloud2Msg, Self::Error> {
-        if DIM > 3 {
-            return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time?
-        }
-
-        let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack
-
-        if DIM > 1 {
-            fields.push(PointFieldMsg {
-                name: "x".into(),
-                offset: 0,
-                datatype: T::field_datatype().into(),
-                count: 1,
-            });
-
-            fields.push(PointFieldMsg {
-                name: "y".into(),
-                offset: SIZE as u32,
-                datatype: T::field_datatype().into(),
-                count: 1,
-            });
-        }
-
-        if DIM == 3 {
-            fields.push(PointFieldMsg {
-                name: "z".into(),
-                offset: 2 * SIZE as u32,
-                datatype: T::field_datatype().into(),
-                count: 1,
-            });
-        }
-
-        let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
-        let point: Point<T, DIM, METADIM> = first_point.clone().into();
-        let meta_names = C::meta_names();
-
-        let mut meta_offsets_acc = DIM as u32 * SIZE as u32;
-        for (meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
-            let datatype_code = meta_value.datatype.into();
-            if FieldDatatype::try_from(datatype_code).is_err() {
-                return Err(ConversionError::UnsupportedFieldType);
-            }
-
-            fields.push(PointFieldMsg {
-                name: meta_name.into(),
-                offset: meta_offsets_acc,
-                datatype: datatype_code,
-                count: 1,
-            });
-            meta_offsets_acc += meta_value.datatype.size() as u32
-        }
-
-        let mut cloud = PointCloud2Msg {
-            point_step: fields.iter().fold(Default::default(), |acc, field| {
-                let field_type: FieldDatatype = field
-                    .datatype
-                    .try_into()
-                    .expect("Unsupported field but checked before.");
-                let field_size = field_type.size();
-                acc + field.count * field_size as u32
-            }),
-            ..Default::default()
-        };
-
-        // actual point -> byte conversion -- O(n)
-        add_point_to_byte_buffer(first_point, &mut cloud)?;
-        for coords in self.coordinates {
-            add_point_to_byte_buffer(coords, &mut cloud)?;
-        }
-
-        cloud.fields = fields;
-        cloud.height = 1; // everything is in one row (unstructured)
-        cloud.is_bigendian = false; // ROS default
-        cloud.is_dense = true; // ROS default
-        cloud.row_step = cloud.width * cloud.point_step; // Note: redundant but defined in PointCloud2 message
-
-        Ok(cloud)
-    }
-}
-
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
-    Writer<T, SIZE, DIM, METADIM, C>
-where
-    T: FromBytes,
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
-{
-    /// Create a Writer from any iterator that iterates over a template-defined point to a ROS message type.
-    /// First use the `from` method for initializing the Writer.
-    /// Then use the `try_into` method to do the actual conversion.
-    ///
-    /// The operation is O(n) in time complexity where n is the number of points in the point cloud.
-    ///
-    /// # Example
-    /// ```
-    /// use ros_pointcloud2::{
-    ///     pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
-    /// };
-    ///
-    /// let cloud_points: Vec<PointXYZ> = vec![
-    ///     PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
-    ///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
-    /// ];
-    // let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
-    /// //                                       ^^^^ Writer creation
-    /// ```
-    pub fn from(coordinates: impl IntoIterator<Item = C> + 'static) -> Self {
-        Self {
-            coordinates: Box::new(coordinates.into_iter()),
-            phantom_t: std::marker::PhantomData,
-        }
-    }
-}
-
-#[inline(always)]
-fn add_point_to_byte_buffer<
-    T: FromBytes,
-    const SIZE: usize,
-    const DIM: usize,
-    const METADIM: usize,
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
->(
-    coords: C,
-    cloud: &mut PointCloud2Msg,
-) -> Result<bool, ConversionError> {
-    let point: Point<T, DIM, METADIM> = coords.into();
-
-    // (x, y, z...)
-    point
-        .coords
-        .iter()
-        .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
-
-    // meta data description
-    point.meta.iter().for_each(|meta| {
-        let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
-        cloud.data.extend_from_slice(truncated_bytes);
-    });
-
-    cloud.width += 1;
-
-    Ok(true)
-}
\ No newline at end of file
diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs
index 9d77b0a..8871a25 100644
--- a/tests/e2e_test.rs
+++ b/tests/e2e_test.rs
@@ -1,33 +1,25 @@
 use ros_pointcloud2::pcl_utils::*;
-use ros_pointcloud2::reader::*;
-use ros_pointcloud2::ros_types::PointCloud2Msg;
-use ros_pointcloud2::writer::*;
+use ros_pointcloud2::PointCloud2Msg;
 use ros_pointcloud2::*;
 use std::fmt::Debug;
 
 macro_rules! convert_from_into {
-    ($reader:ty, $writer:ty, $point:ty, $cloud:expr) => {
-        convert_from_into_in_out_cloud!(
-            $reader,
-            $writer,
-            $point,
-            $cloud.clone(),
-            $point,
-            $cloud,
-            $point
-        );
+    ($point:ty, $cloud:expr) => {
+        convert_from_into_in_out_cloud!($cloud, $point, $cloud, $point);
     };
 }
 
 macro_rules! convert_from_into_in_out_cloud {
-    ($reader:ty, $writer:ty, $point:ty, $in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
-        let msg: Result<PointCloud2Msg, _> = <$writer>::from($in_cloud).try_into();
+    ($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
+        let msg = PointCloud2Msg::try_from_iterable($in_cloud.clone());
         assert!(msg.is_ok());
-        let to_p_type = <$reader>::try_from(msg.unwrap());
+        let msg = msg.unwrap();
+        let to_p_type = msg.try_into_iter();
         assert!(to_p_type.is_ok());
         let to_p_type = to_p_type.unwrap();
         let back_to_type = to_p_type.collect::<Vec<$out_point>>();
-        assert_eq!($out_cloud, back_to_type);
+        let orig_cloud: Vec<$out_point> = $out_cloud.iter().cloned().collect();
+        assert_eq!(orig_cloud, back_to_type);
     };
 }
 
@@ -43,10 +35,16 @@ fn custom_xyz_f32() {
         z: f32,
     }
 
-    type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
-    type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
+    impl From<CustomPoint> for Point<f32, DIM, METADIM> {
+        fn from(point: CustomPoint) -> Self {
+            Point {
+                coords: [point.x, point.y, point.z],
+                meta: [],
+            }
+        }
+    }
 
-    impl From<Point<f32, 3, 0>> for CustomPoint {
+    impl From<Point<f32, DIM, METADIM>> for CustomPoint {
         fn from(point: Point<f32, 3, 0>) -> Self {
             Self {
                 x: point.coords[0],
@@ -56,25 +54,14 @@ fn custom_xyz_f32() {
         }
     }
 
-    impl From<CustomPoint> for Point<f32, 3, 0> {
-        fn from(point: CustomPoint) -> Self {
-            Point {
-                coords: [point.x, point.y, point.z],
-                meta: [],
-            }
-        }
-    }
-
     impl MetaNames<METADIM> for CustomPoint {
         fn meta_names() -> [&'static str; METADIM] {
             []
         }
     }
-    impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
+    impl PointConvertible<f32, { std::mem::size_of::<f32>() }, DIM, METADIM> for CustomPoint {}
 
     convert_from_into!(
-        MyReader,
-        MyWriter,
         CustomPoint,
         vec![
             CustomPoint {
@@ -98,19 +85,51 @@ fn custom_xyz_f32() {
 
 #[test]
 fn custom_xyzi_f32() {
-    type Xyz = f32;
-    const XYZ_S: usize = std::mem::size_of::<Xyz>();
-    const DIM: usize = 3;
-    const METADIM: usize = 1;
+    let cloud: Vec<CustomPointXYZI> = vec![
+        CustomPointXYZI {
+            x: 0.0,
+            y: 1.0,
+            z: 5.0,
+            i: 0,
+        },
+        CustomPointXYZI {
+            x: 1.0,
+            y: 1.5,
+            z: 5.0,
+            i: 1,
+        },
+        CustomPointXYZI {
+            x: 1.3,
+            y: 1.6,
+            z: 5.7,
+            i: 2,
+        },
+        CustomPointXYZI {
+            x: f32::MAX,
+            y: f32::MIN,
+            z: f32::MAX,
+            i: u8::MAX,
+        },
+    ];
+
     #[derive(Debug, PartialEq, Clone)]
-    struct CustomPoint {
+    struct CustomPointXYZI {
         x: f32,
         y: f32,
         z: f32,
         i: u8,
     }
 
-    impl From<Point<f32, 3, 1>> for CustomPoint {
+    impl From<CustomPointXYZI> for Point<f32, 3, 1> {
+        fn from(point: CustomPointXYZI) -> Self {
+            Point {
+                coords: [point.x, point.y, point.z],
+                meta: [point.i.into()],
+            }
+        }
+    }
+
+    impl From<Point<f32, 3, 1>> for CustomPointXYZI {
         fn from(point: Point<f32, 3, 1>) -> Self {
             Self {
                 x: point.coords[0],
@@ -121,59 +140,19 @@ fn custom_xyzi_f32() {
         }
     }
 
-    impl From<CustomPoint> for Point<f32, 3, 1> {
-        fn from(point: CustomPoint) -> Self {
-            Point {
-                coords: [point.x, point.y, point.z],
-                meta: [point.i.into()],
-            }
-        }
-    }
-
-    impl MetaNames<METADIM> for CustomPoint {
-        fn meta_names() -> [&'static str; METADIM] {
+    impl MetaNames<1> for CustomPointXYZI {
+        fn meta_names() -> [&'static str; 1] {
             ["intensity"]
         }
     }
 
-    type MyReader = reader::Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
-    type MyWriter = writer::Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
-
-    impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
+    impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for CustomPointXYZI {}
 
-    let cloud = vec![
-        CustomPoint {
-            x: 0.0,
-            y: 1.0,
-            z: 5.0,
-            i: 0,
-        },
-        CustomPoint {
-            x: 1.0,
-            y: 1.5,
-            z: 5.0,
-            i: 1,
-        },
-        CustomPoint {
-            x: 1.3,
-            y: 1.6,
-            z: 5.7,
-            i: 2,
-        },
-        CustomPoint {
-            x: f32::MAX,
-            y: f32::MIN,
-            z: f32::MAX,
-            i: u8::MAX,
-        },
-    ];
-    convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
+    convert_from_into!(CustomPointXYZI, cloud);
 }
 
 #[test]
 fn custom_rgba_f32() {
-    const DIM: usize = 3;
-    const METADIM: usize = 4;
     #[derive(Debug, PartialEq, Clone)]
     struct CustomPoint {
         x: f32,
@@ -184,8 +163,6 @@ fn custom_rgba_f32() {
         b: u8,
         a: u8,
     }
-    type MyReader = reader::Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
-    type MyWriter = writer::Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
 
     impl From<Point<f32, 3, 4>> for CustomPoint {
         fn from(point: Point<f32, 3, 4>) -> Self {
@@ -215,12 +192,12 @@ fn custom_rgba_f32() {
         }
     }
 
-    impl MetaNames<METADIM> for CustomPoint {
-        fn meta_names() -> [&'static str; METADIM] {
+    impl MetaNames<4> for CustomPoint {
+        fn meta_names() -> [&'static str; 4] {
             ["r", "g", "b", "a"]
         }
     }
-    impl PointConvertible<f32, { std::mem::size_of::<f32>() }, DIM, METADIM> for CustomPoint {}
+    impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 4> for CustomPoint {}
     let cloud = vec![
         CustomPoint {
             x: 0.0,
@@ -259,7 +236,7 @@ fn custom_rgba_f32() {
             a: u8::MAX,
         },
     ];
-    convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
+    convert_from_into!(CustomPoint, cloud);
 }
 
 #[test]
@@ -287,14 +264,12 @@ fn converterxyz() {
         },
     ];
 
-    convert_from_into!(ReaderXYZ, WriterXYZ, PointXYZ, cloud);
+    convert_from_into!(PointXYZ, cloud);
 }
 
 #[test]
 fn converterxyzrgba() {
     convert_from_into!(
-        ReaderXYZRGBA,
-        WriterXYZRGBA,
         PointXYZRGBA,
         vec![
             PointXYZRGBA {
@@ -340,8 +315,6 @@ fn converterxyzrgba() {
 #[test]
 fn converterxyzinormal() {
     convert_from_into!(
-        ReaderXYZINormal,
-        WriterXYZINormal,
         PointXYZINormal,
         vec![
             PointXYZINormal {
@@ -387,8 +360,6 @@ fn converterxyzinormal() {
 #[test]
 fn converterxyzrgbnormal() {
     convert_from_into!(
-        ReaderXYZRGBNormal,
-        WriterXYZRGBNormal,
         PointXYZRGBNormal,
         vec![
             PointXYZRGBNormal {
@@ -442,8 +413,6 @@ fn converterxyzrgbnormal() {
 #[test]
 fn converterxyznormal() {
     convert_from_into!(
-        ReaderXYZNormal,
-        WriterXYZNormal,
         PointXYZNormal,
         vec![
             PointXYZNormal {
@@ -485,8 +454,6 @@ fn converterxyznormal() {
 #[test]
 fn converterxyzrgbl() {
     convert_from_into!(
-        ReaderXYZRGBL,
-        WriterXYZRGBL,
         PointXYZRGBL,
         vec![
             PointXYZRGBL {
@@ -532,8 +499,6 @@ fn converterxyzrgbl() {
 #[test]
 fn converterxyzrgb() {
     convert_from_into!(
-        ReaderXYZRGB,
-        WriterXYZRGB,
         PointXYZRGB,
         vec![
             PointXYZRGB {
@@ -575,8 +540,6 @@ fn converterxyzrgb() {
 #[test]
 fn converterxyzl() {
     convert_from_into!(
-        ReaderXYZL,
-        WriterXYZL,
         PointXYZL,
         vec![
             PointXYZL {
@@ -610,8 +573,6 @@ fn converterxyzl() {
 #[test]
 fn converterxyzi() {
     convert_from_into!(
-        ReaderXYZI,
-        WriterXYZI,
         PointXYZI,
         vec![
             PointXYZI {
@@ -671,7 +632,7 @@ fn write_xyzi_read_xyz() {
         },
     ];
 
-    let read_cloud = vec![
+    let read_cloud = [
         PointXYZ {
             x: 0.0,
             y: 1.0,
@@ -694,22 +655,11 @@ fn write_xyzi_read_xyz() {
         },
     ];
 
-    convert_from_into_in_out_cloud!(
-        ReaderXYZ,
-        WriterXYZI,
-        PointXYZI,
-        write_cloud,
-        PointXYZI,
-        read_cloud,
-        PointXYZ
-    );
+    convert_from_into_in_out_cloud!(write_cloud, PointXYZI, read_cloud, PointXYZ);
 }
 
 #[test]
 fn write_less_than_available() {
-    const DIM: usize = 3;
-    const METADIM: usize = 0;
-
     #[derive(Debug, PartialEq, Clone)]
     struct CustomPoint {
         x: f32,
@@ -718,9 +668,6 @@ fn write_less_than_available() {
         dummy: f32,
     }
 
-    type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
-    type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
-
     impl From<Point<f32, 3, 0>> for CustomPoint {
         fn from(point: Point<f32, 3, 0>) -> Self {
             Self {
@@ -741,8 +688,8 @@ fn write_less_than_available() {
         }
     }
 
-    impl MetaNames<METADIM> for CustomPoint {
-        fn meta_names() -> [&'static str; METADIM] {
+    impl MetaNames<0> for CustomPoint {
+        fn meta_names() -> [&'static str; 0] {
             []
         }
     }
@@ -769,7 +716,7 @@ fn write_less_than_available() {
         },
     ];
 
-    let read_cloud = vec![
+    let read_cloud = [
         CustomPoint {
             x: 1.0,
             y: 2.0,
@@ -790,62 +737,5 @@ fn write_less_than_available() {
         },
     ];
 
-    convert_from_into_in_out_cloud!(
-        MyReader,
-        MyWriter,
-        CustomPoint,
-        write_cloud,
-        CustomPoint,
-        read_cloud,
-        CustomPoint
-    );
-}
-
-#[test]
-fn readme() {
-    use ros_pointcloud2::{
-        pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-    };
-
-    // Your points (here using the predefined type PointXYZ).
-    let cloud_points = vec![
-        PointXYZ {
-            x: 1337.0,
-            y: 42.0,
-            z: 69.0,
-        },
-        PointXYZ {
-            x: f32::MAX,
-            y: f32::MIN,
-            z: f32::MAX,
-        },
-    ];
-
-    // For equality test later
-    let cloud_copy = cloud_points.clone();
-
-    // Vector -> Writer -> Message
-    let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points)
-        .try_into() // iterating points here O(n)
-        .unwrap();
-
-    // Convert to your ROS crate message type, we will use r2r here.
-    // let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
-
-    // Publish ...
-
-    // ... now incoming from a topic.
-    // let internal_msg: PointCloud2Msg = msg.into();
-
-    // Message -> Reader. The Reader implements the Iterator trait.
-    let reader = ReaderXYZ::try_from(internal_msg).unwrap();
-    let new_cloud_points = reader
-        .map(|point: PointXYZ| {
-            // Some logic here
-
-            point
-        })
-        .collect::<Vec<PointXYZ>>();
-
-    assert_eq!(new_cloud_points, cloud_copy);
+    convert_from_into_in_out_cloud!(write_cloud, CustomPoint, read_cloud, CustomPoint);
 }
diff --git a/tests/r2r_msg_test.rs b/tests/r2r_msg_test.rs
index 1c4540b..912a4ba 100644
--- a/tests/r2r_msg_test.rs
+++ b/tests/r2r_msg_test.rs
@@ -1,9 +1,7 @@
 #[cfg(feature = "r2r_msg")]
 #[test]
 fn convertxyz_r2r_msg() {
-    use ros_pointcloud2::{
-        pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-    };
+    use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
 
     use r2r::sensor_msgs::msg::PointCloud2;
 
@@ -25,10 +23,10 @@ fn convertxyz_r2r_msg() {
         },
     ];
     let copy = cloud.clone();
-    let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
+    let internal_cloud = PointCloud2Msg::try_from_iterable(cloud).unwrap();
     let r2r_msg_cloud: PointCloud2 = internal_cloud.into();
     let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into();
-    let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap();
+    let to_convert = convert_back_internal.try_into_iter().unwrap();
     let back_to_type = to_convert.collect::<Vec<PointXYZ>>();
     assert_eq!(copy, back_to_type);
 }
diff --git a/tests/rosrust_msg_test.rs b/tests/rosrust_msg_test.rs
index 03d7fef..30e0bae 100644
--- a/tests/rosrust_msg_test.rs
+++ b/tests/rosrust_msg_test.rs
@@ -1,9 +1,7 @@
 #[cfg(feature = "rosrust_msg")]
 #[test]
 fn convertxyz_rosrust_msg() {
-    use ros_pointcloud2::{
-        pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-    };
+    use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
 
     let cloud = vec![
         PointXYZ {
@@ -23,10 +21,10 @@ fn convertxyz_rosrust_msg() {
         },
     ];
     let copy = cloud.clone();
-    let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
+    let internal_cloud = PointCloud2Msg::try_from_iterable(cloud).unwrap();
     let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into();
     let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into();
-    let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap();
+    let to_convert = convert_back_internal.try_into_iter().unwrap();
     let back_to_type = to_convert.collect::<Vec<PointXYZ>>();
     assert_eq!(copy, back_to_type);
 }

From 593688f381b3dde2a43aa90d76466b2f8f9206de Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 30 Apr 2024 18:12:16 +0200
Subject: [PATCH 02/10] fix msg not found

---
 src/ros_types.rs | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/ros_types.rs b/src/ros_types.rs
index c124fb0..d2466bd 100644
--- a/src/ros_types.rs
+++ b/src/ros_types.rs
@@ -24,7 +24,7 @@ pub struct PointFieldMsg {
 }
 
 #[cfg(feature = "r2r_msg")]
-impl From<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
+impl From<r2r::sensor_msgs::msg::PointCloud2> for crate::PointCloud2Msg {
     fn from(msg: r2r::sensor_msgs::msg::PointCloud2) -> Self {
         Self {
             header: HeaderMsg {
@@ -57,8 +57,8 @@ impl From<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
 }
 
 #[cfg(feature = "r2r_msg")]
-impl From<PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
-    fn from(msg: PointCloud2Msg) -> Self {
+impl From<crate::PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
+    fn from(msg: crate::PointCloud2Msg) -> Self {
         r2r::sensor_msgs::msg::PointCloud2 {
             header: r2r::std_msgs::msg::Header {
                 stamp: r2r::builtin_interfaces::msg::Time {
@@ -89,7 +89,7 @@ impl From<PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
 }
 
 #[cfg(feature = "rosrust_msg")]
-impl From<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
+impl From<rosrust_msg::sensor_msgs::PointCloud2> for crate::PointCloud2Msg {
     fn from(msg: rosrust_msg::sensor_msgs::PointCloud2) -> Self {
         Self {
             header: HeaderMsg {
@@ -122,8 +122,8 @@ impl From<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
 }
 
 #[cfg(feature = "rosrust_msg")]
-impl From<PointCloud2Msg> for rosrust_msg::sensor_msgs::PointCloud2 {
-    fn from(msg: PointCloud2Msg) -> Self {
+impl From<crate::PointCloud2Msg> for rosrust_msg::sensor_msgs::PointCloud2 {
+    fn from(msg: crate::PointCloud2Msg) -> Self {
         rosrust_msg::sensor_msgs::PointCloud2 {
             header: rosrust_msg::std_msgs::Header {
                 seq: msg.header.seq,

From afc44c41ed1fb5ef100ff2d436c974d03f910a99 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 30 Apr 2024 23:16:53 +0200
Subject: [PATCH 03/10] impl par iterator

---
 Cargo.toml           |   4 +-
 README.md            |   2 +-
 benches/roundtrip.rs |  35 ++++++
 src/convert.rs       |   9 +-
 src/iterator.rs      | 248 ++++++++++++++++++++++++++++++++++++++-----
 src/lib.rs           |  11 ++
 6 files changed, 278 insertions(+), 31 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index cb7734d..8cc0739 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -17,6 +17,7 @@ exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**
 rosrust_msg = { version = "0.1", optional = true }
 rosrust = { version = "0.9.11", optional = true }
 r2r = { version = "0.8.4", optional = true }
+rayon = { version = "1.10", optional = true }
 
 [dev-dependencies]
 rand = "0.8"
@@ -28,4 +29,5 @@ harness = false
 
 [features]
 rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
-r2r_msg = ["dep:r2r"]
\ No newline at end of file
+r2r_msg = ["dep:r2r"]
+rayon = ["dep:rayon"]
diff --git a/README.md b/README.md
index 96eb8cc..a945151 100644
--- a/README.md
+++ b/README.md
@@ -28,7 +28,7 @@ let in_msg = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
 // ... now incoming from a topic.
 // let in_msg: PointCloud2Msg = msg.into();
 
-let new_pcl = in_msg.try_into_iterable().unwrap()
+let new_pcl = in_msg.try_into_iter().unwrap()
   .map(|point: PointXYZ| { // Define the type of point here.
     // Some logic here ...
 
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
index 4ebc753..20caf23 100644
--- a/benches/roundtrip.rs
+++ b/benches/roundtrip.rs
@@ -3,6 +3,9 @@ use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
 
 use rand::Rng;
 
+#[cfg(feature = "rayon")]
+use rayon::prelude::*;
+
 pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
     let mut rng = rand::thread_rng();
     let mut pointcloud = Vec::with_capacity(num_points);
@@ -27,6 +30,17 @@ fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
     orig_len == total.len()
 }
 
+#[cfg(feature = "rayon")]
+fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let total = internal_msg
+        .try_into_par_iter()
+        .unwrap()
+        .collect::<Vec<PointXYZ>>();
+    orig_len == total.len()
+}
+
 fn roundtrip_benchmark(c: &mut Criterion) {
     let cloud_points_10k = generate_random_pointcloud(10_000, f32::MIN / 2.0, f32::MAX / 2.0);
     let cloud_points_100k = generate_random_pointcloud(100_000, f32::MIN / 2.0, f32::MAX / 2.0);
@@ -38,17 +52,38 @@ fn roundtrip_benchmark(c: &mut Criterion) {
         })
     });
 
+    #[cfg(feature = "rayon")]
+    c.bench_function("roundtrip_par 10k", |b| {
+        b.iter(|| {
+            black_box(roundtrip_par(cloud_points_10k.clone()));
+        })
+    });
+
     c.bench_function("roundtrip 100k", |b| {
         b.iter(|| {
             black_box(roundtrip(cloud_points_100k.clone()));
         })
     });
 
+    #[cfg(feature = "rayon")]
+    c.bench_function("roundtrip_par 100k", |b| {
+        b.iter(|| {
+            black_box(roundtrip_par(cloud_points_100k.clone()));
+        })
+    });
+
     c.bench_function("roundtrip 1m", |b| {
         b.iter(|| {
             black_box(roundtrip(cloud_points_1m.clone()));
         })
     });
+
+    #[cfg(feature = "rayon")]
+    c.bench_function("roundtrip_par 1m", |b| {
+        b.iter(|| {
+            black_box(roundtrip_par(cloud_points_1m.clone()));
+        })
+    });
 }
 
 criterion_group!(benches, roundtrip_benchmark);
diff --git a/src/convert.rs b/src/convert.rs
index c8c7e3a..ca0061e 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -306,7 +306,7 @@ impl FromBytes for u8 {
 }
 
 #[derive(Default, Clone, Debug, PartialEq)]
-pub(crate) enum Endianness {
+pub enum Endianness {
     Big,
 
     #[default]
@@ -331,7 +331,12 @@ fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> [u8; S] {
     let mut target = [u8::default(); S];
 
     debug_assert!(target.len() == S);
-    debug_assert!(data.len() >= start_idx + S);
+    debug_assert!(
+        data.len() >= start_idx + S,
+        "[ERR] byte read out of bounds: Data len: {}, read_idx: {}",
+        data.len(),
+        start_idx + S
+    );
 
     let source = unsafe { data.get_unchecked(start_idx..start_idx + S) };
     target
diff --git a/src/iterator.rs b/src/iterator.rs
index 4fb07ad..8b783b8 100644
--- a/src/iterator.rs
+++ b/src/iterator.rs
@@ -31,6 +31,7 @@ pub struct PointCloudIterator<
     C: MetaNames<METADIM>,
 {
     iteration: usize,
+    iteration_back: usize,
     data: Vec<u8>,
     point_step_size: usize,
     cloud_length: usize,
@@ -41,6 +42,125 @@ pub struct PointCloudIterator<
     phantom_c: std::marker::PhantomData<C>, // internally used for meta names array
 }
 
+#[cfg(feature = "rayon")]
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> ExactSizeIterator
+    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes + Send + Sync,
+    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+{
+    fn len(&self) -> usize {
+        self.cloud_length
+    }
+}
+
+#[cfg(feature = "rayon")]
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> DoubleEndedIterator
+    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes + Send + Sync,
+    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+{
+    fn next_back(&mut self) -> Option<Self::Item> {
+        if self.iteration_back < self.iteration {
+            return None; // iteration finished
+        }
+
+        let p = self.point_at(self.iteration_back);
+        Some(C::from(p))
+    }
+}
+
+#[cfg(feature = "rayon")]
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> rayon::iter::ParallelIterator
+    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes + Send + Sync,
+    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+{
+    type Item = C;
+
+    fn drive_unindexed<Co>(self, consumer: Co) -> Co::Result
+    where
+        Co: rayon::iter::plumbing::UnindexedConsumer<Self::Item>,
+    {
+        rayon::iter::plumbing::bridge(self, consumer)
+    }
+
+    fn opt_len(&self) -> Option<usize> {
+        Some(self.cloud_length)
+    }
+}
+
+#[cfg(feature = "rayon")]
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
+    rayon::iter::IndexedParallelIterator for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes + Send + Sync,
+    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+{
+    fn len(&self) -> usize {
+        self.cloud_length
+    }
+
+    fn drive<Co>(self, consumer: Co) -> Co::Result
+    where
+        Co: rayon::iter::plumbing::Consumer<Self::Item>,
+    {
+        rayon::iter::plumbing::bridge(self, consumer)
+    }
+
+    fn with_producer<CB: rayon::iter::plumbing::ProducerCallback<Self::Item>>(
+        self,
+        callback: CB,
+    ) -> CB::Output {
+        callback.callback(RayonProducer::from(self))
+    }
+}
+
+#[cfg(feature = "rayon")]
+struct RayonProducer<
+    T: FromBytes,
+    const SIZE: usize,
+    const DIM: usize,
+    const METADIM: usize,
+    C: PointConvertible<T, SIZE, DIM, METADIM>,
+> {
+    iter: PointCloudIterator<T, SIZE, DIM, METADIM, C>,
+}
+
+#[cfg(feature = "rayon")]
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
+    rayon::iter::plumbing::Producer for RayonProducer<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes + Send + Sync,
+    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+{
+    type Item = C;
+    type IntoIter = PointCloudIterator<T, SIZE, DIM, METADIM, C>;
+
+    fn into_iter(self) -> Self::IntoIter {
+        self.iter
+    }
+
+    fn split_at(self, index: usize) -> (Self, Self) {
+        let (left, right) = self.iter.split_at(index);
+        (RayonProducer { iter: left }, RayonProducer { iter: right })
+    }
+}
+
+#[cfg(feature = "rayon")]
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
+    From<PointCloudIterator<T, SIZE, DIM, METADIM, C>> for RayonProducer<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes + Send + Sync,
+    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+{
+    fn from(iterator: PointCloudIterator<T, SIZE, DIM, METADIM, C>) -> Self {
+        Self { iter: iterator }
+    }
+}
+
 /// Implementation of the iterator trait.
 impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> Iterator
     for PointCloudIterator<T, SIZE, DIM, METADIM, C>
@@ -67,35 +187,11 @@ where
             return None; // iteration finished
         }
 
-        let mut xyz = [T::default(); DIM];
-        xyz.iter_mut()
-            .zip(self.offsets.iter())
-            .for_each(|(p_xyz, in_point_offset)| {
-                *p_xyz = load_loadable::<T, SIZE>(
-                    (self.iteration * self.point_step_size) + in_point_offset,
-                    &self.data,
-                    &self.endianness,
-                );
-            });
-
-        debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
-        debug_assert!(
-            self.offsets.len() == DIM + METADIM,
-            "Offset length mismatch"
-        );
-
-        let mut meta = [PointMeta::default(); METADIM];
-        meta.iter_mut()
-            .zip(self.offsets.iter().skip(DIM))
-            .zip(self.meta.iter())
-            .for_each(|((p_meta, in_point_offset), (_, meta_type))| {
-                let start = (self.iteration * self.point_step_size) + in_point_offset;
-                *p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
-            });
+        let p = self.point_at(self.iteration);
 
         self.iteration += 1;
 
-        Some(C::from(Point { coords: xyz, meta }))
+        Some(C::from(p))
     }
 }
 
@@ -223,11 +319,14 @@ where
             return Err(ConversionError::DataLengthMismatch);
         }
 
+        let cloud_length = cloud.width as usize * cloud.height as usize;
+
         Ok(Self {
             iteration: 0,
+            iteration_back: cloud_length - 1,
             data: cloud.data,
             point_step_size,
-            cloud_length: cloud.width as usize * cloud.height as usize,
+            cloud_length,
             offsets,
             meta,
             endianness: endian,
@@ -236,3 +335,98 @@ where
         })
     }
 }
+
+impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
+    PointCloudIterator<T, SIZE, DIM, METADIM, C>
+where
+    T: FromBytes,
+    C: MetaNames<METADIM>,
+{
+    #[inline(always)]
+    fn point_at(&self, offset: usize) -> Point<T, DIM, METADIM> {
+        let mut xyz = [T::default(); DIM];
+        xyz.iter_mut()
+            .zip(self.offsets.iter())
+            .for_each(|(p_xyz, in_point_offset)| {
+                *p_xyz = load_loadable::<T, SIZE>(
+                    (offset * self.point_step_size) + in_point_offset,
+                    &self.data,
+                    &self.endianness,
+                );
+            });
+
+        debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
+        debug_assert!(
+            self.offsets.len() == DIM + METADIM,
+            "Offset length mismatch"
+        );
+
+        let mut meta = [PointMeta::default(); METADIM];
+        meta.iter_mut()
+            .zip(self.offsets.iter().skip(DIM))
+            .zip(self.meta.iter())
+            .for_each(|((p_meta, in_point_offset), (_, meta_type))| {
+                let start = (offset * self.point_step_size) + in_point_offset;
+                *p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
+            });
+
+        Point { coords: xyz, meta }
+    }
+
+    pub fn split_at(self, index: usize) -> (Self, Self) {
+        let data_idx = index * self.point_step_size;
+        let (left, right) = self.data.split_at(data_idx);
+        let left_iteration = self.iteration;
+        let right_iteration = self.iteration + index;
+        let right_iteration_back = self.iteration_back;
+        (
+            Self {
+                iteration: left_iteration,
+                iteration_back: right_iteration,
+                data: left.into(), // TODO richtig mist hier
+                point_step_size: self.point_step_size.clone(),
+                cloud_length: right_iteration - left_iteration,
+                offsets: self.offsets.clone(),
+                meta: self.meta.clone(),
+                endianness: self.endianness.clone(),
+                phantom_t: std::marker::PhantomData,
+                phantom_c: std::marker::PhantomData,
+            },
+            Self {
+                iteration: right_iteration,
+                iteration_back: right_iteration_back,
+                data: right.into(), // TODO richtig mist hier
+                point_step_size: self.point_step_size,
+                cloud_length: right_iteration_back - right_iteration,
+                offsets: self.offsets.clone(),
+                meta: self.meta.clone(),
+                endianness: self.endianness.clone(),
+                phantom_t: std::marker::PhantomData,
+                phantom_c: std::marker::PhantomData,
+            },
+        )
+    }
+
+    /// Split the iterator at the given index.#
+    pub fn from_subslice(
+        data: Vec<u8>,
+        start: usize,
+        end: usize,
+        offsets: Vec<usize>,
+        meta: Vec<(String, FieldDatatype)>,
+        endianness: Endianness,
+    ) -> Self {
+        Self {
+            iteration: start,
+            iteration_back: end,
+            data: data,
+            point_step_size: 0,
+            cloud_length: end - start,
+            offsets: offsets,
+            meta: meta,
+            endianness: endianness,
+            phantom_t: std::marker::PhantomData,
+            phantom_c: std::marker::PhantomData,
+        }
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index c4d5d93..95b1e4a 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -246,6 +246,17 @@ impl PointCloud2Msg {
     {
         iterator::PointCloudIterator::try_from(self)
     }
+
+    #[cfg(feature = "rayon")]
+    pub fn try_into_par_iter<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
+        self,
+    ) -> Result<impl rayon::iter::ParallelIterator<Item = C>, ConversionError>
+    where
+        C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+        T: FromBytes + Send + Sync,
+    {
+        iterator::PointCloudIterator::try_from(self)
+    }
 }
 
 /// Internal point representation. It is used to store the coordinates and meta data of a point.

From bad69d2fa7baacacb2bba29bd306b1a86f33a534 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 30 Apr 2024 23:18:50 +0200
Subject: [PATCH 04/10] clippy

---
 src/iterator.rs | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/iterator.rs b/src/iterator.rs
index 8b783b8..888bc5a 100644
--- a/src/iterator.rs
+++ b/src/iterator.rs
@@ -384,7 +384,7 @@ where
                 iteration: left_iteration,
                 iteration_back: right_iteration,
                 data: left.into(), // TODO richtig mist hier
-                point_step_size: self.point_step_size.clone(),
+                point_step_size: self.point_step_size,
                 cloud_length: right_iteration - left_iteration,
                 offsets: self.offsets.clone(),
                 meta: self.meta.clone(),
@@ -419,12 +419,12 @@ where
         Self {
             iteration: start,
             iteration_back: end,
-            data: data,
+            data,
             point_step_size: 0,
             cloud_length: end - start,
-            offsets: offsets,
-            meta: meta,
-            endianness: endianness,
+            offsets,
+            meta,
+            endianness,
             phantom_t: std::marker::PhantomData,
             phantom_c: std::marker::PhantomData,
         }

From 0e705ac7e1bd1d5c57c0d7f5f13a5aa9727e0a44 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Fri, 3 May 2024 21:35:42 +0200
Subject: [PATCH 05/10] pre endian

---
 Cargo.toml                      |   7 +
 README.md                       |  12 +-
 benches/roundtrip.rs            |  71 +++--
 examples/custom_label_filter.rs |  41 +--
 examples/distance_filter.rs     |   2 +-
 examples/rayon.rs               | 144 ++++++++++
 rpcl2_derive/Cargo.toml         |  12 +
 rpcl2_derive/src/lib.rs         | 158 ++++++++++
 src/convert.rs                  |  76 ++---
 src/iterator.rs                 | 494 ++++++++++++++++++--------------
 src/lib.rs                      | 348 ++++++++++++++--------
 src/pcl_utils.rs                | 400 ++++++++++++++------------
 src/ros_types.rs                |  13 +-
 tests/e2e_test.rs               | 247 +++++++++-------
 14 files changed, 1303 insertions(+), 722 deletions(-)
 create mode 100644 examples/rayon.rs
 create mode 100644 rpcl2_derive/Cargo.toml
 create mode 100644 rpcl2_derive/src/lib.rs

diff --git a/Cargo.toml b/Cargo.toml
index 8cc0739..a897f32 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -18,6 +18,10 @@ rosrust_msg = { version = "0.1", optional = true }
 rosrust = { version = "0.9.11", optional = true }
 r2r = { version = "0.8.4", optional = true }
 rayon = { version = "1.10", optional = true }
+nalgebra = { version = "0.32.5", optional = true }
+rpcl2_derive = { path = "./rpcl2_derive", optional = true }
+bytemuck = "1.15"
+type-layout = "0.2.0"
 
 [dev-dependencies]
 rand = "0.8"
@@ -31,3 +35,6 @@ harness = false
 rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
 r2r_msg = ["dep:r2r"]
 rayon = ["dep:rayon"]
+derive = ["dep:rpcl2_derive"]
+
+default = ["derive"]
diff --git a/README.md b/README.md
index a945151..0166846 100644
--- a/README.md
+++ b/README.md
@@ -5,11 +5,21 @@
   </p>
 </p>
 
+A complete implementation of `sensor_msgs/PointCloud2` conversions with focus on ease of use and maximum throughput.
+
 Providing an easy to use, generics defined, point-wise iterator abstraction over the byte buffer in `PointCloud2` to minimize iterations in your processing pipeline.
 
 To keep the crate a general purpose library for the problem, it uses its own type for the message `PointCloud2Msg`. ROS1 and ROS2 support is added with feature flags.
 
-## Quickstart
+## Performance
+
+The library compares the similarity of the ROS message point type with your given Point, allowing it to select the fastest strategy.
+
+Since PCL is the most used library for this work in C++, the predefined types in ros_pointcloud2 use C padding to be exact copies for maximum performance between C++ and Rust nodes at the cost of a slightly larger message sizes.
+
+Since ros_pointcloud2 optimizes the performance dynamically, it typically outperforms PCL conversions in all tasks.
+
+## Quickstart (Iterator)
 ```rust
 use ros_pointcloud2::{PointCloud2Msg, pcl_utils::PointXYZ};
 
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
index 20caf23..7f3d888 100644
--- a/benches/roundtrip.rs
+++ b/benches/roundtrip.rs
@@ -22,7 +22,7 @@ pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<
 
 fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
-    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
     let total = internal_msg
         .try_into_iter()
         .unwrap()
@@ -30,6 +30,23 @@ fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
     orig_len == total.len()
 }
 
+fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
+    let total = internal_msg
+        .try_into_iter()
+        .unwrap()
+        .filter(|point: &PointXYZ| {
+            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+        })
+        .fold(PointXYZ::default(), |acc, point| PointXYZ {
+            x: acc.x + point.x,
+            y: acc.y + point.y,
+            z: acc.z + point.z,
+        });
+    orig_len == total.x as usize
+}
+
 #[cfg(feature = "rayon")]
 fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
@@ -38,50 +55,70 @@ fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
         .try_into_par_iter()
         .unwrap()
         .collect::<Vec<PointXYZ>>();
-    orig_len == total.len()
+    orig_len != total.len()
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len: usize = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let total = internal_msg
+        .try_into_par_iter()
+        .unwrap()
+        .filter(|point: &PointXYZ| {
+            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+        })
+        .reduce(
+            || PointXYZ::default(),
+            |acc, point| PointXYZ {
+                x: acc.x + point.x,
+                y: acc.y + point.y,
+                z: acc.z + point.z,
+            },
+        );
+    orig_len == total.x as usize
 }
 
 fn roundtrip_benchmark(c: &mut Criterion) {
-    let cloud_points_10k = generate_random_pointcloud(10_000, f32::MIN / 2.0, f32::MAX / 2.0);
-    let cloud_points_100k = generate_random_pointcloud(100_000, f32::MIN / 2.0, f32::MAX / 2.0);
-    let cloud_points_1m = generate_random_pointcloud(1_000_000, f32::MIN / 2.0, f32::MAX / 2.0);
+    let cloud_points_500k = generate_random_pointcloud(500_000, f32::MIN / 2.0, f32::MAX / 2.0);
+    let cloud_points_1_5m = generate_random_pointcloud(1_500_000, f32::MIN / 2.0, f32::MAX / 2.0);
 
-    c.bench_function("roundtrip 10k", |b| {
+    c.bench_function("roundtrip 500k", |b| {
         b.iter(|| {
-            black_box(roundtrip(cloud_points_10k.clone()));
+            black_box(roundtrip(cloud_points_500k.clone()));
         })
     });
 
     #[cfg(feature = "rayon")]
-    c.bench_function("roundtrip_par 10k", |b| {
+    c.bench_function("roundtrip_par 500k", |b| {
         b.iter(|| {
-            black_box(roundtrip_par(cloud_points_10k.clone()));
+            black_box(roundtrip_par(cloud_points_500k.clone()));
         })
     });
 
-    c.bench_function("roundtrip 100k", |b| {
+    c.bench_function("roundtrip_filter 500k", |b| {
         b.iter(|| {
-            black_box(roundtrip(cloud_points_100k.clone()));
+            roundtrip_filter(black_box(cloud_points_500k.clone()));
         })
     });
 
     #[cfg(feature = "rayon")]
-    c.bench_function("roundtrip_par 100k", |b| {
+    c.bench_function("roundtrip_filter_par 500k", |b| {
         b.iter(|| {
-            black_box(roundtrip_par(cloud_points_100k.clone()));
+            roundtrip_filter_par(black_box(cloud_points_500k.clone()));
         })
     });
 
-    c.bench_function("roundtrip 1m", |b| {
+    c.bench_function("roundtrip_filter 1.5m", |b| {
         b.iter(|| {
-            black_box(roundtrip(cloud_points_1m.clone()));
+            roundtrip_filter(black_box(cloud_points_1_5m.clone()));
         })
     });
 
     #[cfg(feature = "rayon")]
-    c.bench_function("roundtrip_par 1m", |b| {
+    c.bench_function("roundtrip_filter_par 1.5m", |b| {
         b.iter(|| {
-            black_box(roundtrip_par(cloud_points_1m.clone()));
+            black_box(roundtrip_filter_par(cloud_points_1_5m.clone()));
         })
     });
 }
diff --git a/examples/custom_label_filter.rs b/examples/custom_label_filter.rs
index 19b92ed..83d7e4b 100644
--- a/examples/custom_label_filter.rs
+++ b/examples/custom_label_filter.rs
@@ -2,10 +2,11 @@
 // 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::{size_of, MetaNames, Point, PointCloud2Msg, PointConvertible};
+use ros_pointcloud2::{Fields, Point, PointCloud2Msg, PointConvertible};
 
-#[derive(Debug, PartialEq, Clone)]
+#[derive(Debug, PartialEq, Clone, Default)]
 enum Label {
+    #[default]
     Human,
     Deer,
     Car,
@@ -33,7 +34,7 @@ impl From<u8> for Label {
 }
 
 // Custom point type with custom metadata
-#[derive(Debug, PartialEq, Clone)]
+#[derive(Debug, PartialEq, Clone, Default)]
 struct CustomPoint {
     x: f32,
     y: f32,
@@ -48,39 +49,41 @@ struct CustomPoint {
 // - coordinate type
 // - dimension (xyz = 3)
 // - metadata dimension (intensity + my_custom_label = 2)
-impl From<CustomPoint> for Point<f32, 3, 2> {
+impl From<CustomPoint> for Point<5> {
     fn from(point: CustomPoint) -> Self {
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
                 point.intensity.into(),
-                u8::from(point.my_custom_label).into(), // encoding the label as u8
+                u8::from(point.my_custom_label).into(),
             ],
         }
     }
 }
 
-impl From<Point<f32, 3, 2>> for CustomPoint {
-    fn from(point: Point<f32, 3, 2>) -> Self {
-        let label_raw: u8 = point.meta[1].get();
+impl From<Point<5>> for CustomPoint {
+    fn from(point: Point<5>) -> Self {
+        let label_raw: u8 = point.fields[1].get();
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
-            intensity: point.meta[0].get(),
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
+            intensity: point.fields[3].get(),
             my_custom_label: Label::from(label_raw), // decoding the label from u8
         }
     }
 }
 
-impl MetaNames<2> for CustomPoint {
-    fn meta_names() -> [&'static str; 2] {
-        ["intensity", "my_custom_label"] // how we want to name the metadata in the message
+impl Fields<5> for CustomPoint {
+    fn field_names_ordered() -> [&'static str; 5] {
+        ["x", "y", "z", "intensity", "my_custom_label"] // how we want to name the metadata in the message
     }
 }
 
 // We implemented everything that is needed so we declare it as a PointConvertible
-impl PointConvertible<f32, { size_of!(f32) }, 3, 2> for CustomPoint {}
+impl PointConvertible<5> for CustomPoint {}
 
 fn main() {
     let cloud = vec![
@@ -109,7 +112,7 @@ fn main() {
 
     println!("Original cloud: {:?}", cloud);
 
-    let msg = PointCloud2Msg::try_from_iterable(cloud.clone()).unwrap();
+    let msg = PointCloud2Msg::try_from_iter(cloud.clone().into_iter()).unwrap();
 
     println!("filtering by label == Deer");
     let out = msg
diff --git a/examples/distance_filter.rs b/examples/distance_filter.rs
index 23dc4fc..4debb98 100644
--- a/examples/distance_filter.rs
+++ b/examples/distance_filter.rs
@@ -25,7 +25,7 @@ fn main() {
 
     println!("Original cloud: {:?}", cloud);
 
-    let msg = PointCloud2Msg::try_from_iterable(cloud.clone()).unwrap();
+    let msg = PointCloud2Msg::try_from_iter(cloud.clone().into_iter()).unwrap();
 
     println!("filtering by distance < 1.9m");
     let out = msg
diff --git a/examples/rayon.rs b/examples/rayon.rs
new file mode 100644
index 0000000..1d522d7
--- /dev/null
+++ b/examples/rayon.rs
@@ -0,0 +1,144 @@
+// This example demonstrates a very simple distance filter with predefined point types.
+// Note that this example is a simplified version of the custom_filter.rs example.
+// Also, it effectively demonstrates a typesafe byte-to-byte buffer filter with a single iteration.
+
+use std::time::Duration;
+
+use criterion::black_box;
+use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
+
+use rand::Rng;
+
+#[cfg(feature = "rayon")]
+use rayon::prelude::*;
+
+pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
+    let mut rng = rand::thread_rng();
+    let mut pointcloud = Vec::with_capacity(num_points);
+    for _ in 0..num_points {
+        let point = PointXYZ {
+            x: rng.gen_range(min..max),
+            y: rng.gen_range(min..max),
+            z: rng.gen_range(min..max),
+        };
+        pointcloud.push(point);
+    }
+    pointcloud
+}
+
+fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud.into_iter()).unwrap();
+    let total = internal_msg
+        .try_into_iter()
+        .unwrap()
+        .collect::<Vec<PointXYZ>>();
+    orig_len == total.len()
+}
+
+fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud.into_iter()).unwrap();
+    let total = internal_msg
+        .try_into_iter()
+        .unwrap()
+        .filter(|point: &PointXYZ| {
+            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+        })
+        .fold(PointXYZ::default(), |acc, point| PointXYZ {
+            x: acc.x + point.x,
+            y: acc.y + point.y,
+            z: acc.z + point.z,
+        });
+    orig_len == total.x as usize
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let total = internal_msg
+        .try_into_par_iter()
+        .unwrap()
+        .collect::<Vec<PointXYZ>>();
+    orig_len != total.len()
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len: usize = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let total = internal_msg
+        .try_into_par_iter()
+        .unwrap()
+        .filter(|point: &PointXYZ| {
+            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+        })
+        .reduce(
+            || PointXYZ::default(),
+            |acc, point| PointXYZ {
+                x: acc.x + point.x,
+                y: acc.y + point.y,
+                z: acc.z + point.z,
+            },
+        );
+    orig_len == total.x as usize
+}
+
+// call measure_func X times and print the average time
+fn measure_func_avg(
+    num_iterations: u32,
+    pcl_size: usize,
+    func: fn(Vec<PointXYZ>) -> bool,
+) -> Duration {
+    let mut total_time = Duration::new(0, 0);
+    for _ in 0..num_iterations {
+        total_time += measure_func(pcl_size, func);
+    }
+    total_time / num_iterations
+}
+
+fn measure_func<F>(pcl_size: usize, func: F) -> Duration
+where
+    F: Fn(Vec<PointXYZ>) -> bool,
+{
+    let cloud_points = generate_random_pointcloud(pcl_size, f32::MIN / 2.0, f32::MAX / 2.0);
+    let start = std::time::Instant::now();
+    black_box(func(cloud_points));
+    start.elapsed()
+}
+
+fn main() {
+    println!("100k");
+    let how_many = 10_000;
+    let how_often = 1_000;
+
+    let dur = measure_func_avg(how_often, how_many, roundtrip);
+    println!("roundtrip: {:?}", dur);
+
+    #[cfg(feature = "rayon")]
+    let dur = measure_func_avg(how_often, how_many, roundtrip_par);
+    println!("roundtrip_par: {:?}", dur);
+
+    println!("200k");
+    let how_many = 200_000;
+    let how_often = 100;
+
+    let dur = measure_func_avg(how_often, how_many, roundtrip_filter);
+    println!("roundtrip_filter: {:?}", dur);
+
+    #[cfg(feature = "rayon")]
+    let dur = measure_func_avg(how_often, how_many, roundtrip_filter_par);
+    println!("roundtrip_filter_par: {:?}", dur);
+
+    println!("10m");
+    let how_many = 10_000_000;
+    let how_often = 10;
+
+    let dur = measure_func_avg(how_often, how_many, roundtrip_filter);
+    println!("roundtrip_filter: {:?}", dur);
+
+    #[cfg(feature = "rayon")]
+    let dur = measure_func_avg(how_often, how_many, roundtrip_filter_par);
+    println!("roundtrip_filter_par: {:?}", dur);
+}
diff --git a/rpcl2_derive/Cargo.toml b/rpcl2_derive/Cargo.toml
new file mode 100644
index 0000000..05371e7
--- /dev/null
+++ b/rpcl2_derive/Cargo.toml
@@ -0,0 +1,12 @@
+[package]
+name = "rpcl2_derive"
+version = "0.1.0"
+edition = "2021"
+
+[lib]
+proc-macro = true
+
+[dependencies]
+syn = "2"
+quote = "1"
+proc-macro2 = "1"
\ No newline at end of file
diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs
new file mode 100644
index 0000000..f90b64e
--- /dev/null
+++ b/rpcl2_derive/src/lib.rs
@@ -0,0 +1,158 @@
+extern crate proc_macro;
+
+use std::{any::Any, collections::HashMap, fmt::Debug};
+
+use proc_macro::TokenStream;
+use quote::{quote, ToTokens};
+use syn::{parse_macro_input, DeriveInput};
+
+/// 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.
+#[proc_macro_derive(RosFields)]
+pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
+    let input = parse_macro_input!(input as DeriveInput);
+    let name = input.clone().ident;
+
+    let fields = match input.data {
+        syn::Data::Struct(ref data) => data.fields.clone(),
+        _ => 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);
+
+    if fields.is_empty() {
+        return syn::Error::new_spanned(input, "No fields found").to_compile_error().into();
+    }
+
+    for field in fields.iter() {
+        let ty = field.ty.to_token_stream().to_string();
+        if !allowed_datatypes.contains_key(&ty.as_str()) {
+            return syn::Error::new_spanned(field, "Field type not allowed").to_compile_error().into();
+        }
+    }
+
+    let field_len_token: usize = fields.len();
+    
+    let field_names = fields.iter().map(|field| {
+        let field_name = field.ident.as_ref().unwrap();
+        quote! { stringify!(#field_name) }
+    }).collect::<Vec<_>>();
+
+    let field_impl = quote! {
+        impl Fields<#field_len_token> for #name {
+            fn field_names_ordered() -> [&'static str; #field_len_token] {
+                [
+                    #(#field_names,)*
+                ]
+            }
+        }
+    };
+
+    TokenStream::from(field_impl)
+}
+
+/// This macro will fully implement the `PointConvertible` trait for your struct so you can use your point for the PointCloud2 conversion.
+///
+/// Note that the repr(C) attribute is required for the struct to work efficiently with C++ PCL.
+/// With Rust layout optimizations, the struct might not work with the PCL library but the message still conforms to the specification of PointCloud2.
+/// Furthermore, Rust layout can lead to smaller messages to be send over the network.
+#[proc_macro_derive(RosFull)]
+pub fn ros_point_derive(input: TokenStream) -> TokenStream {
+    let input = parse_macro_input!(input as DeriveInput);
+    let name = input.clone().ident;
+
+    let fields = match input.data {
+        syn::Data::Struct(ref data) => data.fields.clone(),
+        _ => 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);
+
+    if fields.is_empty() {
+        return syn::Error::new_spanned(input, "No fields found").to_compile_error().into();
+    }
+
+    for field in fields.iter() {
+        let ty = field.ty.to_token_stream().to_string();
+        if !allowed_datatypes.contains_key(&ty.as_str()) {
+            return syn::Error::new_spanned(field, "Field type not allowed").to_compile_error().into();
+        }
+    }
+
+    let field_len_token: usize = fields.len();
+    
+    let field_names = fields.iter().map(|field| {
+        let field_name = field.ident.as_ref().unwrap();
+        quote! { stringify!(#field_name) }
+    }).collect::<Vec<_>>();
+
+    let field_impl = quote! {
+        impl ros_pointcloud2::Fields<#field_len_token> for #name {
+            fn field_names_ordered() -> [&'static str; #field_len_token] {
+                [
+                    #(#field_names,)*
+                ]
+            }
+        }
+    };
+
+    let field_names_get = fields.iter().enumerate().map(|(idx, f)| {
+        let field_name = f.ident.as_ref().unwrap();
+        quote! { #field_name: point.fields[#idx].get() }
+    }).collect::<Vec<_>>();
+
+    let from_my_point =  quote! {
+        impl From<ros_pointcloud2::Point<#field_len_token>> for #name {
+            fn from(point: ros_pointcloud2::Point<#field_len_token>) -> Self {
+                Self {
+                    #(#field_names_get,)*
+                }
+            }
+        }
+    };
+
+    let field_names_into = fields.iter().map(|f| {
+        let field_name = f.ident.as_ref().unwrap();
+        quote! { point.#field_name.into() }
+    }).collect::<Vec<_>>();
+
+    let from_custom_point = quote! {
+        impl From<#name> for ros_pointcloud2::Point<#field_len_token> {
+            fn from(point: #name) -> Self {
+                ros_pointcloud2::Point {
+                    fields: [ #(#field_names_into,)* ]
+                }
+            }
+        }
+    };
+
+    let convertible = quote! {
+        impl ros_pointcloud2::PointConvertible<#field_len_token> for #name {}
+    };
+
+    let out = TokenStream::from(quote! {
+        #field_impl
+        #from_my_point
+        #from_custom_point
+        #convertible
+    });
+
+    TokenStream::from(out)
+}
\ No newline at end of file
diff --git a/src/convert.rs b/src/convert.rs
index ca0061e..e07f0c3 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -29,6 +29,24 @@ impl FieldDatatype {
     }
 }
 
+impl TryFrom<String> for FieldDatatype {
+    type Error = ConversionError;
+
+    fn try_from(value: String) -> Result<Self, Self::Error> {
+        match value.as_str() {
+            "f32" => Ok(FieldDatatype::F32),
+            "f64" => Ok(FieldDatatype::F64),
+            "i32" => Ok(FieldDatatype::I32),
+            "u8" => Ok(FieldDatatype::U8),
+            "u16" => Ok(FieldDatatype::U16),
+            "u32" => Ok(FieldDatatype::U32),
+            "i8" => Ok(FieldDatatype::I8),
+            "i16" => Ok(FieldDatatype::I16),
+            _ => Err(ConversionError::UnsupportedFieldType),
+        }
+    }
+}
+
 /// Getter trait for the datatype of a field value.
 pub trait GetFieldDatatype {
     fn field_datatype() -> FieldDatatype;
@@ -115,23 +133,6 @@ impl From<FieldDatatype> for u8 {
     }
 }
 
-pub(crate) fn check_coord(
-    coord: Option<usize>,
-    fields: &[PointFieldMsg],
-    xyz_field_type: &FieldDatatype,
-) -> Result<PointFieldMsg, ConversionError> {
-    match coord {
-        Some(y_idx) => {
-            let field = &fields[y_idx];
-            if field.datatype != u8::from(*xyz_field_type) {
-                return Err(ConversionError::InvalidFieldFormat);
-            }
-            Ok(field.clone())
-        }
-        None => Err(ConversionError::NotEnoughFields),
-    }
-}
-
 /// Matching field names from each meta data per point to the PointField name.
 /// Always make sure to use the same order as in your conversion implementation to have a correct mapping.
 ///
@@ -155,8 +156,8 @@ pub(crate) fn check_coord(
 ///   }
 /// }
 /// ```
-pub trait MetaNames<const METADIM: usize> {
-    fn meta_names() -> [&'static str; METADIM];
+pub trait Fields<const N: usize> {
+    fn field_names_ordered() -> [&'static str; N];
 }
 
 /// This trait is used to convert a byte slice to a primitive type.
@@ -305,7 +306,7 @@ impl FromBytes for u8 {
     }
 }
 
-#[derive(Default, Clone, Debug, PartialEq)]
+#[derive(Default, Clone, Debug, PartialEq, Copy)]
 pub enum Endianness {
     Big,
 
@@ -313,41 +314,6 @@ pub enum Endianness {
     Little,
 }
 
-#[inline(always)]
-pub(crate) fn load_loadable<T: FromBytes, const SIZE: usize>(
-    start_idx: usize,
-    data: &[u8],
-    endian: &Endianness,
-) -> T {
-    match endian {
-        Endianness::Big => T::from_be_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
-        Endianness::Little => T::from_le_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
-    }
-}
-
-/// Note: check if the data slice is long enough to load the bytes beforehand! Uses unsafe indexing.
-#[inline(always)]
-fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> [u8; S] {
-    let mut target = [u8::default(); S];
-
-    debug_assert!(target.len() == S);
-    debug_assert!(
-        data.len() >= start_idx + S,
-        "[ERR] byte read out of bounds: Data len: {}, read_idx: {}",
-        data.len(),
-        start_idx + S
-    );
-
-    let source = unsafe { data.get_unchecked(start_idx..start_idx + S) };
-    target
-        .iter_mut()
-        .zip(source.iter())
-        .for_each(|(target, source)| {
-            *target = *source;
-        });
-    target
-}
-
 #[cfg(test)]
 mod tests {
     use super::*;
diff --git a/src/iterator.rs b/src/iterator.rs
index 888bc5a..985348d 100644
--- a/src/iterator.rs
+++ b/src/iterator.rs
@@ -1,6 +1,6 @@
 use crate::{
-    convert::{check_coord, load_loadable, Endianness, FieldDatatype, FromBytes},
-    ConversionError, MetaNames, Point, PointCloud2Msg, PointConvertible, PointMeta,
+    convert::{Endianness, FieldDatatype},
+    ConversionError, Fields, Point, PointCloud2Msg, PointConvertible, PointMeta,
 };
 
 /// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg.
@@ -20,25 +20,13 @@ use crate::{
 ///
 /// ros_pointcloud2 supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags.
 ///
-pub struct PointCloudIterator<
-    T: FromBytes,
-    const SIZE: usize,
-    const DIM: usize,
-    const METADIM: usize,
-    C,
-> where
-    T: FromBytes,
-    C: MetaNames<METADIM>,
+pub struct PointCloudIterator<const N: usize, C>
+where
+    C: Fields<N>,
 {
     iteration: usize,
     iteration_back: usize,
-    data: Vec<u8>,
-    point_step_size: usize,
-    cloud_length: usize,
-    offsets: Vec<usize>,
-    meta: Vec<(String, FieldDatatype)>,
-    endianness: Endianness,
-    phantom_t: std::marker::PhantomData<T>, // internally used for byte and datatype conversions
+    data: ByteBufferView<N>,
     phantom_c: std::marker::PhantomData<C>, // internally used for meta names array
 }
 
@@ -50,7 +38,7 @@ where
     C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
 {
     fn len(&self) -> usize {
-        self.cloud_length
+        self.data.len()
     }
 }
 
@@ -66,7 +54,8 @@ where
             return None; // iteration finished
         }
 
-        let p = self.point_at(self.iteration_back);
+        let p = self.data.point_at(self.iteration_back);
+        self.iteration_back -= 1;
         Some(C::from(p))
     }
 }
@@ -88,7 +77,7 @@ where
     }
 
     fn opt_len(&self) -> Option<usize> {
-        Some(self.cloud_length)
+        Some(self.data.len())
     }
 }
 
@@ -100,7 +89,7 @@ where
     C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
 {
     fn len(&self) -> usize {
-        self.cloud_length
+        self.data.len()
     }
 
     fn drive<Co>(self, consumer: Co) -> Co::Result
@@ -143,8 +132,8 @@ where
         self.iter
     }
 
-    fn split_at(self, index: usize) -> (Self, Self) {
-        let (left, right) = self.iter.split_at(index);
+    fn split_at(self, point_index: usize) -> (Self, Self) {
+        let (left, right) = self.iter.split_at(point_index);
         (RayonProducer { iter: left }, RayonProducer { iter: right })
     }
 }
@@ -162,44 +151,116 @@ where
 }
 
 /// Implementation of the iterator trait.
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> Iterator
-    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> Iterator for PointCloudIterator<N, C>
 where
-    T: FromBytes,
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
+    C: PointConvertible<N>,
 {
     type Item = C;
 
-    /// The size_hint is the length of the remaining elements and the maximum length of the iterator.
-    ///
-    /// PointCloud2 messages contain the length of the cloud, so we can prepare coming operations.
-    /// This hint is used inside common iterator functions like `collect<Vec<_>>`, for example.
     fn size_hint(&self) -> (usize, Option<usize>) {
-        (self.cloud_length - self.iteration, Some(self.cloud_length))
+        let buf_len = self.data.len();
+        (buf_len, Some(buf_len))
     }
 
-    /// Get the data from the byte buffer and convert it to the predefined point.
-    /// It must keep track of the current iteration and the length of the cloud so it has to mutate self.
-    ///
-    /// The current point is then converted into the custom type. If the conversion fails, an error is returned.
     fn next(&mut self) -> Option<Self::Item> {
-        if self.iteration >= self.cloud_length {
+        if self.iteration >= self.data.len() || self.iteration_back < self.iteration {
             return None; // iteration finished
         }
 
-        let p = self.point_at(self.iteration);
-
+        let p = self.data.point_at(self.iteration);
         self.iteration += 1;
-
         Some(C::from(p))
     }
 }
 
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryFrom<PointCloud2Msg>
-    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+struct ByteBufferView<const N: usize> {
+    data: std::sync::Arc<[u8]>,
+    start_point_idx: usize,
+    end_point_idx: usize,
+    point_step_size: usize,
+    offsets: [usize; N],
+    meta: Vec<(String, FieldDatatype)>,
+    endianness: Endianness,
+}
+
+impl<const N: usize> ByteBufferView<N> {
+    fn new(
+        data: Vec<u8>,
+        point_step_size: usize,
+        start_point_idx: usize,
+        end_point_idx: usize,
+        offsets: [usize; N],
+        meta: Vec<(String, FieldDatatype)>,
+        endianness: Endianness,
+    ) -> Self {
+        Self {
+            data: std::sync::Arc::<[u8]>::from(data),
+            start_point_idx,
+            end_point_idx,
+            point_step_size,
+            offsets,
+            meta,
+            endianness,
+        }
+    }
+
+    #[inline]
+    fn len(&self) -> usize {
+        self.end_point_idx - self.start_point_idx + 1
+    }
+
+    #[inline(always)]
+    fn point_at(&self, idx: usize) -> Point<N> {
+        let offset = (self.start_point_idx + idx) * self.point_step_size;
+
+        // TODO memcpy entire point at once, then extract fields?
+        let mut meta = [PointMeta::default(); N];
+        meta.iter_mut()
+            .zip(self.offsets.iter())
+            .zip(self.meta.iter())
+            .for_each(|((p_meta, in_point_offset), (_, meta_type))| {
+                *p_meta = PointMeta::from_buffer(
+                    &self.data,
+                    offset + in_point_offset,
+                    *meta_type,
+                    self.endianness,
+                );
+            });
+
+        Point { fields: meta }
+    }
+
+    #[inline]
+    fn clone_with_bounds(&self, start: usize, size: usize) -> Self {
+        Self {
+            data: self.data.clone(),
+            start_point_idx: start,
+            end_point_idx: start + size - 1,
+            point_step_size: self.point_step_size,
+            offsets: self.offsets,
+            meta: self.meta.clone(),
+            endianness: self.endianness,
+        }
+    }
+
+    #[inline]
+    pub fn split_at(self, point_index: usize) -> (Self, Self) {
+        let left_start = self.start_point_idx;
+        let left_size = point_index;
+
+        let right_start = point_index;
+        let right_size = self.len() - point_index;
+
+        (
+            self.clone_with_bounds(left_start, left_size),
+            self.clone_with_bounds(right_start, right_size),
+        )
+    }
+}
+
+impl<const N: usize, C> TryFrom<PointCloud2Msg> for PointCloudIterator<N, C>
 where
-    T: FromBytes,
-    C: MetaNames<METADIM>,
+    C: Fields<N>,
 {
     type Error = ConversionError;
 
@@ -209,99 +270,63 @@ where
     /// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
     /// It therefore has a constant time complexity O(1) for practical purposes.
     fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
-        if cloud.fields.len() < DIM {
-            return Err(ConversionError::NotEnoughFields);
+        let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];
+
+        let not_found_fieldnames = C::field_names_ordered()
+            .into_iter()
+            .map(|name| {
+                let found = cloud.fields.iter().any(|field| field.name == *name);
+                (name, found)
+            })
+            .filter(|(_, found)| !*found)
+            .collect::<Vec<_>>();
+
+        if !not_found_fieldnames.is_empty() {
+            let names_not_found = not_found_fieldnames
+                .into_iter()
+                .map(|(name, _)| (*name).to_owned())
+                .collect::<Vec<String>>();
+            return Err(ConversionError::FieldNotFound(names_not_found));
         }
 
-        let xyz_field_type = T::field_datatype();
-
-        let mut has_x: Option<usize> = None;
-        let mut has_y: Option<usize> = None;
-        let mut has_z: Option<usize> = None;
-
-        let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); METADIM];
-
-        let lower_meta_names = C::meta_names()
-            .iter()
-            .map(|x| x.to_lowercase())
-            .collect::<Vec<String>>();
-
-        for (idx, field) in cloud.fields.iter().enumerate() {
-            let lower_field_name = field.name.to_lowercase();
-            match lower_field_name.as_str() {
-                "x" => has_x = Some(idx),
-                "y" => has_y = Some(idx),
-                "z" => has_z = Some(idx),
-                _ => {
-                    if lower_meta_names.contains(&lower_field_name) {
-                        let meta_idx = idx - DIM;
-                        debug_assert!(
-                            meta_idx < meta_with_offsets.len(),
-                            "Meta data length mismatch"
-                        );
-                        meta_with_offsets[meta_idx].0.clone_from(&field.name);
-                        meta_with_offsets[meta_idx].1 = field.datatype.try_into()?;
-                        meta_with_offsets[meta_idx].2 = field.offset as usize;
-                    }
-                }
+        let ordered_fieldnames = C::field_names_ordered();
+        for (field, with_offset) in cloud.fields.iter().zip(meta_with_offsets.iter_mut()) {
+            if ordered_fieldnames.contains(&field.name.as_str()) {
+                *with_offset = (
+                    field.name.clone(),
+                    field.datatype.try_into()?,
+                    field.offset as usize,
+                );
             }
         }
 
         meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
 
         debug_assert!(
-            meta_with_offsets.len() == METADIM,
-            "Meta data length mismatch"
+            meta_with_offsets.len() == N,
+            "Not all fields were found in the message. Expected {} but found {}.",
+            N,
+            meta_with_offsets.len()
         );
 
-        let mut meta_offsets = [usize::default(); METADIM];
-        let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM];
+        let mut offsets = [usize::default(); N];
+        let mut meta = vec![(String::default(), FieldDatatype::default()); N];
 
         meta_with_offsets
             .into_iter()
             .zip(meta.iter_mut())
-            .zip(meta_offsets.iter_mut())
+            .zip(offsets.iter_mut())
             .for_each(|(((name, datatype, offset), meta), meta_offset)| {
                 *meta = (name, datatype);
                 *meta_offset = offset;
             });
 
-        let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
-        let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?;
-
-        let mut offsets = vec![x_field.offset as usize, y_field.offset as usize];
-
-        let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type);
-        match z_field {
-            Ok(z_field) => {
-                offsets.push(z_field.offset as usize);
-            }
-            Err(err) => match err {
-                ConversionError::NotEnoughFields => {
-                    if DIM == 3 {
-                        return Err(ConversionError::NotEnoughFields);
-                    }
-                }
-                _ => return Err(err),
-            },
-        }
-
         let endian = if cloud.is_bigendian {
             Endianness::Big
         } else {
             Endianness::Little
         };
 
-        if offsets.len() != DIM {
-            return Err(ConversionError::NotEnoughFields);
-        }
-
-        offsets.extend(meta_offsets);
-
-        if offsets.len() != DIM + METADIM {
-            return Err(ConversionError::NotEnoughFields);
-        }
-
         let point_step_size = cloud.point_step as usize;
         let cloud_length = cloud.width as usize * cloud.height as usize;
         if point_step_size * cloud_length != cloud.data.len() {
@@ -310,123 +335,176 @@ where
 
         let last_offset = offsets.last().expect("Dimensionality is 0.");
 
-        if let Some(last_meta) = meta.last() {
-            let size_with_last_meta = last_offset + last_meta.1.size();
-            if size_with_last_meta > point_step_size {
-                return Err(ConversionError::DataLengthMismatch);
-            }
-        } else if last_offset + xyz_field_type.size() > point_step_size {
+        let last_meta = meta.last().expect("Dimensionality is 0.");
+        let size_with_last_meta = last_offset + last_meta.1.size();
+        if size_with_last_meta > point_step_size {
             return Err(ConversionError::DataLengthMismatch);
         }
 
         let cloud_length = cloud.width as usize * cloud.height as usize;
 
-        Ok(Self {
-            iteration: 0,
-            iteration_back: cloud_length - 1,
-            data: cloud.data,
+        let data = ByteBufferView::new(
+            cloud.data,
             point_step_size,
-            cloud_length,
+            0,
+            cloud_length - 1,
             offsets,
             meta,
-            endianness: endian,
-            phantom_t: std::marker::PhantomData,
+            endian,
+        );
+
+        Ok(Self {
+            iteration: 0,
+            iteration_back: cloud_length - 1,
+            data,
             phantom_c: std::marker::PhantomData,
         })
     }
 }
 
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
-    PointCloudIterator<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> PointCloudIterator<N, C>
 where
-    T: FromBytes,
-    C: MetaNames<METADIM>,
+    C: Fields<N>,
 {
-    #[inline(always)]
-    fn point_at(&self, offset: usize) -> Point<T, DIM, METADIM> {
-        let mut xyz = [T::default(); DIM];
-        xyz.iter_mut()
-            .zip(self.offsets.iter())
-            .for_each(|(p_xyz, in_point_offset)| {
-                *p_xyz = load_loadable::<T, SIZE>(
-                    (offset * self.point_step_size) + in_point_offset,
-                    &self.data,
-                    &self.endianness,
-                );
-            });
+    #[inline]
+    fn from_byte_buffer_view(data: ByteBufferView<N>) -> Self {
+        Self {
+            iteration: 0,
+            iteration_back: data.len() - 1,
+            data,
+            phantom_c: std::marker::PhantomData,
+        }
+    }
 
-        debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
-        debug_assert!(
-            self.offsets.len() == DIM + METADIM,
-            "Offset length mismatch"
-        );
+    #[inline]
+    pub fn split_at(self, point_index: usize) -> (Self, Self) {
+        let (left_data, right_data) = self.data.split_at(point_index);
+        (
+            Self::from_byte_buffer_view(left_data),
+            Self::from_byte_buffer_view(right_data),
+        )
+    }
+}
 
-        let mut meta = [PointMeta::default(); METADIM];
-        meta.iter_mut()
-            .zip(self.offsets.iter().skip(DIM))
-            .zip(self.meta.iter())
-            .for_each(|((p_meta, in_point_offset), (_, meta_type))| {
-                let start = (offset * self.point_step_size) + in_point_offset;
-                *p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
-            });
+#[cfg(feature = "rayon")]
+mod test {
+
+    #[test]
+    fn test_double_ended_iter() {
+        let cloud = vec![
+            crate::pcl_utils::PointXYZ {
+                x: 1.0,
+                y: 1.0,
+                z: 1.0,
+            },
+            crate::pcl_utils::PointXYZ {
+                x: 2.0,
+                y: 2.0,
+                z: 2.0,
+            },
+            crate::pcl_utils::PointXYZ {
+                x: 3.0,
+                y: 3.0,
+                z: 3.0,
+            },
+        ];
+
+        let internal_msg = crate::PointCloud2Msg::try_from_iterable(cloud).unwrap();
+        let mut iter = crate::iterator::PointCloudIterator::try_from(internal_msg).unwrap();
+        let last_p = iter.next_back();
+
+        assert!(last_p.is_some());
+        let last_p: crate::pcl_utils::PointXYZ = last_p.unwrap();
+
+        assert_eq!(last_p.x, 3.0);
+        assert_eq!(last_p.y, 3.0);
+        assert_eq!(last_p.z, 3.0);
+
+        let first_p = iter.next();
+        assert!(first_p.is_some());
+        let first_p: crate::pcl_utils::PointXYZ = first_p.unwrap();
+
+        assert_eq!(first_p.x, 1.0);
+        assert_eq!(first_p.y, 1.0);
+        assert_eq!(first_p.z, 1.0);
+
+        let last_p = iter.next_back();
+        assert!(last_p.is_some());
+        let last_p: crate::pcl_utils::PointXYZ = last_p.unwrap();
+
+        assert_eq!(last_p.x, 2.0);
+        assert_eq!(last_p.y, 2.0);
+        assert_eq!(last_p.z, 2.0);
+
+        let last_p = iter.next_back();
+        assert!(last_p.is_none());
 
-        Point { coords: xyz, meta }
+        let first_p = iter.next();
+        assert!(first_p.is_none());
     }
 
-    pub fn split_at(self, index: usize) -> (Self, Self) {
-        let data_idx = index * self.point_step_size;
-        let (left, right) = self.data.split_at(data_idx);
-        let left_iteration = self.iteration;
-        let right_iteration = self.iteration + index;
-        let right_iteration_back = self.iteration_back;
-        (
-            Self {
-                iteration: left_iteration,
-                iteration_back: right_iteration,
-                data: left.into(), // TODO richtig mist hier
-                point_step_size: self.point_step_size,
-                cloud_length: right_iteration - left_iteration,
-                offsets: self.offsets.clone(),
-                meta: self.meta.clone(),
-                endianness: self.endianness.clone(),
-                phantom_t: std::marker::PhantomData,
-                phantom_c: std::marker::PhantomData,
+    #[test]
+    fn test_split_at() {
+        let cloud = vec![
+            crate::pcl_utils::PointXYZ {
+                x: 1.0,
+                y: 1.0,
+                z: 1.0,
             },
-            Self {
-                iteration: right_iteration,
-                iteration_back: right_iteration_back,
-                data: right.into(), // TODO richtig mist hier
-                point_step_size: self.point_step_size,
-                cloud_length: right_iteration_back - right_iteration,
-                offsets: self.offsets.clone(),
-                meta: self.meta.clone(),
-                endianness: self.endianness.clone(),
-                phantom_t: std::marker::PhantomData,
-                phantom_c: std::marker::PhantomData,
+            crate::pcl_utils::PointXYZ {
+                x: 2.0,
+                y: 2.0,
+                z: 2.0,
             },
-        )
-    }
+            crate::pcl_utils::PointXYZ {
+                x: 3.0,
+                y: 3.0,
+                z: 3.0,
+            },
+        ];
 
-    /// Split the iterator at the given index.#
-    pub fn from_subslice(
-        data: Vec<u8>,
-        start: usize,
-        end: usize,
-        offsets: Vec<usize>,
-        meta: Vec<(String, FieldDatatype)>,
-        endianness: Endianness,
-    ) -> Self {
-        Self {
-            iteration: start,
-            iteration_back: end,
-            data,
-            point_step_size: 0,
-            cloud_length: end - start,
-            offsets,
-            meta,
-            endianness,
-            phantom_t: std::marker::PhantomData,
-            phantom_c: std::marker::PhantomData,
-        }
+        let internal_msg = crate::PointCloud2Msg::try_from_iterable(cloud).unwrap();
+        let iter = crate::iterator::PointCloudIterator::try_from(internal_msg).unwrap();
+
+        let (mut left, mut right) = iter.split_at(1);
+
+        assert_eq!(left.size_hint(), (1, Some(1)));
+        assert_eq!(right.size_hint(), (2, Some(2)));
+
+        let first_left = left.next();
+        assert!(first_left.is_some());
+        let first_left: crate::pcl_utils::PointXYZ = first_left.unwrap();
+
+        assert_eq!(first_left.x, 1.0);
+        assert_eq!(first_left.y, 1.0);
+        assert_eq!(first_left.z, 1.0);
+
+        let first_right = right.next();
+        assert!(first_right.is_some());
+        let first_right: crate::pcl_utils::PointXYZ = first_right.unwrap();
+
+        assert_eq!(first_right.x, 2.0);
+        assert_eq!(first_right.y, 2.0);
+        assert_eq!(first_right.z, 2.0);
+
+        let last_right = right.next_back();
+        assert!(last_right.is_some());
+
+        let last_right: crate::pcl_utils::PointXYZ = last_right.unwrap();
+        assert_eq!(last_right.x, 3.0);
+        assert_eq!(last_right.y, 3.0);
+        assert_eq!(last_right.z, 3.0);
+
+        let last_left = left.next_back();
+        assert!(last_left.is_none());
+
+        let last_right = right.next_back();
+        assert!(last_right.is_none());
+
+        let first_left = left.next();
+        assert!(first_left.is_none());
+
+        let first_right = right.next();
+        assert!(first_right.is_none());
     }
 }
diff --git a/src/lib.rs b/src/lib.rs
index 95b1e4a..d5793c6 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -56,19 +56,17 @@ pub mod convert;
 pub mod pcl_utils;
 pub mod ros_types;
 
-/// Macro to get the size of a type at compile time. This is a convenience macro to avoid writing out the full std::mem::size_of::<T>().
-/// Use it for your custom point conversion implementations with the [`ros_pointcloud2::PointConvertible`] trait.
-#[macro_export]
-macro_rules! size_of {
-    ($t:ty) => {
-        std::mem::size_of::<$t>()
-    };
-}
-
 pub mod iterator;
+
 use crate::convert::{FieldDatatype, FromBytes};
 use crate::ros_types::{HeaderMsg, PointFieldMsg};
-pub use convert::MetaNames;
+use convert::Endianness;
+pub use convert::Fields;
+
+use type_layout::TypeLayout;
+
+#[cfg(feature = "derive")]
+pub use rpcl2_derive::*;
 
 /// All errors that can occur while converting to or from the PointCloud2 message.
 #[derive(Debug)]
@@ -90,37 +88,12 @@ pub enum ConversionError {
 
     /// The length of the byte buffer in the message does not match the expected length computed from the fields.
     DataLengthMismatch,
-}
 
-#[inline(always)]
-fn add_point_to_byte_buffer<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
-    coords: C,
-    cloud: &mut PointCloud2Msg,
-) -> Result<bool, ConversionError>
-where
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
-    T: FromBytes,
-{
-    let point: Point<T, DIM, METADIM> = coords.into();
-
-    // (x, y, z...)
-    point
-        .coords
-        .iter()
-        .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
-
-    // meta data description
-    point.meta.iter().for_each(|meta| {
-        let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
-        cloud.data.extend_from_slice(truncated_bytes);
-    });
-
-    cloud.width += 1;
-
-    Ok(true)
+    /// There are fields missing in the message.
+    FieldNotFound(Vec<String>),
 }
 
-#[derive(Clone, Debug, Default)]
+#[derive(Clone, Debug)]
 pub struct PointCloud2Msg {
     pub header: HeaderMsg,
     pub height: u32,
@@ -133,7 +106,107 @@ pub struct PointCloud2Msg {
     pub is_dense: bool,
 }
 
+impl Default for PointCloud2Msg {
+    fn default() -> Self {
+        Self {
+            header: HeaderMsg::default(),
+            height: 1, // everything is in one row (unstructured)
+            width: 0,
+            fields: Vec::new(),
+            is_bigendian: false, // ROS default
+            point_step: 0,
+            row_step: 0,
+            data: Vec::new(),
+            is_dense: false, // ROS default
+        }
+    }
+}
+
 impl PointCloud2Msg {
+    fn prepare_direct_copy<const N: usize, C>() -> Result<Self, ConversionError>
+    where
+        C: PointConvertible<N>,
+    {
+        let point: Point<N> = 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 = C::layout();
+        let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(layout.fields.len());
+        for f in layout.fields.into_iter() {
+            match f {
+                PointField::Field {
+                    name,
+                    datatype,
+                    size,
+                } => {
+                    fields.push(PointFieldMsg {
+                        name,
+                        offset,
+                        datatype,
+                        ..Default::default()
+                    });
+                    offset += size; // assume field_count 1
+                }
+                PointField::Padding(size) => {
+                    offset += size; // assume field_count 1
+                }
+            }
+        }
+
+        Ok(PointCloud2Msg {
+            point_step: offset,
+            fields,
+            ..Default::default()
+        })
+    }
+
+    #[inline(always)]
+    fn prepare<const N: usize, C>() -> Result<Self, ConversionError>
+    where
+        C: PointConvertible<N>,
+    {
+        let point: Point<N> = C::default().into();
+        debug_assert!(point.fields.len() == N);
+
+        let meta_names = C::field_names_ordered();
+        debug_assert!(meta_names.len() == N);
+
+        let mut meta_offsets_acc = 0;
+        let mut fields = vec![PointFieldMsg::default(); N];
+        for ((meta_value, meta_name), field_val) in point
+            .fields
+            .into_iter()
+            .zip(meta_names.into_iter())
+            .zip(fields.iter_mut())
+        {
+            let datatype_code = meta_value.datatype.into();
+            if FieldDatatype::try_from(datatype_code).is_err() {
+                return Err(ConversionError::UnsupportedFieldType);
+            }
+
+            let field_count = 1;
+
+            *field_val = PointFieldMsg {
+                name: meta_name.into(),
+                offset: meta_offsets_acc,
+                datatype: datatype_code,
+                count: 1,
+            };
+
+            meta_offsets_acc += field_count * meta_value.datatype.size() as u32
+        }
+
+        Ok(PointCloud2Msg {
+            point_step: meta_offsets_acc,
+            fields,
+            ..Default::default()
+        })
+    }
+
     /// Create a PointCloud2Msg from any iterable type.
     ///
     /// The operation is O(n) in time complexity where n is the number of points in the point cloud.
@@ -151,98 +224,60 @@ impl PointCloud2Msg {
     ///
     // let msg_out = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
     /// ```
-    pub fn try_from_iterable<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
-        iterable: impl IntoIterator<Item = C>,
+    pub fn try_from_iter<const N: usize, C>(
+        iterable: impl Iterator<Item = C>,
     ) -> Result<Self, ConversionError>
     where
-        C: PointConvertible<T, SIZE, DIM, METADIM>,
-        T: FromBytes,
+        C: PointConvertible<N>,
     {
-        if DIM > 3 {
-            return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time?
-        }
+        let mut cloud = Self::prepare::<N, C>()?;
 
-        let mut iter = iterable.into_iter();
-        let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack
+        iterable.into_iter().for_each(|coords| {
+            let point: Point<N> = coords.into();
 
-        if DIM > 1 {
-            fields.push(PointFieldMsg {
-                name: "x".into(),
-                offset: 0,
-                datatype: T::field_datatype().into(),
-                count: 1,
-            });
-
-            fields.push(PointFieldMsg {
-                name: "y".into(),
-                offset: SIZE as u32,
-                datatype: T::field_datatype().into(),
-                count: 1,
+            point.fields.iter().for_each(|meta| {
+                let truncated_bytes = unsafe {
+                    std::slice::from_raw_parts(meta.bytes.as_ptr(), meta.datatype.size())
+                };
+                cloud.data.extend_from_slice(truncated_bytes);
             });
-        }
 
-        if DIM == 3 {
-            fields.push(PointFieldMsg {
-                name: "z".into(),
-                offset: 2 * SIZE as u32,
-                datatype: T::field_datatype().into(),
-                count: 1,
-            });
-        }
+            cloud.width += 1;
+        });
 
-        let first_point = iter.next().ok_or(ConversionError::NoPoints)?;
-        let point: Point<T, DIM, METADIM> = first_point.clone().into();
-        let meta_names = C::meta_names();
+        cloud.row_step = cloud.width * cloud.point_step;
 
-        let mut meta_offsets_acc = DIM as u32 * SIZE as u32;
-        for (meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
-            let datatype_code = meta_value.datatype.into();
-            if FieldDatatype::try_from(datatype_code).is_err() {
-                return Err(ConversionError::UnsupportedFieldType);
-            }
-
-            fields.push(PointFieldMsg {
-                name: meta_name.into(),
-                offset: meta_offsets_acc,
-                datatype: datatype_code,
-                count: 1,
-            });
-            meta_offsets_acc += meta_value.datatype.size() as u32
-        }
-
-        let mut cloud = PointCloud2Msg {
-            point_step: fields.iter().fold(Default::default(), |acc, field| {
-                let field_type: FieldDatatype = field
-                    .datatype
-                    .try_into()
-                    .expect("Unsupported field but checked before.");
-                let field_size = field_type.size();
-                acc + field.count * field_size as u32
-            }),
-            ..Default::default()
-        };
+        Ok(cloud)
+    }
 
-        // actual point -> byte conversion -- O(n)
-        add_point_to_byte_buffer(first_point, &mut cloud)?;
-        for coords in iter {
-            add_point_to_byte_buffer(coords, &mut cloud)?;
+    pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, ConversionError>
+    where
+        C: PointConvertible<N>,
+    {
+        let mut cloud = Self::prepare_direct_copy::<N, C>()?;
+
+        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.fields = fields;
-        cloud.height = 1; // everything is in one row (unstructured)
-        cloud.is_bigendian = false; // ROS default
-        cloud.is_dense = true; // ROS default
-        cloud.row_step = cloud.width * cloud.point_step; // Note: redundant but defined in PointCloud2 message
+        cloud.width = vec.len() as u32;
+        cloud.row_step = cloud.width * cloud.point_step;
 
         Ok(cloud)
     }
 
-    pub fn try_into_iter<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
+    pub fn try_into_iter<const N: usize, C>(
         self,
     ) -> Result<impl Iterator<Item = C>, ConversionError>
     where
-        C: PointConvertible<T, SIZE, DIM, METADIM>,
-        T: FromBytes,
+        C: PointConvertible<N>,
     {
         iterator::PointCloudIterator::try_from(self)
     }
@@ -264,9 +299,8 @@ impl PointCloud2Msg {
 /// Implement the `From` traits for your point type to use the conversion.
 ///
 /// See the [`ros_pointcloud2::PointConvertible`] trait for more information.
-pub struct Point<T, const DIM: usize, const METADIM: usize> {
-    pub coords: [T; DIM],
-    pub meta: [PointMeta; METADIM],
+pub struct Point<const N: usize> {
+    pub fields: [PointMeta; N],
 }
 
 /// Trait to enable point conversions on the fly while iterating.
@@ -313,13 +347,60 @@ pub struct Point<T, const DIM: usize, const METADIM: usize> {
 ///
 /// impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for MyPointXYZI {}
 /// ```
-pub trait PointConvertible<T, const SIZE: usize, const DIM: usize, const METADIM: usize>:
-    From<Point<T, DIM, METADIM>> + Into<Point<T, DIM, METADIM>> + MetaNames<METADIM> + Clone + 'static
-where
-    T: FromBytes,
+pub trait PointConvertible<const N: usize>:
+    KnownLayout + From<Point<N>> + Into<Point<N>> + Fields<N> + Clone + 'static + Default
 {
 }
 
+enum PointField {
+    Padding(u32),
+    Field {
+        name: String,
+        size: u32,
+        datatype: u8,
+    },
+}
+
+impl TryFrom<type_layout::Field> for PointField {
+    type Error = ConversionError;
+
+    fn try_from(f: type_layout::Field) -> Result<Self, Self::Error> {
+        match f {
+            type_layout::Field::Field { name, ty, size } => {
+                let typename: String = ty.to_owned().into();
+                let datatype = FieldDatatype::try_from(typename)?;
+                Ok(Self::Field {
+                    name: name.to_owned().into(),
+                    size: size as u32,
+                    datatype: datatype.into(),
+                })
+            }
+            type_layout::Field::Padding { size } => Ok(Self::Padding(size as u32)),
+        }
+    }
+}
+
+struct TypeLayoutInfo {
+    fields: Vec<PointField>,
+}
+
+impl TryFrom<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
+    type Error = ConversionError;
+
+    fn try_from(t: type_layout::TypeLayoutInfo) -> Result<Self, Self::Error> {
+        let fields: Vec<PointField> = t
+            .fields
+            .into_iter()
+            .map(PointField::try_from)
+            .collect::<Result<Vec<_>, _>>()?;
+        Ok(Self { fields })
+    }
+}
+
+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
@@ -336,6 +417,7 @@ where
 #[derive(Debug, Clone, Copy)]
 pub struct PointMeta {
     bytes: [u8; std::mem::size_of::<f64>()],
+    endianness: Endianness,
     datatype: FieldDatatype,
 }
 
@@ -344,6 +426,7 @@ impl Default for PointMeta {
         Self {
             bytes: [u8::default(); std::mem::size_of::<f64>()],
             datatype: FieldDatatype::F32,
+            endianness: Endianness::default(),
         }
     }
 }
@@ -366,22 +449,29 @@ impl PointMeta {
         Self {
             bytes,
             datatype: T::field_datatype(),
+            ..Default::default()
         }
     }
 
     #[inline(always)]
-    fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self {
+    fn from_buffer(
+        data: &[u8],
+        offset: usize,
+        datatype: FieldDatatype,
+        endianness: Endianness,
+    ) -> Self {
         debug_assert!(data.len() >= offset + datatype.size());
-
-        let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) };
-        let mut bytes_array = [0; std::mem::size_of::<f64>()]; // 8 bytes as f64 is the largest type
-        for (byte, save_byte) in bytes.iter().zip(bytes_array.iter_mut()) {
-            *save_byte = *byte;
+        let bytes = [u8::default(); std::mem::size_of::<f64>()];
+        unsafe {
+            let data_ptr = data.as_ptr().add(offset);
+            let bytes_ptr = bytes.as_ptr() as *mut u8;
+            std::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size());
         }
 
         Self {
-            bytes: bytes_array,
-            datatype: *datatype,
+            bytes,
+            datatype,
+            endianness,
         }
     }
 
@@ -399,7 +489,11 @@ impl PointMeta {
             .bytes
             .get(0..size)
             .expect("Exceeds bounds of f64, which is the largest type");
-        T::from_le_bytes(bytes)
+
+        match self.endianness {
+            Endianness::Big => T::from_be_bytes(bytes),
+            Endianness::Little => T::from_le_bytes(bytes),
+        }
     }
 }
 
diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs
index 2439e9d..ce4458b 100644
--- a/src/pcl_utils.rs
+++ b/src/pcl_utils.rs
@@ -1,4 +1,4 @@
-use crate::{MetaNames, Point, PointConvertible};
+use crate::{Fields, Point, PointConvertible};
 
 /// Pack an RGB color into a single f32 value as used in ROS with PCL for RViz usage.
 #[inline]
@@ -19,43 +19,44 @@ 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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZ {
     pub x: f32,
     pub y: f32,
     pub z: f32,
 }
 
-impl From<Point<f32, 3, 0>> for PointXYZ {
-    fn from(point: Point<f32, 3, 0>) -> Self {
+impl Fields<3> for PointXYZ {
+    fn field_names_ordered() -> [&'static str; 3] {
+        ["x", "y", "z"]
+    }
+}
+
+impl From<Point<3>> for PointXYZ {
+    fn from(point: Point<3>) -> Self {
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
         }
     }
 }
 
-impl From<PointXYZ> for Point<f32, 3, 0> {
+impl From<PointXYZ> for Point<3> {
     fn from(point: PointXYZ) -> Self {
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [],
+            fields: [point.x.into(), point.y.into(), point.z.into()],
         }
     }
 }
 
-impl MetaNames<0> for PointXYZ {
-    fn meta_names() -> [&'static str; 0] {
-        []
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for PointXYZ {}
+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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZI {
     pub x: f32,
     pub y: f32,
@@ -63,37 +64,85 @@ pub struct PointXYZI {
     pub intensity: f32,
 }
 
-impl From<PointXYZI> for Point<f32, 3, 1> {
+impl Fields<4> for PointXYZI {
+    fn field_names_ordered() -> [&'static str; 4] {
+        ["x", "y", "z", "intensity"]
+    }
+}
+
+impl From<Point<4>> for PointXYZI {
+    fn from(point: Point<4>) -> Self {
+        Self {
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
+            intensity: point.fields[3].get(),
+        }
+    }
+}
+
+impl From<PointXYZI> for Point<4> {
     fn from(point: PointXYZI) -> Self {
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [point.intensity.into()],
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.intensity.into(),
+            ],
         }
     }
 }
 
-impl From<Point<f32, 3, 1>> for PointXYZI {
-    fn from(point: Point<f32, 3, 1>) -> Self {
+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 Fields<4> for PointXYZL {
+    fn field_names_ordered() -> [&'static str; 4] {
+        ["x", "y", "z", "label"]
+    }
+}
+
+impl From<Point<4>> for PointXYZL {
+    fn from(point: Point<4>) -> Self {
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
-            intensity: point.meta[0].get(),
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
+            label: point.fields[3].get(),
         }
     }
 }
 
-impl MetaNames<1> for PointXYZI {
-    fn meta_names() -> [&'static str; 1] {
-        ["intensity"]
+impl From<PointXYZL> for Point<4> {
+    fn from(point: PointXYZL) -> Self {
+        Point {
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.label.into(),
+            ],
+        }
     }
 }
 
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 1> for PointXYZI {}
+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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZRGB {
     pub x: f32,
     pub y: f32,
@@ -103,14 +152,20 @@ pub struct PointXYZRGB {
     pub b: u8,
 }
 
-impl From<Point<f32, 3, 1>> for PointXYZRGB {
-    fn from(point: Point<f32, 3, 1>) -> Self {
-        let rgb = point.meta[0].get::<f32>();
+impl Fields<4> for PointXYZRGB {
+    fn field_names_ordered() -> [&'static str; 4] {
+        ["x", "y", "z", "rgb"]
+    }
+}
+
+impl From<Point<4>> for PointXYZRGB {
+    fn from(point: Point<4>) -> Self {
+        let rgb = point.fields[3].get::<f32>();
         let rgb_unpacked = unpack_rgb(rgb);
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
             r: rgb_unpacked[0],
             g: rgb_unpacked[1],
             b: rgb_unpacked[2],
@@ -118,27 +173,22 @@ impl From<Point<f32, 3, 1>> for PointXYZRGB {
     }
 }
 
-impl From<PointXYZRGB> for Point<f32, 3, 1> {
+impl From<PointXYZRGB> for Point<4> {
     fn from(point: PointXYZRGB) -> Self {
+        let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [pack_rgb(point.r, point.g, point.b).into()],
+            fields: [point.x.into(), point.y.into(), point.z.into(), rgb.into()],
         }
     }
 }
 
-impl MetaNames<1> for PointXYZRGB {
-    fn meta_names() -> [&'static str; 1] {
-        ["rgb"]
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 1> for PointXYZRGB {}
+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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZRGBA {
     pub x: f32,
     pub y: f32,
@@ -149,43 +199,49 @@ pub struct PointXYZRGBA {
     pub a: u8,
 }
 
-impl From<Point<f32, 3, 2>> for PointXYZRGBA {
-    fn from(point: Point<f32, 3, 2>) -> Self {
-        let rgb = point.meta[0].get::<f32>();
+impl Fields<5> for PointXYZRGBA {
+    fn field_names_ordered() -> [&'static str; 5] {
+        ["x", "y", "z", "rgb", "a"]
+    }
+}
+
+impl From<Point<5>> for PointXYZRGBA {
+    fn from(point: Point<5>) -> Self {
+        let rgb = point.fields[3].get::<f32>();
         let rgb_unpacked = unpack_rgb(rgb);
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
             r: rgb_unpacked[0],
             g: rgb_unpacked[1],
             b: rgb_unpacked[2],
-            a: point.meta[1].get(),
+            a: point.fields[4].get(),
         }
     }
 }
 
-impl From<PointXYZRGBA> for Point<f32, 3, 2> {
+impl From<PointXYZRGBA> for Point<5> {
     fn from(point: PointXYZRGBA) -> Self {
         let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [rgb.into(), point.a.into()],
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                rgb.into(),
+                point.a.into(),
+            ],
         }
     }
 }
 
-impl MetaNames<2> for PointXYZRGBA {
-    fn meta_names() -> [&'static str; 2] {
-        ["rgb", "a"]
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 2> for PointXYZRGBA {}
+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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZRGBNormal {
     pub x: f32,
     pub y: f32,
@@ -198,30 +254,38 @@ pub struct PointXYZRGBNormal {
     pub normal_z: f32,
 }
 
-impl From<Point<f32, 3, 4>> for PointXYZRGBNormal {
-    fn from(point: Point<f32, 3, 4>) -> Self {
-        let rgb = point.meta[0].get::<f32>();
+impl Fields<7> for PointXYZRGBNormal {
+    fn field_names_ordered() -> [&'static str; 7] {
+        ["x", "y", "z", "rgb", "normal_x", "normal_y", "normal_z"]
+    }
+}
+
+impl From<Point<7>> for PointXYZRGBNormal {
+    fn from(point: Point<7>) -> Self {
+        let rgb = point.fields[3].get::<f32>();
         let rgb_unpacked = unpack_rgb(rgb);
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
             r: rgb_unpacked[0],
             g: rgb_unpacked[1],
             b: rgb_unpacked[2],
-            normal_x: point.meta[1].get(),
-            normal_y: point.meta[2].get(),
-            normal_z: point.meta[3].get(),
+            normal_x: point.fields[4].get(),
+            normal_y: point.fields[5].get(),
+            normal_z: point.fields[6].get(),
         }
     }
 }
 
-impl From<PointXYZRGBNormal> for Point<f32, 3, 4> {
+impl From<PointXYZRGBNormal> for Point<7> {
     fn from(point: PointXYZRGBNormal) -> Self {
         let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
                 rgb.into(),
                 point.normal_x.into(),
                 point.normal_y.into(),
@@ -231,17 +295,12 @@ impl From<PointXYZRGBNormal> for Point<f32, 3, 4> {
     }
 }
 
-impl MetaNames<4> for PointXYZRGBNormal {
-    fn meta_names() -> [&'static str; 4] {
-        ["rgb", "normal_x", "normal_y", "normal_z"]
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 4> for PointXYZRGBNormal {}
+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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZINormal {
     pub x: f32,
     pub y: f32,
@@ -252,83 +311,48 @@ pub struct PointXYZINormal {
     pub normal_z: f32,
 }
 
-impl From<PointXYZINormal> for Point<f32, 3, 4> {
-    fn from(point: PointXYZINormal) -> Self {
-        Point {
-            coords: [point.x, point.y, point.z],
-            meta: [
-                point.intensity.into(),
-                point.normal_x.into(),
-                point.normal_y.into(),
-                point.normal_z.into(),
-            ],
-        }
-    }
-}
-
-impl From<Point<f32, 3, 4>> for PointXYZINormal {
-    fn from(point: Point<f32, 3, 4>) -> Self {
-        Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
-            intensity: point.meta[0].get(),
-            normal_x: point.meta[1].get(),
-            normal_y: point.meta[2].get(),
-            normal_z: point.meta[3].get(),
-        }
+impl Fields<7> for PointXYZINormal {
+    fn field_names_ordered() -> [&'static str; 7] {
+        ["x", "y", "z", "i", "normal_x", "normal_y", "normal_z"]
     }
 }
 
-impl MetaNames<4> for PointXYZINormal {
-    fn meta_names() -> [&'static str; 4] {
-        ["intensity", "normal_x", "normal_y", "normal_z"]
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 4> 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)]
-pub struct PointXYZL {
-    pub x: f32,
-    pub y: f32,
-    pub z: f32,
-    pub label: u32,
-}
-
-impl From<Point<f32, 3, 1>> for PointXYZL {
-    fn from(point: Point<f32, 3, 1>) -> Self {
+impl From<Point<7>> for PointXYZINormal {
+    fn from(point: Point<7>) -> Self {
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
-            label: point.meta[0].get(),
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
+            intensity: point.fields[3].get(),
+            normal_x: point.fields[4].get(),
+            normal_y: point.fields[5].get(),
+            normal_z: point.fields[6].get(),
         }
     }
 }
 
-impl From<PointXYZL> for Point<f32, 3, 1> {
-    fn from(point: PointXYZL) -> Self {
+impl From<PointXYZINormal> for Point<7> {
+    fn from(point: PointXYZINormal) -> Self {
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [point.label.into()],
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.intensity.into(),
+                point.normal_x.into(),
+                point.normal_y.into(),
+                point.normal_z.into(),
+            ],
         }
     }
 }
 
-impl MetaNames<1> for PointXYZL {
-    fn meta_names() -> [&'static str; 1] {
-        ["label"]
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 1> for PointXYZL {}
+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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZRGBL {
     pub x: f32,
     pub y: f32,
@@ -339,45 +363,49 @@ pub struct PointXYZRGBL {
     pub label: u32,
 }
 
-impl From<Point<f32, 3, 2>> for PointXYZRGBL {
-    fn from(point: Point<f32, 3, 2>) -> Self {
-        let rgb = point.meta[0].get::<f32>();
+impl Fields<5> for PointXYZRGBL {
+    fn field_names_ordered() -> [&'static str; 5] {
+        ["x", "y", "z", "rgb", "label"]
+    }
+}
+
+impl From<Point<5>> for PointXYZRGBL {
+    fn from(point: Point<5>) -> Self {
+        let rgb = point.fields[3].get::<f32>();
         let rgb_unpacked = unpack_rgb(rgb);
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
             r: rgb_unpacked[0],
             g: rgb_unpacked[1],
             b: rgb_unpacked[2],
-            label: point.meta[1].get(),
+            label: point.fields[4].get(),
         }
     }
 }
 
-impl From<PointXYZRGBL> for Point<f32, 3, 2> {
+impl From<PointXYZRGBL> for Point<5> {
     fn from(point: PointXYZRGBL) -> Self {
+        let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [
-                pack_rgb(point.r, point.g, point.b).into(),
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                rgb.into(),
                 point.label.into(),
             ],
         }
     }
 }
 
-impl MetaNames<2> for PointXYZRGBL {
-    fn meta_names() -> [&'static str; 2] {
-        ["rgb", "label"]
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 2> for PointXYZRGBL {}
+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)]
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[repr(C)]
 pub struct PointXYZNormal {
     pub x: f32,
     pub y: f32,
@@ -387,24 +415,32 @@ pub struct PointXYZNormal {
     pub normal_z: f32,
 }
 
-impl From<Point<f32, 3, 3>> for PointXYZNormal {
-    fn from(point: Point<f32, 3, 3>) -> Self {
+impl Fields<6> for PointXYZNormal {
+    fn field_names_ordered() -> [&'static str; 6] {
+        ["x", "y", "z", "normal_x", "normal_y", "normal_z"]
+    }
+}
+
+impl From<Point<6>> for PointXYZNormal {
+    fn from(point: Point<6>) -> Self {
         Self {
-            x: point.coords[0],
-            y: point.coords[1],
-            z: point.coords[2],
-            normal_x: point.meta[0].get(),
-            normal_y: point.meta[1].get(),
-            normal_z: point.meta[2].get(),
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
+            normal_x: point.fields[3].get(),
+            normal_y: point.fields[4].get(),
+            normal_z: point.fields[5].get(),
         }
     }
 }
 
-impl From<PointXYZNormal> for Point<f32, 3, 3> {
+impl From<PointXYZNormal> for Point<6> {
     fn from(point: PointXYZNormal) -> Self {
         Point {
-            coords: [point.x, point.y, point.z],
-            meta: [
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
                 point.normal_x.into(),
                 point.normal_y.into(),
                 point.normal_z.into(),
@@ -413,10 +449,4 @@ impl From<PointXYZNormal> for Point<f32, 3, 3> {
     }
 }
 
-impl MetaNames<3> for PointXYZNormal {
-    fn meta_names() -> [&'static str; 3] {
-        ["normal_x", "normal_y", "normal_z"]
-    }
-}
-
-impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 3> for PointXYZNormal {}
+impl PointConvertible<6> for PointXYZNormal {}
diff --git a/src/ros_types.rs b/src/ros_types.rs
index d2466bd..a17dd53 100644
--- a/src/ros_types.rs
+++ b/src/ros_types.rs
@@ -15,7 +15,7 @@ pub struct HeaderMsg {
     pub frame_id: String,
 }
 
-#[derive(Clone, Debug, Default)]
+#[derive(Clone, Debug)]
 pub struct PointFieldMsg {
     pub name: String,
     pub offset: u32,
@@ -23,6 +23,17 @@ pub struct PointFieldMsg {
     pub count: u32,
 }
 
+impl Default for PointFieldMsg {
+    fn default() -> Self {
+        Self {
+            name: String::new(),
+            offset: 0,
+            datatype: 0,
+            count: 1,
+        }
+    }
+}
+
 #[cfg(feature = "r2r_msg")]
 impl From<r2r::sensor_msgs::msg::PointCloud2> for crate::PointCloud2Msg {
     fn from(msg: r2r::sensor_msgs::msg::PointCloud2) -> Self {
diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs
index 8871a25..652e344 100644
--- a/tests/e2e_test.rs
+++ b/tests/e2e_test.rs
@@ -9,9 +9,29 @@ macro_rules! convert_from_into {
     };
 }
 
+macro_rules! convert_from_into_vec {
+    ($point:ty, $cloud:expr) => {
+        convert_from_into_in_out_cloud_vec!($cloud, $point, $cloud, $point);
+    };
+}
+
 macro_rules! convert_from_into_in_out_cloud {
     ($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
-        let msg = PointCloud2Msg::try_from_iterable($in_cloud.clone());
+        let msg = PointCloud2Msg::try_from_iter($in_cloud.clone().into_iter());
+        assert!(msg.is_ok());
+        let msg = msg.unwrap();
+        let to_p_type = msg.try_into_iter();
+        assert!(to_p_type.is_ok());
+        let to_p_type = to_p_type.unwrap();
+        let back_to_type = to_p_type.collect::<Vec<$out_point>>();
+        let orig_cloud: Vec<$out_point> = $out_cloud.iter().cloned().collect();
+        assert_eq!(orig_cloud, back_to_type);
+    };
+}
+
+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());
         assert!(msg.is_ok());
         let msg = msg.unwrap();
         let to_p_type = msg.try_into_iter();
@@ -24,43 +44,75 @@ macro_rules! convert_from_into_in_out_cloud {
 }
 
 #[test]
-fn custom_xyz_f32() {
-    const DIM: usize = 3;
-    const METADIM: usize = 0;
+fn write_cloud() {
+    let cloud = vec![
+        PointXYZ {
+            x: 0.0,
+            y: 1.0,
+            z: 5.0,
+        },
+        PointXYZ {
+            x: 1.0,
+            y: 1.5,
+            z: 5.0,
+        },
+        PointXYZ {
+            x: 1.3,
+            y: 1.6,
+            z: 5.7,
+        },
+        PointXYZ {
+            x: f32::MAX,
+            y: f32::MIN,
+            z: f32::MAX,
+        },
+    ];
+
+    let msg = PointCloud2Msg::try_from_iter(cloud.into_iter());
+    assert!(msg.is_ok());
+}
+
+#[test]
+fn write_cloud_from_vec() {
+    let cloud = vec![
+        PointXYZ {
+            x: 0.0,
+            y: 1.0,
+            z: 5.0,
+        },
+        PointXYZ {
+            x: 1.0,
+            y: 1.5,
+            z: 5.0,
+        },
+        PointXYZ {
+            x: 1.3,
+            y: 1.6,
+            z: 5.7,
+        },
+        PointXYZ {
+            x: f32::MAX,
+            y: f32::MIN,
+            z: f32::MAX,
+        },
+    ];
 
-    #[derive(Debug, PartialEq, Clone)]
+    let msg = PointCloud2Msg::try_from_vec(cloud);
+    assert!(msg.is_ok());
+
+    let msg = msg.unwrap();
+    println!("{:?}", msg);
+}
+
+#[test]
+fn custom_xyz_f32() {
+    #[derive(Debug, PartialEq, Clone, Default, RosFull)]
     struct CustomPoint {
         x: f32,
         y: f32,
         z: f32,
     }
 
-    impl From<CustomPoint> for Point<f32, DIM, METADIM> {
-        fn from(point: CustomPoint) -> Self {
-            Point {
-                coords: [point.x, point.y, point.z],
-                meta: [],
-            }
-        }
-    }
-
-    impl From<Point<f32, DIM, METADIM>> for CustomPoint {
-        fn from(point: Point<f32, 3, 0>) -> Self {
-            Self {
-                x: point.coords[0],
-                y: point.coords[1],
-                z: point.coords[2],
-            }
-        }
-    }
-
-    impl MetaNames<METADIM> for CustomPoint {
-        fn meta_names() -> [&'static str; METADIM] {
-            []
-        }
-    }
-    impl PointConvertible<f32, { std::mem::size_of::<f32>() }, DIM, METADIM> for CustomPoint {}
-
     convert_from_into!(
         CustomPoint,
         vec![
@@ -112,7 +164,7 @@ fn custom_xyzi_f32() {
         },
     ];
 
-    #[derive(Debug, PartialEq, Clone)]
+    #[derive(Debug, PartialEq, Clone, Default, RosFull)]
     struct CustomPointXYZI {
         x: f32,
         y: f32,
@@ -120,40 +172,12 @@ fn custom_xyzi_f32() {
         i: u8,
     }
 
-    impl From<CustomPointXYZI> for Point<f32, 3, 1> {
-        fn from(point: CustomPointXYZI) -> Self {
-            Point {
-                coords: [point.x, point.y, point.z],
-                meta: [point.i.into()],
-            }
-        }
-    }
-
-    impl From<Point<f32, 3, 1>> for CustomPointXYZI {
-        fn from(point: Point<f32, 3, 1>) -> Self {
-            Self {
-                x: point.coords[0],
-                y: point.coords[1],
-                z: point.coords[2],
-                i: point.meta[0].get(),
-            }
-        }
-    }
-
-    impl MetaNames<1> for CustomPointXYZI {
-        fn meta_names() -> [&'static str; 1] {
-            ["intensity"]
-        }
-    }
-
-    impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for CustomPointXYZI {}
-
     convert_from_into!(CustomPointXYZI, cloud);
 }
 
 #[test]
 fn custom_rgba_f32() {
-    #[derive(Debug, PartialEq, Clone)]
+    #[derive(Debug, PartialEq, Clone, Default, RosFull)]
     struct CustomPoint {
         x: f32,
         y: f32,
@@ -164,40 +188,6 @@ fn custom_rgba_f32() {
         a: u8,
     }
 
-    impl From<Point<f32, 3, 4>> for CustomPoint {
-        fn from(point: Point<f32, 3, 4>) -> Self {
-            Self {
-                x: point.coords[0],
-                y: point.coords[1],
-                z: point.coords[2],
-                r: point.meta[0].get(),
-                g: point.meta[1].get(),
-                b: point.meta[2].get(),
-                a: point.meta[3].get(),
-            }
-        }
-    }
-
-    impl From<CustomPoint> for Point<f32, 3, 4> {
-        fn from(point: CustomPoint) -> Self {
-            Point {
-                coords: [point.x, point.y, point.z],
-                meta: [
-                    point.r.into(),
-                    point.g.into(),
-                    point.b.into(),
-                    point.a.into(),
-                ],
-            }
-        }
-    }
-
-    impl MetaNames<4> for CustomPoint {
-        fn meta_names() -> [&'static str; 4] {
-            ["r", "g", "b", "a"]
-        }
-    }
-    impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 4> for CustomPoint {}
     let cloud = vec![
         CustomPoint {
             x: 0.0,
@@ -537,6 +527,47 @@ fn converterxyzrgb() {
     );
 }
 
+#[test]
+fn converterxyzrgb_from_vec() {
+    convert_from_into_vec!(
+        PointXYZRGB,
+        vec![
+            PointXYZRGB {
+                x: 0.0,
+                y: 1.0,
+                z: 5.0,
+                r: 0,
+                g: 0,
+                b: 0,
+            },
+            PointXYZRGB {
+                x: 1.0,
+                y: 1.5,
+                z: 5.0,
+                r: 1,
+                g: 1,
+                b: 1,
+            },
+            PointXYZRGB {
+                x: 1.3,
+                y: 1.6,
+                z: 5.7,
+                r: 2,
+                g: 2,
+                b: 2,
+            },
+            PointXYZRGB {
+                x: f32::MAX,
+                y: f32::MIN,
+                z: f32::MAX,
+                r: u8::MAX,
+                g: u8::MAX,
+                b: u8::MAX,
+            },
+        ]
+    );
+}
+
 #[test]
 fn converterxyzl() {
     convert_from_into!(
@@ -660,7 +691,7 @@ fn write_xyzi_read_xyz() {
 
 #[test]
 fn write_less_than_available() {
-    #[derive(Debug, PartialEq, Clone)]
+    #[derive(Debug, PartialEq, Clone, Default)]
     struct CustomPoint {
         x: f32,
         y: f32,
@@ -668,32 +699,32 @@ fn write_less_than_available() {
         dummy: f32,
     }
 
-    impl From<Point<f32, 3, 0>> for CustomPoint {
-        fn from(point: Point<f32, 3, 0>) -> Self {
+    impl From<Point<3>> for CustomPoint {
+        fn from(point: Point<3>) -> Self {
             Self {
-                x: point.coords[0],
-                y: point.coords[1],
-                z: point.coords[2],
+                x: point.fields[0].get(),
+                y: point.fields[1].get(),
+                z: point.fields[2].get(),
                 dummy: 0.0,
             }
         }
     }
 
-    impl From<CustomPoint> for Point<f32, 3, 0> {
+    impl From<CustomPoint> for Point<3> {
         fn from(point: CustomPoint) -> Self {
             Point {
-                coords: [point.x, point.y, point.z],
-                meta: [],
+                fields: [point.x.into(), point.y.into(), point.z.into()],
             }
         }
     }
 
-    impl MetaNames<0> for CustomPoint {
-        fn meta_names() -> [&'static str; 0] {
-            []
+    impl Fields<3> for CustomPoint {
+        fn field_names_ordered() -> [&'static str; 3] {
+            ["x", "y", "z"]
         }
     }
-    impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
+
+    impl PointConvertible<3> for CustomPoint {}
 
     let write_cloud = vec![
         CustomPoint {

From 2c637c40ed207294ea5c5eee59db81db6bef39f8 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 7 May 2024 10:01:17 +0200
Subject: [PATCH 06/10] 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::<Vec<_>>();
-
-    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::<Vec<_>>();
+
+        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<const N: usize, C>() -> Result<Self, ConversionError>
     where
         C: PointConvertible<N>,
@@ -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<PointFieldMsg> = 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<const N: usize, C>(&self) -> Result<bool, ConversionError>
+    where
+        C: PointConvertible<N>,
+    {
+        let point: Point<N> = 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<const N: usize, C>() -> Result<Self, ConversionError>
     where
@@ -250,27 +299,73 @@ impl PointCloud2Msg {
         Ok(cloud)
     }
 
+    #[cfg(feature = "derive")]
     pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, ConversionError>
     where
         C: PointConvertible<N>,
     {
-        let mut cloud = Self::prepare_direct_copy::<N, C>()?;
+        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::<N, C>()?;
+
+                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<const N: usize, C>(self) -> Result<Vec<C>, ConversionError>
+    where
+        C: PointConvertible<N>,
+    {
+        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::<N, C>()?;
+
+        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<const N: usize, C>(
@@ -347,8 +442,15 @@ pub struct Point<const N: usize> {
 ///
 /// impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for MyPointXYZI {}
 /// ```
+#[cfg(not(feature = "derive"))]
 pub trait PointConvertible<const N: usize>:
-    KnownLayout + From<Point<N>> + Into<Point<N>> + Fields<N> + Clone + 'static + Default
+    From<Point<N>> + Into<Point<N>> + Fields<N> + Clone + 'static + Default
+{
+}
+
+#[cfg(feature = "derive")]
+pub trait PointConvertible<const N: usize>:
+    type_layout::TypeLayout + From<Point<N>> + Into<Point<N>> + Fields<N> + Clone + 'static + Default
 {
 }
 
@@ -397,10 +499,6 @@ impl TryFrom<type_layout::TypeLayoutInfo> 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<CustomPointXYZI> = 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,

From ff8ef4c57c001fe0213f44502bde4fd7f3f6ab62 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 7 May 2024 19:03:36 +0200
Subject: [PATCH 07/10] custom conversions

---
 Cargo.toml                            |  30 +-
 README.md                             |  12 +-
 benches/roundtrip.rs                  |   9 +-
 examples/custom_enum_field_filter.rs  | 160 +++++++++
 examples/custom_label_filter.rs       | 143 --------
 examples/distance_filter.rs           |  49 ---
 examples/{rayon.rs => rayon_bench.rs} |  20 +-
 examples/simple_distance_filter.rs    |  31 ++
 src/convert.rs                        |  58 ++-
 src/iterator.rs                       |  88 ++---
 src/lib.rs                            | 158 +++++----
 src/{pcl_utils.rs => points.rs}       | 413 +++++++++++++++++++---
 src/prelude.rs                        |  15 +
 tests/e2e_test.rs                     | 491 +++++---------------------
 tests/r2r_msg_test.rs                 |   4 +-
 tests/rosrust_msg_test.rs             |   4 +-
 16 files changed, 865 insertions(+), 820 deletions(-)
 create mode 100644 examples/custom_enum_field_filter.rs
 delete mode 100644 examples/custom_label_filter.rs
 delete mode 100644 examples/distance_filter.rs
 rename examples/{rayon.rs => rayon_bench.rs} (86%)
 create mode 100644 examples/simple_distance_filter.rs
 rename src/{pcl_utils.rs => points.rs} (56%)
 create mode 100644 src/prelude.rs

diff --git a/Cargo.toml b/Cargo.toml
index 8c4dadd..89425e1 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -7,21 +7,34 @@ description = "Customizable conversions for working with sensor_msgs/PointCloud2
 repository = "https://github.com/stelzo/ros_pointcloud2"
 license = "MIT"
 keywords = ["ros", "pointcloud2", "pointcloud", "message"]
-categories = ["science::robotics", "encoding", "data-structures", "api-bindings"]
+categories = [
+    "science::robotics",
+    "encoding",
+    "data-structures",
+    "api-bindings",
+]
 readme = "README.md"
 documentation = "https://docs.rs/ros_pointcloud2"
 homepage = "https://github.com/stelzo/ros_pointcloud2"
-exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**/build/**", "**/dist/**", "**/docs/**", "**/doc/**"]
+exclude = [
+    "**/tests/**",
+    "**/examples/**",
+    "**/benches/**",
+    "**/target/**",
+    "**/build/**",
+    "**/dist/**",
+    "**/docs/**",
+    "**/doc/**",
+]
 
 [dependencies]
 rosrust_msg = { version = "0.1", optional = true }
 rosrust = { version = "0.9.11", optional = true }
 r2r = { version = "0.8.4", optional = true }
-rayon = { version = "1.10", optional = true }
-nalgebra = { version = "0.32.5", optional = true }
+rayon = { version = "1", optional = true }
+nalgebra = { version = "0", optional = true }
 rpcl2_derive = { path = "./rpcl2_derive", optional = true }
-bytemuck = "1.15"
-type-layout = "0.2.0"
+type-layout = { version = "0.2", optional = true }
 
 [dev-dependencies]
 rand = "0.8"
@@ -35,6 +48,5 @@ harness = false
 rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
 r2r_msg = ["dep:r2r"]
 rayon = ["dep:rayon"]
-derive = ["dep:rpcl2_derive"]
-
-default = ["derive"]
\ No newline at end of file
+derive = ["dep:rpcl2_derive", "dep:type-layout"]
+nalgebra = ["dep:nalgebra"]
diff --git a/README.md b/README.md
index 0166846..ef290db 100644
--- a/README.md
+++ b/README.md
@@ -20,8 +20,9 @@ Since PCL is the most used library for this work in C++, the predefined types in
 Since ros_pointcloud2 optimizes the performance dynamically, it typically outperforms PCL conversions in all tasks.
 
 ## Quickstart (Iterator)
+
 ```rust
-use ros_pointcloud2::{PointCloud2Msg, pcl_utils::PointXYZ};
+use ros_pointcloud2::prelude::*;
 
 // Your points (here using a predefined type PointXYZ).
 let cloud_points = vec![
@@ -30,7 +31,7 @@ let cloud_points = vec![
 ];
 
 // Give the Vec or anything that implements `IntoIterator`.
-let in_msg = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
+let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
 
 // Convert the ROS crate message type, we will use r2r here.
 // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
@@ -50,6 +51,7 @@ let new_pcl = in_msg.try_into_iter().unwrap()
 ## Integrations
 
 There are currently 3 integrations for common ROS crates.
+
 - [rosrust_msg](https://github.com/adnanademovic/rosrust)
   - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml)
 - [r2r_msg](https://github.com/sequenceplanner/r2r)
@@ -61,6 +63,7 @@ There are currently 3 integrations for common ROS crates.
   - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_iron.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_iron.yml)
 
 You can use `rosrust` and `r2r` by enabling the respective feature:
+
 ```toml
 [dependencies]
 ros_pointcloud2 = { version = "*", features = ["r2r_msg"]}
@@ -69,12 +72,16 @@ ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]}
 ```
 
 ### rclrs (ros2_rust)
+
 Features do not work properly with `rcrls` because the messages are linked externally. You need to use tags instead:
+
 ```toml
 [dependencies]
 ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.4.0_rclrs" }
 ```
+
 Also, indicate the following dependencies to your linker inside the `package.xml` of your package.
+
 ```xml
 <depend>std_msgs</depend>
 <depend>sensor_msgs</depend>
@@ -84,4 +91,5 @@ Also, indicate the following dependencies to your linker inside the `package.xml
 Please open an issue or PR if you want to see support for other crates.
 
 ## License
+
 [MIT](https://choosealicense.com/licenses/mit/)
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
index 1b631f9..9c60355 100644
--- a/benches/roundtrip.rs
+++ b/benches/roundtrip.rs
@@ -1,13 +1,10 @@
 #![cfg(feature = "derive")]
 
 use criterion::{black_box, criterion_group, criterion_main, Criterion};
-use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
+use ros_pointcloud2::prelude::*;
 
 use rand::Rng;
 
-#[cfg(feature = "rayon")]
-use rayon::prelude::*;
-
 pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
     let mut rng = rand::thread_rng();
     let mut pointcloud = Vec::with_capacity(num_points);
@@ -52,7 +49,7 @@ fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
 #[cfg(feature = "rayon")]
 fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
-    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let total = internal_msg
         .try_into_par_iter()
         .unwrap()
@@ -63,7 +60,7 @@ fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
 #[cfg(feature = "rayon")]
 fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
     let orig_len: usize = cloud.len();
-    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let total = internal_msg
         .try_into_par_iter()
         .unwrap()
diff --git a/examples/custom_enum_field_filter.rs b/examples/custom_enum_field_filter.rs
new file mode 100644
index 0000000..05d7ef7
--- /dev/null
+++ b/examples/custom_enum_field_filter.rs
@@ -0,0 +1,160 @@
+/// This example demonstrates how to use a custom point with encoded metadata.
+/// The use case is a segmentation point cloud where each point holds a label and we want to filter by it.
+/// Since the datatypes for the PointCloud2 message are very limited,
+/// we need to encode the enum into a supported type.
+/// This needs some manual work to tell the library how to encode and decode the enum.
+///
+/// Important Note: This example is only possible with disabled `derive` feature,
+/// because the library (currently) does not know the size of your chosen supported type at compile time.
+/// This makes direct copies impossible.
+use ros_pointcloud2::prelude::*;
+
+#[derive(Debug, PartialEq, Clone, Default, Copy)]
+enum Label {
+    #[default]
+    Human,
+    Deer,
+    Car,
+}
+
+// Define a custom point with an enum (not supported by PointCloud2)
+#[derive(Debug, PartialEq, Clone, Default)]
+struct CustomPoint {
+    x: f32,
+    y: f32,
+    z: f32,
+    intensity: f32,
+    my_custom_label: Label,
+}
+
+// For us as convenience
+impl CustomPoint {
+    fn new(x: f32, y: f32, z: f32, intensity: f32, my_custom_label: Label) -> Self {
+        Self {
+            x,
+            y,
+            z,
+            intensity,
+            my_custom_label,
+        }
+    }
+}
+
+impl From<Label> for u8 {
+    fn from(label: Label) -> Self {
+        match label {
+            Label::Human => 0,
+            Label::Deer => 1,
+            Label::Car => 2,
+        }
+    }
+}
+
+impl From<u8> for Label {
+    fn from(label: u8) -> Self {
+        match label {
+            0 => Label::Human,
+            1 => Label::Deer,
+            2 => Label::Car,
+            _ => panic!("Invalid label"),
+        }
+    }
+}
+
+// We implement the PointConvertible trait (needed for every custom point)
+
+// A ros_pointcloud2::Point takes as generic arguments:
+// - coordinate type
+// - dimension (xyz = 3)
+// - metadata dimension (intensity + my_custom_label = 2)
+impl From<CustomPoint> for Point<5> {
+    fn from(point: CustomPoint) -> Self {
+        Point {
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.intensity.into(),
+                u8::from(point.my_custom_label).into(),
+            ],
+        }
+    }
+}
+
+impl From<Point<5>> for CustomPoint {
+    fn from(point: Point<5>) -> Self {
+        Self {
+            x: point.fields[0].get(),
+            y: point.fields[1].get(),
+            z: point.fields[2].get(),
+            intensity: point.fields[3].get(),
+            my_custom_label: point.fields[4].get(), // decoding the label from u8
+        }
+    }
+}
+
+impl Fields<5> for CustomPoint {
+    fn field_names_ordered() -> [&'static str; 5] {
+        ["x", "y", "z", "intensity", "my_custom_label"] // how we want to name the metadata in the message
+    }
+}
+
+// We implemented everything that is needed so we declare it as a PointConvertible
+#[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 {
+    fn field_datatype() -> FieldDatatype {
+        FieldDatatype::U8 // we want to encode the label as u8
+    }
+}
+
+// Again, you don't need this with only supported field types.
+// Technically, PointCloud2 supports big and little endian even though it is rarely used.
+// 'be' stands for big endian and 'le' for little endian.
+impl FromBytes for Label {
+    // u8 -> Label
+    fn from_be_bytes(bytes: &[u8]) -> Self {
+        u8::from_be_bytes([bytes[0]]).into()
+    }
+
+    fn from_le_bytes(bytes: &[u8]) -> Self {
+        u8::from_le_bytes([bytes[0]]).into()
+    }
+
+    // Label -> u8
+    fn bytes(label: &Self) -> Vec<u8> {
+        u8::bytes(&u8::from(*label))
+    }
+}
+
+fn main() {
+    #[cfg(not(feature = "derive"))]
+    {
+        let cloud = vec![
+            CustomPoint::new(1.0, 2.0, 3.0, 4.0, Label::Deer),
+            CustomPoint::new(4.0, 5.0, 6.0, 7.0, Label::Car),
+            CustomPoint::new(7.0, 8.0, 9.0, 10.0, Label::Human),
+        ];
+
+        println!("Original cloud: {:?}", cloud);
+
+        let msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+
+        println!("filtering by label == Deer");
+        let out = msg
+            .try_into_iter()
+            .unwrap()
+            .filter(|point: &CustomPoint| point.my_custom_label == Label::Deer)
+            .collect::<Vec<_>>();
+
+        println!("Filtered cloud: {:?}", out);
+
+        assert_eq!(
+            vec![CustomPoint::new(1.0, 2.0, 3.0, 4.0, Label::Deer),],
+            out
+        );
+    }
+}
diff --git a/examples/custom_label_filter.rs b/examples/custom_label_filter.rs
deleted file mode 100644
index a27404d..0000000
--- a/examples/custom_label_filter.rs
+++ /dev/null
@@ -1,143 +0,0 @@
-// This example demonstrates how to use custom point types with custom metadata.
-// 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};
-
-#[cfg(not(feature = "derive"))]
-use ros_pointcloud2::{PointCloud2Msg, PointConvertible};
-
-#[derive(Debug, PartialEq, Clone, Default)]
-enum Label {
-    #[default]
-    Human,
-    Deer,
-    Car,
-}
-
-impl From<Label> for u8 {
-    fn from(label: Label) -> Self {
-        match label {
-            Label::Human => 0,
-            Label::Deer => 1,
-            Label::Car => 2,
-        }
-    }
-}
-
-impl From<u8> for Label {
-    fn from(label: u8) -> Self {
-        match label {
-            0 => Label::Human,
-            1 => Label::Deer,
-            2 => Label::Car,
-            _ => panic!("Invalid label"),
-        }
-    }
-}
-
-// Custom point type with custom metadata
-#[derive(Debug, PartialEq, Clone, Default)]
-struct CustomPoint {
-    x: f32,
-    y: f32,
-    z: f32,
-    intensity: f32,
-    my_custom_label: Label,
-}
-
-// We implement the PointConvertible trait
-
-// A ros_pointcloud2::Point takes as generic arguments:
-// - coordinate type
-// - dimension (xyz = 3)
-// - metadata dimension (intensity + my_custom_label = 2)
-impl From<CustomPoint> for Point<5> {
-    fn from(point: CustomPoint) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                point.intensity.into(),
-                u8::from(point.my_custom_label).into(),
-            ],
-        }
-    }
-}
-
-impl From<Point<5>> for CustomPoint {
-    fn from(point: Point<5>) -> Self {
-        let label_raw: u8 = point.fields[1].get();
-        Self {
-            x: point.fields[0].get(),
-            y: point.fields[1].get(),
-            z: point.fields[2].get(),
-            intensity: point.fields[3].get(),
-            my_custom_label: Label::from(label_raw), // decoding the label from u8
-        }
-    }
-}
-
-impl Fields<5> for CustomPoint {
-    fn field_names_ordered() -> [&'static str; 5] {
-        ["x", "y", "z", "intensity", "my_custom_label"] // how we want to name the metadata in the message
-    }
-}
-
-// We implemented everything that is needed so we declare it as a PointConvertible
-#[cfg(not(feature = "derive"))]
-impl PointConvertible<5> for CustomPoint {}
-
-fn main() {
-    #[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::<Vec<_>>();
-
-        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/examples/distance_filter.rs b/examples/distance_filter.rs
deleted file mode 100644
index 4debb98..0000000
--- a/examples/distance_filter.rs
+++ /dev/null
@@ -1,49 +0,0 @@
-// This example demonstrates a very simple distance filter with predefined point types.
-// Note that this example is a simplified version of the custom_filter.rs example.
-// Also, it effectively demonstrates a typesafe byte-to-byte buffer filter with a single iteration.
-
-use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
-
-fn main() {
-    let cloud = vec![
-        PointXYZ {
-            x: 1.0,
-            y: 1.0,
-            z: 1.0,
-        },
-        PointXYZ {
-            x: 2.0,
-            y: 2.0,
-            z: 2.0,
-        },
-        PointXYZ {
-            x: 3.0,
-            y: 3.0,
-            z: 3.0,
-        },
-    ];
-
-    println!("Original cloud: {:?}", cloud);
-
-    let msg = PointCloud2Msg::try_from_iter(cloud.clone().into_iter()).unwrap();
-
-    println!("filtering by distance < 1.9m");
-    let out = msg
-        .try_into_iter()
-        .unwrap()
-        .filter(|point: &PointXYZ| {
-            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
-        })
-        .collect::<Vec<_>>();
-
-    println!("Filtered cloud: {:?}", out);
-
-    assert_eq!(
-        vec![PointXYZ {
-            x: 1.0,
-            y: 1.0,
-            z: 1.0,
-        },],
-        out
-    );
-}
diff --git a/examples/rayon.rs b/examples/rayon_bench.rs
similarity index 86%
rename from examples/rayon.rs
rename to examples/rayon_bench.rs
index 1d522d7..c3aa9eb 100644
--- a/examples/rayon.rs
+++ b/examples/rayon_bench.rs
@@ -1,17 +1,13 @@
-// This example demonstrates a very simple distance filter with predefined point types.
-// Note that this example is a simplified version of the custom_filter.rs example.
-// Also, it effectively demonstrates a typesafe byte-to-byte buffer filter with a single iteration.
-
+/// This example implements a naive benchmark for the library so you can evaluate the use of rayon for parallel processing.
+/// It generates a random point cloud and measures the time it takes to iterate over it.
+/// The code works mainly as a showcase. For real benchmarks, check the `benches` directory.
+/// It also implements the same benchmark shown here.
 use std::time::Duration;
 
-use criterion::black_box;
-use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
+use ros_pointcloud2::prelude::*;
 
 use rand::Rng;
 
-#[cfg(feature = "rayon")]
-use rayon::prelude::*;
-
 pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
     let mut rng = rand::thread_rng();
     let mut pointcloud = Vec::with_capacity(num_points);
@@ -56,7 +52,7 @@ fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
 #[cfg(feature = "rayon")]
 fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
-    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let total = internal_msg
         .try_into_par_iter()
         .unwrap()
@@ -67,7 +63,7 @@ fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
 #[cfg(feature = "rayon")]
 fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
     let orig_len: usize = cloud.len();
-    let internal_msg = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let total = internal_msg
         .try_into_par_iter()
         .unwrap()
@@ -104,7 +100,7 @@ where
 {
     let cloud_points = generate_random_pointcloud(pcl_size, f32::MIN / 2.0, f32::MAX / 2.0);
     let start = std::time::Instant::now();
-    black_box(func(cloud_points));
+    let _ = func(cloud_points);
     start.elapsed()
 }
 
diff --git a/examples/simple_distance_filter.rs b/examples/simple_distance_filter.rs
new file mode 100644
index 0000000..59c6948
--- /dev/null
+++ b/examples/simple_distance_filter.rs
@@ -0,0 +1,31 @@
+/// This example demonstrates a very simple distance filter with predefined point types.
+/// Note that this example is a simplified version of the custom_enum_field_filter.rs example.
+/// Also, it effectively demonstrates a typesafe byte-to-byte buffer filter with a single iteration.
+///
+/// It also works without any dependencies, making it a good "hello world" example.
+use ros_pointcloud2::prelude::*;
+
+fn main() {
+    let cloud = vec![
+        PointXYZ::new(1.0, 1.0, 1.0),
+        PointXYZ::new(2.0, 2.0, 2.0),
+        PointXYZ::new(3.0, 3.0, 3.0),
+    ];
+
+    println!("Original cloud: {:?}", cloud);
+
+    let msg = PointCloud2Msg::try_from_iter(cloud.clone().into_iter()).unwrap();
+
+    println!("filtering by distance < 1.9m");
+    let out = msg
+        .try_into_iter()
+        .unwrap()
+        .filter(|point: &PointXYZ| {
+            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+        })
+        .collect::<Vec<_>>();
+
+    println!("Filtered cloud: {:?}", out);
+
+    assert_eq!(vec![PointXYZ::new(1.0, 1.0, 1.0),], out);
+}
diff --git a/src/convert.rs b/src/convert.rs
index e07f0c3..14aaf1e 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -12,28 +12,34 @@ pub enum FieldDatatype {
     U32,
     I8,
     I16,
+
+    /// While RGB is not officially supported by ROS, it is used in practice as a packed f32.
+    /// To make it easier to work with and avoid packing code, the
+    /// [`ros_pointcloud2::points::RGB`] union is supported here and handled like a f32.
+    RGB,
 }
 
 impl FieldDatatype {
     pub fn size(&self) -> usize {
         match self {
-            FieldDatatype::U8 => 1,
-            FieldDatatype::U16 => 2,
-            FieldDatatype::U32 => 4,
-            FieldDatatype::I8 => 1,
-            FieldDatatype::I16 => 2,
-            FieldDatatype::I32 => 4,
-            FieldDatatype::F32 => 4,
-            FieldDatatype::F64 => 8,
+            FieldDatatype::U8 => std::mem::size_of::<u8>(),
+            FieldDatatype::U16 => std::mem::size_of::<u16>(),
+            FieldDatatype::U32 => std::mem::size_of::<u32>(),
+            FieldDatatype::I8 => std::mem::size_of::<i8>(),
+            FieldDatatype::I16 => std::mem::size_of::<i16>(),
+            FieldDatatype::I32 => std::mem::size_of::<i32>(),
+            FieldDatatype::F32 => std::mem::size_of::<f32>(),
+            FieldDatatype::F64 => std::mem::size_of::<f64>(),
+            FieldDatatype::RGB => std::mem::size_of::<f32>(), // packed in f32
         }
     }
 }
 
 impl TryFrom<String> for FieldDatatype {
-    type Error = ConversionError;
+    type Error = MsgConversionError;
 
     fn try_from(value: String) -> Result<Self, Self::Error> {
-        match value.as_str() {
+        match value.to_lowercase().as_str() {
             "f32" => Ok(FieldDatatype::F32),
             "f64" => Ok(FieldDatatype::F64),
             "i32" => Ok(FieldDatatype::I32),
@@ -42,7 +48,8 @@ impl TryFrom<String> for FieldDatatype {
             "u32" => Ok(FieldDatatype::U32),
             "i8" => Ok(FieldDatatype::I8),
             "i16" => Ok(FieldDatatype::I16),
-            _ => Err(ConversionError::UnsupportedFieldType),
+            "rgb" => Ok(FieldDatatype::RGB),
+            _ => Err(MsgConversionError::UnsupportedFieldType(value)),
         }
     }
 }
@@ -100,8 +107,15 @@ impl GetFieldDatatype for i16 {
     }
 }
 
+/// Convenience implementation for the RGB union.
+impl GetFieldDatatype for crate::points::RGB {
+    fn field_datatype() -> FieldDatatype {
+        FieldDatatype::RGB
+    }
+}
+
 impl TryFrom<u8> for FieldDatatype {
-    type Error = ConversionError;
+    type Error = MsgConversionError;
 
     fn try_from(value: u8) -> Result<Self, Self::Error> {
         match value {
@@ -113,7 +127,7 @@ impl TryFrom<u8> for FieldDatatype {
             6 => Ok(FieldDatatype::U32),
             7 => Ok(FieldDatatype::F32),
             8 => Ok(FieldDatatype::F64),
-            _ => Err(ConversionError::UnsupportedFieldType),
+            _ => Err(MsgConversionError::UnsupportedFieldType(value.to_string())),
         }
     }
 }
@@ -129,6 +143,7 @@ impl From<FieldDatatype> for u8 {
             FieldDatatype::U32 => 6,
             FieldDatatype::F32 => 7,
             FieldDatatype::F64 => 8,
+            FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer
         }
     }
 }
@@ -254,6 +269,23 @@ impl FromBytes for f32 {
     }
 }
 
+impl FromBytes for points::RGB {
+    #[inline]
+    fn from_be_bytes(bytes: &[u8]) -> Self {
+        Self::new_from_packed_f32(f32::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]))
+    }
+
+    #[inline]
+    fn from_le_bytes(bytes: &[u8]) -> Self {
+        Self::new_from_packed_f32(f32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]))
+    }
+
+    #[inline]
+    fn bytes(x: &points::RGB) -> Vec<u8> {
+        Vec::from(x.raw().to_le_bytes())
+    }
+}
+
 impl FromBytes for i32 {
     #[inline]
     fn from_be_bytes(bytes: &[u8]) -> Self {
diff --git a/src/iterator.rs b/src/iterator.rs
index 985348d..fdbcc22 100644
--- a/src/iterator.rs
+++ b/src/iterator.rs
@@ -1,6 +1,6 @@
 use crate::{
     convert::{Endianness, FieldDatatype},
-    ConversionError, Fields, Point, PointCloud2Msg, PointConvertible, PointMeta,
+    Fields, MsgConversionError, Point, PointCloud2Msg, PointConvertible, PointMeta,
 };
 
 /// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg.
@@ -31,11 +31,9 @@ where
 }
 
 #[cfg(feature = "rayon")]
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> ExactSizeIterator
-    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> ExactSizeIterator for PointCloudIterator<N, C>
 where
-    T: FromBytes + Send + Sync,
-    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+    C: PointConvertible<N> + Send + Sync,
 {
     fn len(&self) -> usize {
         self.data.len()
@@ -43,11 +41,9 @@ where
 }
 
 #[cfg(feature = "rayon")]
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> DoubleEndedIterator
-    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> DoubleEndedIterator for PointCloudIterator<N, C>
 where
-    T: FromBytes + Send + Sync,
-    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+    C: PointConvertible<N> + Send + Sync,
 {
     fn next_back(&mut self) -> Option<Self::Item> {
         if self.iteration_back < self.iteration {
@@ -61,11 +57,9 @@ where
 }
 
 #[cfg(feature = "rayon")]
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> rayon::iter::ParallelIterator
-    for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> rayon::iter::ParallelIterator for PointCloudIterator<N, C>
 where
-    T: FromBytes + Send + Sync,
-    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+    C: PointConvertible<N> + Send + Sync,
 {
     type Item = C;
 
@@ -82,11 +76,9 @@ where
 }
 
 #[cfg(feature = "rayon")]
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
-    rayon::iter::IndexedParallelIterator for PointCloudIterator<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> rayon::iter::IndexedParallelIterator for PointCloudIterator<N, C>
 where
-    T: FromBytes + Send + Sync,
-    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+    C: PointConvertible<N> + Send + Sync,
 {
     fn len(&self) -> usize {
         self.data.len()
@@ -108,25 +100,17 @@ where
 }
 
 #[cfg(feature = "rayon")]
-struct RayonProducer<
-    T: FromBytes,
-    const SIZE: usize,
-    const DIM: usize,
-    const METADIM: usize,
-    C: PointConvertible<T, SIZE, DIM, METADIM>,
-> {
-    iter: PointCloudIterator<T, SIZE, DIM, METADIM, C>,
+struct RayonProducer<const N: usize, C: PointConvertible<N>> {
+    iter: PointCloudIterator<N, C>,
 }
 
 #[cfg(feature = "rayon")]
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
-    rayon::iter::plumbing::Producer for RayonProducer<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> rayon::iter::plumbing::Producer for RayonProducer<N, C>
 where
-    T: FromBytes + Send + Sync,
-    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+    C: PointConvertible<N> + Send + Sync,
 {
     type Item = C;
-    type IntoIter = PointCloudIterator<T, SIZE, DIM, METADIM, C>;
+    type IntoIter = PointCloudIterator<N, C>;
 
     fn into_iter(self) -> Self::IntoIter {
         self.iter
@@ -139,13 +123,11 @@ where
 }
 
 #[cfg(feature = "rayon")]
-impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
-    From<PointCloudIterator<T, SIZE, DIM, METADIM, C>> for RayonProducer<T, SIZE, DIM, METADIM, C>
+impl<const N: usize, C> From<PointCloudIterator<N, C>> for RayonProducer<N, C>
 where
-    T: FromBytes + Send + Sync,
-    C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
+    C: PointConvertible<N> + Send + Sync,
 {
-    fn from(iterator: PointCloudIterator<T, SIZE, DIM, METADIM, C>) -> Self {
+    fn from(iterator: PointCloudIterator<N, C>) -> Self {
         Self { iter: iterator }
     }
 }
@@ -262,7 +244,7 @@ impl<const N: usize, C> TryFrom<PointCloud2Msg> for PointCloudIterator<N, C>
 where
     C: Fields<N>,
 {
-    type Error = ConversionError;
+    type Error = MsgConversionError;
 
     /// Convert a PointCloud2Msg into an iterator.
     /// Converting a PointCloud2Msg into an iterator is a fallible operation since the message can contain only a subset of the required fields.
@@ -286,7 +268,7 @@ where
                 .into_iter()
                 .map(|(name, _)| (*name).to_owned())
                 .collect::<Vec<String>>();
-            return Err(ConversionError::FieldNotFound(names_not_found));
+            return Err(MsgConversionError::FieldNotFound(names_not_found));
         }
 
         let ordered_fieldnames = C::field_names_ordered();
@@ -330,7 +312,7 @@ where
         let point_step_size = cloud.point_step as usize;
         let cloud_length = cloud.width as usize * cloud.height as usize;
         if point_step_size * cloud_length != cloud.data.len() {
-            return Err(ConversionError::DataLengthMismatch);
+            return Err(MsgConversionError::DataLengthMismatch);
         }
 
         let last_offset = offsets.last().expect("Dimensionality is 0.");
@@ -338,7 +320,7 @@ where
         let last_meta = meta.last().expect("Dimensionality is 0.");
         let size_with_last_meta = last_offset + last_meta.1.size();
         if size_with_last_meta > point_step_size {
-            return Err(ConversionError::DataLengthMismatch);
+            return Err(MsgConversionError::DataLengthMismatch);
         }
 
         let cloud_length = cloud.width as usize * cloud.height as usize;
@@ -392,29 +374,29 @@ mod test {
     #[test]
     fn test_double_ended_iter() {
         let cloud = vec![
-            crate::pcl_utils::PointXYZ {
+            crate::points::PointXYZ {
                 x: 1.0,
                 y: 1.0,
                 z: 1.0,
             },
-            crate::pcl_utils::PointXYZ {
+            crate::points::PointXYZ {
                 x: 2.0,
                 y: 2.0,
                 z: 2.0,
             },
-            crate::pcl_utils::PointXYZ {
+            crate::points::PointXYZ {
                 x: 3.0,
                 y: 3.0,
                 z: 3.0,
             },
         ];
 
-        let internal_msg = crate::PointCloud2Msg::try_from_iterable(cloud).unwrap();
+        let internal_msg = crate::PointCloud2Msg::try_from_iter(cloud).unwrap();
         let mut iter = crate::iterator::PointCloudIterator::try_from(internal_msg).unwrap();
         let last_p = iter.next_back();
 
         assert!(last_p.is_some());
-        let last_p: crate::pcl_utils::PointXYZ = last_p.unwrap();
+        let last_p: crate::points::PointXYZ = last_p.unwrap();
 
         assert_eq!(last_p.x, 3.0);
         assert_eq!(last_p.y, 3.0);
@@ -422,7 +404,7 @@ mod test {
 
         let first_p = iter.next();
         assert!(first_p.is_some());
-        let first_p: crate::pcl_utils::PointXYZ = first_p.unwrap();
+        let first_p: crate::points::PointXYZ = first_p.unwrap();
 
         assert_eq!(first_p.x, 1.0);
         assert_eq!(first_p.y, 1.0);
@@ -430,7 +412,7 @@ mod test {
 
         let last_p = iter.next_back();
         assert!(last_p.is_some());
-        let last_p: crate::pcl_utils::PointXYZ = last_p.unwrap();
+        let last_p: crate::points::PointXYZ = last_p.unwrap();
 
         assert_eq!(last_p.x, 2.0);
         assert_eq!(last_p.y, 2.0);
@@ -446,24 +428,24 @@ mod test {
     #[test]
     fn test_split_at() {
         let cloud = vec![
-            crate::pcl_utils::PointXYZ {
+            crate::points::PointXYZ {
                 x: 1.0,
                 y: 1.0,
                 z: 1.0,
             },
-            crate::pcl_utils::PointXYZ {
+            crate::points::PointXYZ {
                 x: 2.0,
                 y: 2.0,
                 z: 2.0,
             },
-            crate::pcl_utils::PointXYZ {
+            crate::points::PointXYZ {
                 x: 3.0,
                 y: 3.0,
                 z: 3.0,
             },
         ];
 
-        let internal_msg = crate::PointCloud2Msg::try_from_iterable(cloud).unwrap();
+        let internal_msg = crate::PointCloud2Msg::try_from_iter(cloud).unwrap();
         let iter = crate::iterator::PointCloudIterator::try_from(internal_msg).unwrap();
 
         let (mut left, mut right) = iter.split_at(1);
@@ -473,7 +455,7 @@ mod test {
 
         let first_left = left.next();
         assert!(first_left.is_some());
-        let first_left: crate::pcl_utils::PointXYZ = first_left.unwrap();
+        let first_left: crate::points::PointXYZ = first_left.unwrap();
 
         assert_eq!(first_left.x, 1.0);
         assert_eq!(first_left.y, 1.0);
@@ -481,7 +463,7 @@ mod test {
 
         let first_right = right.next();
         assert!(first_right.is_some());
-        let first_right: crate::pcl_utils::PointXYZ = first_right.unwrap();
+        let first_right: crate::points::PointXYZ = first_right.unwrap();
 
         assert_eq!(first_right.x, 2.0);
         assert_eq!(first_right.y, 2.0);
@@ -490,7 +472,7 @@ mod test {
         let last_right = right.next_back();
         assert!(last_right.is_some());
 
-        let last_right: crate::pcl_utils::PointXYZ = last_right.unwrap();
+        let last_right: crate::points::PointXYZ = last_right.unwrap();
         assert_eq!(last_right.x, 3.0);
         assert_eq!(last_right.y, 3.0);
         assert_eq!(last_right.z, 3.0);
diff --git a/src/lib.rs b/src/lib.rs
index c764b3c..3ef4075 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -3,7 +3,7 @@
 //! ros_pointcloud2 mainly provides a [`ros_pointcloud2::PointCloud2Msg`] type that implements
 //! functions for conversion to and from iterators.
 //!
-//! - [`ros_pointcloud2::PointCloud2Msg::try_from_iterable`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_from_iter`]
 //! - [`ros_pointcloud2::PointCloud2Msg::try_into_iter`]
 //!
 //! For ROS interoperability, the message type generated by the respective crate must be converted to
@@ -33,7 +33,7 @@
 //! let cloud_copy = cloud_points.clone();
 //!
 //! // Give the Vec or anything that implements `IntoIterator`.
-//! let in_msg = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
+//! let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
 //!
 //! // Convert to your ROS crate message type, we will use r2r here.
 //! // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
@@ -53,7 +53,8 @@
 //! ```
 
 pub mod convert;
-pub mod pcl_utils;
+pub mod points;
+pub mod prelude;
 pub mod ros_types;
 
 pub mod iterator;
@@ -63,35 +64,69 @@ use crate::ros_types::{HeaderMsg, PointFieldMsg};
 use convert::Endianness;
 pub use convert::Fields;
 
-#[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 {
-    /// The coordinate field does not match the expected datatype.
+pub enum MsgConversionError {
     InvalidFieldFormat,
-
-    /// Not enough meta or dimensional fields in the PointCloud2 message.
     NotEnoughFields,
-
-    /// The dimensionality of the point inside the message is too high.
     TooManyDimensions,
+    UnsupportedFieldType(String),
+    NoPoints,
+    DataLengthMismatch,
+    FieldNotFound(Vec<String>),
+}
 
-    /// The field type is not supported by the ROS message description.
-    UnsupportedFieldType,
+impl std::fmt::Display for MsgConversionError {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        match self {
+            MsgConversionError::InvalidFieldFormat => {
+                write!(f, "The field does not match the expected datatype.")
+            }
+            MsgConversionError::NotEnoughFields => {
+                write!(f, "Not enough fields in the PointCloud2 message.")
+            }
+            MsgConversionError::TooManyDimensions => {
+                write!(
+                    f,
+                    "The dimensionality of the point inside the message is too high."
+                )
+            }
+            MsgConversionError::UnsupportedFieldType(datatype) => {
+                write!(
+                    f,
+                    "The field datatype is not supported by the ROS message description: {}",
+                    datatype
+                )
+            }
+            MsgConversionError::NoPoints => {
+                write!(f, "There are no points in the point cloud.")
+            }
+            MsgConversionError::DataLengthMismatch => {
+                write!(f, "The length of the byte buffer in the message does not match the expected length computed from the fields.")
+            }
+            MsgConversionError::FieldNotFound(fields) => {
+                write!(f, "There are fields missing in the message: {:?}", fields)
+            }
+        }
+    }
+}
 
-    /// There are no points in the point cloud.
-    NoPoints,
+impl std::fmt::Debug for MsgConversionError {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        <MsgConversionError as std::fmt::Display>::fmt(self, f)
+    }
+}
 
-    /// The length of the byte buffer in the message does not match the expected length computed from the fields.
-    DataLengthMismatch,
+impl std::error::Error for MsgConversionError {}
 
-    /// There are fields missing in the message.
-    FieldNotFound(Vec<String>),
+#[cfg(feature = "derive")]
+fn system_endian() -> Endianness {
+    if cfg!(target_endian = "big") {
+        Endianness::Big
+    } else if cfg!(target_endian = "little") {
+        Endianness::Little
+    } else {
+        panic!("Unsupported endianness");
+    }
 }
 
 #[derive(Clone, Debug)]
@@ -125,7 +160,7 @@ impl Default for PointCloud2Msg {
 
 impl PointCloud2Msg {
     #[cfg(feature = "derive")]
-    fn prepare_direct_copy<const N: usize, C>() -> Result<Self, ConversionError>
+    fn prepare_direct_copy<const N: usize, C>() -> Result<Self, MsgConversionError>
     where
         C: PointConvertible<N>,
     {
@@ -167,7 +202,7 @@ impl PointCloud2Msg {
     }
 
     #[cfg(feature = "derive")]
-    fn assert_byte_similarity<const N: usize, C>(&self) -> Result<bool, ConversionError>
+    fn assert_byte_similarity<const N: usize, C>(&self) -> Result<bool, MsgConversionError>
     where
         C: PointConvertible<N>,
     {
@@ -187,19 +222,19 @@ impl PointCloud2Msg {
                     size,
                 } => {
                     if msg_f.name != name {
-                        return Err(ConversionError::FieldNotFound(vec![name.clone()]));
+                        return Err(MsgConversionError::FieldNotFound(vec![name.clone()]));
                     }
 
                     if msg_f.datatype != datatype {
-                        return Err(ConversionError::InvalidFieldFormat);
+                        return Err(MsgConversionError::InvalidFieldFormat);
                     }
 
                     if msg_f.offset != offset {
-                        return Err(ConversionError::DataLengthMismatch);
+                        return Err(MsgConversionError::DataLengthMismatch);
                     }
 
                     if msg_f.count != 1 {
-                        return Err(ConversionError::UnsupportedFieldType);
+                        return Err(MsgConversionError::DataLengthMismatch);
                     }
 
                     offset += size; // assume field_count 1
@@ -214,7 +249,7 @@ impl PointCloud2Msg {
     }
 
     #[inline(always)]
-    fn prepare<const N: usize, C>() -> Result<Self, ConversionError>
+    fn prepare<const N: usize, C>() -> Result<Self, MsgConversionError>
     where
         C: PointConvertible<N>,
     {
@@ -233,9 +268,7 @@ impl PointCloud2Msg {
             .zip(fields.iter_mut())
         {
             let datatype_code = meta_value.datatype.into();
-            if FieldDatatype::try_from(datatype_code).is_err() {
-                return Err(ConversionError::UnsupportedFieldType);
-            }
+            FieldDatatype::try_from(datatype_code)?;
 
             let field_count = 1;
 
@@ -271,11 +304,11 @@ impl PointCloud2Msg {
     ///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
     /// ];
     ///
-    // let msg_out = PointCloud2Msg::try_from_iterable(cloud_points).unwrap();
+    // let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
     /// ```
     pub fn try_from_iter<const N: usize, C>(
-        iterable: impl Iterator<Item = C>,
-    ) -> Result<Self, ConversionError>
+        iterable: impl IntoIterator<Item = C>,
+    ) -> Result<Self, MsgConversionError>
     where
         C: PointConvertible<N>,
     {
@@ -300,19 +333,11 @@ impl PointCloud2Msg {
     }
 
     #[cfg(feature = "derive")]
-    pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, ConversionError>
+    pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError>
     where
         C: PointConvertible<N>,
     {
-        let endianness = if cfg!(target_endian = "big") {
-            Endianness::Big
-        } else if cfg!(target_endian = "little") {
-            Endianness::Little
-        } else {
-            panic!("Unsupported endianness");
-        };
-
-        match endianness {
+        match system_endian() {
             Endianness::Big => Self::try_from_iter(vec.into_iter()),
             Endianness::Little => {
                 let mut cloud = Self::prepare_direct_copy::<N, C>()?;
@@ -337,21 +362,13 @@ impl PointCloud2Msg {
     }
 
     #[cfg(feature = "derive")]
-    pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, ConversionError>
+    pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError>
     where
         C: PointConvertible<N>,
     {
-        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::<N, C>()?;
 
-        match endianness {
+        match system_endian() {
             Endianness::Big => Ok(self.try_into_iter()?.collect()),
             Endianness::Little => {
                 let mut vec = Vec::with_capacity(self.width as usize);
@@ -370,7 +387,7 @@ impl PointCloud2Msg {
 
     pub fn try_into_iter<const N: usize, C>(
         self,
-    ) -> Result<impl Iterator<Item = C>, ConversionError>
+    ) -> Result<impl Iterator<Item = C>, MsgConversionError>
     where
         C: PointConvertible<N>,
     {
@@ -378,12 +395,11 @@ impl PointCloud2Msg {
     }
 
     #[cfg(feature = "rayon")]
-    pub fn try_into_par_iter<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>(
+    pub fn try_into_par_iter<const N: usize, C>(
         self,
-    ) -> Result<impl rayon::iter::ParallelIterator<Item = C>, ConversionError>
+    ) -> Result<impl rayon::iter::ParallelIterator<Item = C>, MsgConversionError>
     where
-        C: PointConvertible<T, SIZE, DIM, METADIM> + Send + Sync,
-        T: FromBytes + Send + Sync,
+        C: PointConvertible<N> + Send + Sync,
     {
         iterator::PointCloudIterator::try_from(self)
     }
@@ -450,10 +466,11 @@ pub trait PointConvertible<const N: usize>:
 
 #[cfg(feature = "derive")]
 pub trait PointConvertible<const N: usize>:
-    type_layout::TypeLayout + From<Point<N>> + Into<Point<N>> + Fields<N> + Clone + 'static + Default
+    type_layout::TypeLayout + From<Point<N>> + Into<Point<N>> + Fields<N> + 'static + Default
 {
 }
 
+#[cfg(feature = "derive")]
 enum PointField {
     Padding(u32),
     Field {
@@ -463,8 +480,14 @@ enum PointField {
     },
 }
 
+#[cfg(feature = "derive")]
+struct TypeLayoutInfo {
+    fields: Vec<PointField>,
+}
+
+#[cfg(feature = "derive")]
 impl TryFrom<type_layout::Field> for PointField {
-    type Error = ConversionError;
+    type Error = MsgConversionError;
 
     fn try_from(f: type_layout::Field) -> Result<Self, Self::Error> {
         match f {
@@ -482,12 +505,9 @@ impl TryFrom<type_layout::Field> for PointField {
     }
 }
 
-struct TypeLayoutInfo {
-    fields: Vec<PointField>,
-}
-
+#[cfg(feature = "derive")]
 impl TryFrom<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
-    type Error = ConversionError;
+    type Error = MsgConversionError;
 
     fn try_from(t: type_layout::TypeLayoutInfo) -> Result<Self, Self::Error> {
         let fields: Vec<PointField> = t
diff --git a/src/pcl_utils.rs b/src/points.rs
similarity index 56%
rename from src/pcl_utils.rs
rename to src/points.rs
index 1a13c46..cba5cd3 100644
--- a/src/pcl_utils.rs
+++ b/src/points.rs
@@ -1,23 +1,101 @@
-use crate::{ConversionError, Fields, Point, PointConvertible};
+use crate::{Fields, Point, PointConvertible};
 
 #[cfg(feature = "derive")]
-use crate::TypeLayout;
+use type_layout::TypeLayout;
 
-/// Pack an RGB color into a single f32 value as used in ROS with PCL for RViz usage.
-#[inline]
-fn pack_rgb(r: u8, g: u8, b: u8) -> f32 {
-    let packed = ((r as u32) << 16) + ((g as u32) << 8) + (b as u32);
-    f32::from_bits(packed)
+/// A packed RGB color encoding as used in ROS tools.
+#[derive(Clone, Copy)]
+#[repr(C)]
+pub union RGB {
+    packed: f32,
+    unpacked: [u8; 4], // 1 byte padding
+}
+
+unsafe impl Send for RGB {}
+unsafe impl Sync for RGB {}
+
+impl Default for RGB {
+    fn default() -> Self {
+        Self { packed: 0.0 }
+    }
+}
+
+impl PartialEq for RGB {
+    fn eq(&self, other: &Self) -> bool {
+        self.r() == other.r() && self.g() == other.g() && self.b() == other.b()
+    }
+}
+
+impl core::fmt::Display for RGB {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        write!(f, "#{:02X}{:02X}{:02X}", self.r(), self.g(), self.b())
+    }
+}
+
+impl core::fmt::Debug for RGB {
+    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+        f.debug_struct("RGB")
+            .field("r", &self.r())
+            .field("g", &self.g())
+            .field("b", &self.b())
+            .finish()
+    }
+}
+
+impl RGB {
+    pub fn new(r: u8, g: u8, b: u8) -> Self {
+        Self {
+            unpacked: [r, g, b, 0],
+        }
+    }
+
+    pub fn new_from_packed_f32(packed: f32) -> Self {
+        Self { packed }
+    }
+
+    pub fn new_from_packed(packed: u32) -> Self {
+        Self::new_from_packed_f32(f32::from_bits(packed))
+    }
+
+    pub fn raw(&self) -> f32 {
+        unsafe { self.packed }
+    }
+
+    pub fn r(&self) -> u8 {
+        unsafe { self.unpacked[0] }
+    }
+
+    pub fn g(&self) -> u8 {
+        unsafe { self.unpacked[1] }
+    }
+
+    pub fn b(&self) -> u8 {
+        unsafe { self.unpacked[2] }
+    }
+
+    pub fn set_r(&mut self, r: u8) {
+        unsafe { self.unpacked[0] = r }
+    }
+
+    pub fn set_g(&mut self, g: u8) {
+        unsafe { self.unpacked[1] = g }
+    }
+
+    pub fn set_b(&mut self, b: u8) {
+        unsafe { self.unpacked[2] = b }
+    }
 }
 
-/// Unpack an RGB color from a single f32 value as used in ROS with PCL for RViz usage.
-#[inline]
-fn unpack_rgb(rgb: f32) -> [u8; 3] {
-    let packed: u32 = rgb.to_bits();
-    let r: u8 = (packed >> 16) as u8;
-    let g: u8 = (packed >> 8) as u8;
-    let b: u8 = packed as u8;
-    [r, g, b]
+impl From<RGB> for f32 {
+    fn from(rgb: RGB) -> Self {
+        unsafe { rgb.packed }
+    }
+}
+
+impl From<f32> for RGB {
+    fn from(packed: f32) -> Self {
+        RGB::new_from_packed_f32(packed)
+    }
 }
 
 /// Predefined point type commonly used in ROS with PCL.
@@ -31,6 +109,38 @@ pub struct PointXYZ {
     pub z: f32,
 }
 
+#[cfg(feature = "nalgebra")]
+impl From<nalgebra::Point3<f32>> for PointXYZ {
+    fn from(point: nalgebra::Point3<f32>) -> Self {
+        Self {
+            x: point.x,
+            y: point.y,
+            z: point.z,
+        }
+    }
+}
+
+#[cfg(feature = "nalgebra")]
+impl From<PointXYZ> for nalgebra::Point3<f32> {
+    fn from(point: PointXYZ) -> Self {
+        nalgebra::Point3::new(point.x, point.y, point.z)
+    }
+}
+
+impl PointXYZ {
+    pub fn new(x: f32, y: f32, z: f32) -> Self {
+        Self { x, y, z }
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
@@ -69,6 +179,20 @@ pub struct PointXYZI {
     pub intensity: f32,
 }
 
+impl PointXYZI {
+    pub fn new(x: f32, y: f32, z: f32, intensity: f32) -> Self {
+        Self { x, y, z, intensity }
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
@@ -113,6 +237,25 @@ pub struct PointXYZL {
     pub label: u32,
 }
 
+impl PointXYZL {
+    pub fn new(x: f32, y: f32, z: f32, intensity: f32) -> Self {
+        Self {
+            x,
+            y,
+            z,
+            label: intensity as u32,
+        }
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
@@ -154,11 +297,36 @@ pub struct PointXYZRGB {
     pub x: f32,
     pub y: f32,
     pub z: f32,
-    pub r: u8,
-    pub g: u8,
-    pub b: u8,
+    pub rgb: RGB,
 }
 
+impl PointXYZRGB {
+    pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8) -> Self {
+        let rgb = RGB::new(r, g, b);
+        Self { x, y, z, rgb }
+    }
+
+    pub fn r(&self) -> u8 {
+        self.rgb.r()
+    }
+
+    pub fn g(&self) -> u8 {
+        self.rgb.g()
+    }
+
+    pub fn b(&self) -> u8 {
+        self.rgb.b()
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
@@ -167,24 +335,24 @@ impl Fields<4> for PointXYZRGB {
 
 impl From<Point<4>> for PointXYZRGB {
     fn from(point: Point<4>) -> Self {
-        let rgb = point.fields[3].get::<f32>();
-        let rgb_unpacked = unpack_rgb(rgb);
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
             z: point.fields[2].get(),
-            r: rgb_unpacked[0],
-            g: rgb_unpacked[1],
-            b: rgb_unpacked[2],
+            rgb: point.fields[3].get(),
         }
     }
 }
 
 impl From<PointXYZRGB> for Point<4> {
     fn from(point: PointXYZRGB) -> Self {
-        let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
-            fields: [point.x.into(), point.y.into(), point.z.into(), rgb.into()],
+            fields: [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                f32::from(point.rgb).into(),
+            ],
         }
     }
 }
@@ -201,12 +369,37 @@ pub struct PointXYZRGBA {
     pub x: f32,
     pub y: f32,
     pub z: f32,
-    pub r: u8,
-    pub g: u8,
-    pub b: u8,
+    pub rgb: RGB,
     pub a: u8,
 }
 
+impl PointXYZRGBA {
+    pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, a: u8) -> Self {
+        let rgb = RGB::new(r, g, b);
+        Self { x, y, z, rgb, a }
+    }
+
+    pub fn r(&self) -> u8 {
+        self.rgb.r()
+    }
+
+    pub fn g(&self) -> u8 {
+        self.rgb.g()
+    }
+
+    pub fn b(&self) -> u8 {
+        self.rgb.b()
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
@@ -215,15 +408,11 @@ impl Fields<5> for PointXYZRGBA {
 
 impl From<Point<5>> for PointXYZRGBA {
     fn from(point: Point<5>) -> Self {
-        let rgb = point.fields[3].get::<f32>();
-        let rgb_unpacked = unpack_rgb(rgb);
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
             z: point.fields[2].get(),
-            r: rgb_unpacked[0],
-            g: rgb_unpacked[1],
-            b: rgb_unpacked[2],
+            rgb: point.fields[3].get::<f32>().into(),
             a: point.fields[4].get(),
         }
     }
@@ -231,13 +420,12 @@ impl From<Point<5>> for PointXYZRGBA {
 
 impl From<PointXYZRGBA> for Point<5> {
     fn from(point: PointXYZRGBA) -> Self {
-        let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
             fields: [
                 point.x.into(),
                 point.y.into(),
                 point.z.into(),
-                rgb.into(),
+                f32::from(point.rgb).into(),
                 point.a.into(),
             ],
         }
@@ -255,14 +443,57 @@ pub struct PointXYZRGBNormal {
     pub x: f32,
     pub y: f32,
     pub z: f32,
-    pub r: u8,
-    pub g: u8,
-    pub b: u8,
+    pub rgb: RGB,
     pub normal_x: f32,
     pub normal_y: f32,
     pub normal_z: f32,
 }
 
+impl PointXYZRGBNormal {
+    pub fn new(
+        x: f32,
+        y: f32,
+        z: f32,
+        r: u8,
+        g: u8,
+        b: u8,
+        normal_x: f32,
+        normal_y: f32,
+        normal_z: f32,
+    ) -> Self {
+        let rgb = RGB::new(r, g, b);
+        Self {
+            x,
+            y,
+            z,
+            rgb,
+            normal_x,
+            normal_y,
+            normal_z,
+        }
+    }
+
+    pub fn r(&self) -> u8 {
+        self.rgb.r()
+    }
+
+    pub fn g(&self) -> u8 {
+        self.rgb.g()
+    }
+
+    pub fn b(&self) -> u8 {
+        self.rgb.b()
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
@@ -271,15 +502,11 @@ impl Fields<7> for PointXYZRGBNormal {
 
 impl From<Point<7>> for PointXYZRGBNormal {
     fn from(point: Point<7>) -> Self {
-        let rgb = point.fields[3].get::<f32>();
-        let rgb_unpacked = unpack_rgb(rgb);
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
             z: point.fields[2].get(),
-            r: rgb_unpacked[0],
-            g: rgb_unpacked[1],
-            b: rgb_unpacked[2],
+            rgb: point.fields[3].get::<f32>().into(),
             normal_x: point.fields[4].get(),
             normal_y: point.fields[5].get(),
             normal_z: point.fields[6].get(),
@@ -289,13 +516,12 @@ impl From<Point<7>> for PointXYZRGBNormal {
 
 impl From<PointXYZRGBNormal> for Point<7> {
     fn from(point: PointXYZRGBNormal) -> Self {
-        let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
             fields: [
                 point.x.into(),
                 point.y.into(),
                 point.z.into(),
-                rgb.into(),
+                f32::from(point.rgb).into(),
                 point.normal_x.into(),
                 point.normal_y.into(),
                 point.normal_z.into(),
@@ -321,6 +547,36 @@ pub struct PointXYZINormal {
     pub normal_z: f32,
 }
 
+impl PointXYZINormal {
+    pub fn new(
+        x: f32,
+        y: f32,
+        z: f32,
+        intensity: f32,
+        normal_x: f32,
+        normal_y: f32,
+        normal_z: f32,
+    ) -> Self {
+        Self {
+            x,
+            y,
+            z,
+            intensity,
+            normal_x,
+            normal_y,
+            normal_z,
+        }
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
@@ -368,12 +624,43 @@ pub struct PointXYZRGBL {
     pub x: f32,
     pub y: f32,
     pub z: f32,
-    pub r: u8,
-    pub g: u8,
-    pub b: u8,
+    pub rgb: RGB,
     pub label: u32,
 }
 
+unsafe impl Send for PointXYZRGBL {}
+unsafe impl Sync for PointXYZRGBL {}
+
+impl PointXYZRGBL {
+    pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, label: u32) -> Self {
+        let rgb = RGB::new(r, g, b);
+        Self {
+            x,
+            y,
+            z,
+            rgb,
+            label,
+        }
+    }
+
+    pub fn r(&self) -> u8 {
+        self.rgb.r()
+    }
+
+    pub fn g(&self) -> u8 {
+        self.rgb.g()
+    }
+
+    pub fn b(&self) -> u8 {
+        self.rgb.b()
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
 impl Fields<5> for PointXYZRGBL {
     fn field_names_ordered() -> [&'static str; 5] {
         ["x", "y", "z", "rgb", "label"]
@@ -382,15 +669,11 @@ impl Fields<5> for PointXYZRGBL {
 
 impl From<Point<5>> for PointXYZRGBL {
     fn from(point: Point<5>) -> Self {
-        let rgb = point.fields[3].get::<f32>();
-        let rgb_unpacked = unpack_rgb(rgb);
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
             z: point.fields[2].get(),
-            r: rgb_unpacked[0],
-            g: rgb_unpacked[1],
-            b: rgb_unpacked[2],
+            rgb: point.fields[3].get::<f32>().into(),
             label: point.fields[4].get(),
         }
     }
@@ -398,13 +681,12 @@ impl From<Point<5>> for PointXYZRGBL {
 
 impl From<PointXYZRGBL> for Point<5> {
     fn from(point: PointXYZRGBL) -> Self {
-        let rgb = pack_rgb(point.r, point.g, point.b);
         Point {
             fields: [
                 point.x.into(),
                 point.y.into(),
                 point.z.into(),
-                rgb.into(),
+                f32::from(point.rgb).into(),
                 point.label.into(),
             ],
         }
@@ -427,6 +709,27 @@ pub struct PointXYZNormal {
     pub normal_z: f32,
 }
 
+impl PointXYZNormal {
+    pub fn new(x: f32, y: f32, z: f32, normal_x: f32, normal_y: f32, normal_z: f32) -> Self {
+        Self {
+            x,
+            y,
+            z,
+            normal_x,
+            normal_y,
+            normal_z,
+        }
+    }
+
+    #[cfg(feature = "nalgebra")]
+    pub fn xyz(&self) -> nalgebra::Point3<f32> {
+        nalgebra::Point3::new(self.x, self.y, self.z)
+    }
+}
+
+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"]
diff --git a/src/prelude.rs b/src/prelude.rs
new file mode 100644
index 0000000..cce24f7
--- /dev/null
+++ b/src/prelude.rs
@@ -0,0 +1,15 @@
+/// Re-export commonly used types and traits.
+pub use crate::points::*;
+
+#[cfg(feature = "rayon")]
+pub use rayon::prelude::*;
+
+pub use crate::{Fields, MsgConversionError, Point, PointCloud2Msg, PointConvertible};
+
+#[cfg(feature = "derive")]
+pub use type_layout::TypeLayout;
+
+#[cfg(feature = "derive")]
+pub use rpcl2_derive::*;
+
+pub use crate::convert::{FieldDatatype, FromBytes, GetFieldDatatype};
diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs
index c325bed..44718da 100644
--- a/tests/e2e_test.rs
+++ b/tests/e2e_test.rs
@@ -1,6 +1,6 @@
-use ros_pointcloud2::pcl_utils::*;
-use ros_pointcloud2::PointCloud2Msg;
-use ros_pointcloud2::*;
+use ros_pointcloud2::prelude::*;
+
+#[cfg(feature = "derive")]
 use std::fmt::Debug;
 
 macro_rules! convert_from_into {
@@ -19,7 +19,7 @@ macro_rules! convert_from_into_vec {
 macro_rules! convert_from_into_in_out_cloud {
     ($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
         let msg = PointCloud2Msg::try_from_iter($in_cloud.clone().into_iter());
-        assert!(msg.is_ok());
+        assert!(msg.is_ok(), "{:?}", msg);
         let msg = msg.unwrap();
         let to_p_type = msg.try_into_iter();
         assert!(to_p_type.is_ok());
@@ -34,7 +34,7 @@ macro_rules! convert_from_into_in_out_cloud {
 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());
-        assert!(msg.is_ok());
+        assert!(msg.is_ok(), "{:?}", msg);
         let msg = msg.unwrap();
         let to_p_type = msg.try_into_iter();
         assert!(to_p_type.is_ok());
@@ -48,26 +48,10 @@ macro_rules! convert_from_into_in_out_cloud_vec {
 #[test]
 fn write_cloud() {
     let cloud = vec![
-        PointXYZ {
-            x: 0.0,
-            y: 1.0,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.0,
-            y: 1.5,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.3,
-            y: 1.6,
-            z: 5.7,
-        },
-        PointXYZ {
-            x: f32::MAX,
-            y: f32::MIN,
-            z: f32::MAX,
-        },
+        PointXYZ::new(0.0, 1.0, 5.0),
+        PointXYZ::new(1.0, 1.5, 5.0),
+        PointXYZ::new(1.3, 1.6, 5.7),
+        PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
     ];
 
     let msg = PointCloud2Msg::try_from_iter(cloud.into_iter());
@@ -78,26 +62,10 @@ fn write_cloud() {
 #[cfg(feature = "derive")]
 fn write_cloud_from_vec() {
     let cloud = vec![
-        PointXYZ {
-            x: 0.0,
-            y: 1.0,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.0,
-            y: 1.5,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.3,
-            y: 1.6,
-            z: 5.7,
-        },
-        PointXYZ {
-            x: f32::MAX,
-            y: f32::MIN,
-            z: f32::MAX,
-        },
+        PointXYZ::new(0.0, 1.0, 5.0),
+        PointXYZ::new(1.0, 1.5, 5.0),
+        PointXYZ::new(1.3, 1.6, 5.7),
+        PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
     ];
 
     let msg = PointCloud2Msg::try_from_vec(cloud);
@@ -241,26 +209,10 @@ fn custom_rgba_f32() {
 #[test]
 fn converterxyz() {
     let cloud = vec![
-        PointXYZ {
-            x: 0.0,
-            y: 1.0,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.0,
-            y: 1.5,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.3,
-            y: 1.6,
-            z: 5.7,
-        },
-        PointXYZ {
-            x: f32::MAX,
-            y: f32::MIN,
-            z: f32::MAX,
-        },
+        PointXYZ::new(0.0, 1.0, 5.0),
+        PointXYZ::new(1.0, 1.5, 5.0),
+        PointXYZ::new(1.3, 1.6, 5.7),
+        PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
     ];
 
     convert_from_into!(PointXYZ, cloud);
@@ -271,42 +223,18 @@ fn converterxyzrgba() {
     convert_from_into!(
         PointXYZRGBA,
         vec![
-            PointXYZRGBA {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                r: 0,
-                g: 0,
-                b: 0,
-                a: 0,
-            },
-            PointXYZRGBA {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                r: 1,
-                g: 1,
-                b: 1,
-                a: 1,
-            },
-            PointXYZRGBA {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                r: 2,
-                g: 2,
-                b: 2,
-                a: 2,
-            },
-            PointXYZRGBA {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                r: u8::MAX,
-                g: u8::MAX,
-                b: u8::MAX,
-                a: u8::MAX,
-            },
+            PointXYZRGBA::new(0.0, 1.0, 5.0, 0, 0, 0, 0),
+            PointXYZRGBA::new(1.0, 1.5, 5.0, 1, 1, 1, 1),
+            PointXYZRGBA::new(1.3, 1.6, 5.7, 2, 2, 2, 2),
+            PointXYZRGBA::new(
+                f32::MAX,
+                f32::MIN,
+                f32::MAX,
+                u8::MAX,
+                u8::MAX,
+                u8::MAX,
+                u8::MAX
+            ),
         ]
     );
 }
@@ -316,42 +244,9 @@ fn converterxyzinormal() {
     convert_from_into!(
         PointXYZINormal,
         vec![
-            PointXYZINormal {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                intensity: 0.0,
-                normal_x: 0.0,
-                normal_y: 0.0,
-                normal_z: 0.0,
-            },
-            PointXYZINormal {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                intensity: 1.0,
-                normal_x: 1.0,
-                normal_y: 1.0,
-                normal_z: 1.0,
-            },
-            PointXYZINormal {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                intensity: 2.0,
-                normal_x: 2.0,
-                normal_y: 2.0,
-                normal_z: 2.0,
-            },
-            PointXYZINormal {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                intensity: f32::MAX,
-                normal_x: f32::MAX,
-                normal_y: f32::MAX,
-                normal_z: f32::MAX,
-            },
+            PointXYZINormal::new(0.0, 1.0, 5.0, 0.0, 0.0, 0.0, 0.0),
+            PointXYZINormal::new(1.0, 1.5, 5.0, 1.0, 1.0, 1.0, 1.0),
+            PointXYZINormal::new(1.3, 1.6, 5.7, 2.0, 2.0, 2.0, 2.0),
         ]
     );
 }
@@ -361,50 +256,20 @@ fn converterxyzrgbnormal() {
     convert_from_into!(
         PointXYZRGBNormal,
         vec![
-            PointXYZRGBNormal {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                r: 0,
-                g: 0,
-                b: 0,
-                normal_x: 0.0,
-                normal_y: 0.0,
-                normal_z: 0.0,
-            },
-            PointXYZRGBNormal {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                r: 1,
-                g: 1,
-                b: 1,
-                normal_x: 1.0,
-                normal_y: 1.0,
-                normal_z: 1.0,
-            },
-            PointXYZRGBNormal {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                r: 2,
-                g: 2,
-                b: 2,
-                normal_x: 2.0,
-                normal_y: 2.0,
-                normal_z: 2.0,
-            },
-            PointXYZRGBNormal {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                r: u8::MAX,
-                g: u8::MAX,
-                b: u8::MAX,
-                normal_x: f32::MAX,
-                normal_y: f32::MAX,
-                normal_z: f32::MAX,
-            },
+            PointXYZRGBNormal::new(0.0, 1.0, 5.0, 0, 0, 0, 0.0, 0.0, 0.0),
+            PointXYZRGBNormal::new(1.0, 1.5, 5.0, 1, 1, 1, 1.0, 1.0, 1.0),
+            PointXYZRGBNormal::new(1.3, 1.6, 5.7, 2, 2, 2, 2.0, 2.0, 2.0),
+            PointXYZRGBNormal::new(
+                f32::MAX,
+                f32::MIN,
+                f32::MAX,
+                u8::MAX,
+                u8::MAX,
+                u8::MAX,
+                f32::MAX,
+                f32::MAX,
+                f32::MAX,
+            ),
         ]
     );
 }
@@ -414,38 +279,10 @@ fn converterxyznormal() {
     convert_from_into!(
         PointXYZNormal,
         vec![
-            PointXYZNormal {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                normal_x: 0.0,
-                normal_y: 0.0,
-                normal_z: 0.0,
-            },
-            PointXYZNormal {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                normal_x: 1.0,
-                normal_y: 1.0,
-                normal_z: 1.0,
-            },
-            PointXYZNormal {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                normal_x: 2.0,
-                normal_y: 2.0,
-                normal_z: 2.0,
-            },
-            PointXYZNormal {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                normal_x: f32::MAX,
-                normal_y: f32::MAX,
-                normal_z: f32::MAX,
-            },
+            PointXYZNormal::new(0.0, 1.0, 5.0, 0.0, 0.0, 0.0),
+            PointXYZNormal::new(1.0, 1.5, 5.0, 1.0, 1.0, 1.0),
+            PointXYZNormal::new(1.3, 1.6, 5.7, 2.0, 2.0, 2.0),
+            PointXYZNormal::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX, f32::MAX, f32::MAX),
         ]
     );
 }
@@ -455,42 +292,18 @@ fn converterxyzrgbl() {
     convert_from_into!(
         PointXYZRGBL,
         vec![
-            PointXYZRGBL {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                r: 0,
-                g: 0,
-                b: 0,
-                label: 0,
-            },
-            PointXYZRGBL {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                r: 1,
-                g: 1,
-                b: 1,
-                label: 1,
-            },
-            PointXYZRGBL {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                r: 2,
-                g: 2,
-                b: 2,
-                label: 2,
-            },
-            PointXYZRGBL {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                r: u8::MAX,
-                g: u8::MAX,
-                b: u8::MAX,
-                label: u32::MAX,
-            },
+            PointXYZRGBL::new(0.0, 1.0, 5.0, 0, 0, 0, 0),
+            PointXYZRGBL::new(1.0, 1.5, 5.0, 1, 1, 1, 1),
+            PointXYZRGBL::new(1.3, 1.6, 5.7, 2, 2, 2, 2),
+            PointXYZRGBL::new(
+                f32::MAX,
+                f32::MIN,
+                f32::MAX,
+                u8::MAX,
+                u8::MAX,
+                u8::MAX,
+                u32::MAX
+            ),
         ]
     );
 }
@@ -500,38 +313,10 @@ fn converterxyzrgb() {
     convert_from_into!(
         PointXYZRGB,
         vec![
-            PointXYZRGB {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                r: 0,
-                g: 0,
-                b: 0,
-            },
-            PointXYZRGB {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                r: 1,
-                g: 1,
-                b: 1,
-            },
-            PointXYZRGB {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                r: 2,
-                g: 2,
-                b: 2,
-            },
-            PointXYZRGB {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                r: u8::MAX,
-                g: u8::MAX,
-                b: u8::MAX,
-            },
+            PointXYZRGB::new(0.0, 1.0, 5.0, 0, 0, 0),
+            PointXYZRGB::new(1.0, 1.5, 5.0, 1, 1, 1),
+            PointXYZRGB::new(1.3, 1.6, 5.7, 2, 2, 2),
+            PointXYZRGB::new(f32::MAX, f32::MIN, f32::MAX, u8::MAX, u8::MAX, u8::MAX),
         ]
     );
 }
@@ -542,38 +327,9 @@ fn converterxyzrgb_from_vec() {
     convert_from_into_vec!(
         PointXYZRGB,
         vec![
-            PointXYZRGB {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                r: 0,
-                g: 0,
-                b: 0,
-            },
-            PointXYZRGB {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                r: 1,
-                g: 1,
-                b: 1,
-            },
-            PointXYZRGB {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                r: 2,
-                g: 2,
-                b: 2,
-            },
-            PointXYZRGB {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                r: u8::MAX,
-                g: u8::MAX,
-                b: u8::MAX,
-            },
+            PointXYZRGB::new(0.0, 1.0, 5.0, 0, 0, 0),
+            PointXYZRGB::new(1.3, 1.6, 5.7, 2, 2, 2),
+            PointXYZRGB::new(f32::MAX, f32::MIN, f32::MAX, u8::MAX, u8::MAX, u8::MAX),
         ]
     );
 }
@@ -583,30 +339,10 @@ fn converterxyzl() {
     convert_from_into!(
         PointXYZL,
         vec![
-            PointXYZL {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                label: 0,
-            },
-            PointXYZL {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                label: 1,
-            },
-            PointXYZL {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                label: 2,
-            },
-            PointXYZL {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                label: u32::MAX,
-            },
+            PointXYZL::new(0.0, 1.0, 5.0, 0.0),
+            PointXYZL::new(1.0, 1.5, 5.0, 1.0),
+            PointXYZL::new(1.3, 1.6, 5.7, 2.0),
+            PointXYZL::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
         ]
     );
 }
@@ -616,30 +352,10 @@ fn converterxyzi() {
     convert_from_into!(
         PointXYZI,
         vec![
-            PointXYZI {
-                x: 0.0,
-                y: 1.0,
-                z: 5.0,
-                intensity: 0.0,
-            },
-            PointXYZI {
-                x: 1.0,
-                y: 1.5,
-                z: 5.0,
-                intensity: 1.0,
-            },
-            PointXYZI {
-                x: 1.3,
-                y: 1.6,
-                z: 5.7,
-                intensity: 2.0,
-            },
-            PointXYZI {
-                x: f32::MAX,
-                y: f32::MIN,
-                z: f32::MAX,
-                intensity: f32::MAX,
-            },
+            PointXYZI::new(0.0, 1.0, 5.0, 0.0),
+            PointXYZI::new(1.0, 1.5, 5.0, 1.0),
+            PointXYZI::new(1.3, 1.6, 5.7, 2.0),
+            PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
         ]
     );
 }
@@ -647,58 +363,23 @@ fn converterxyzi() {
 #[test]
 fn write_xyzi_read_xyz() {
     let write_cloud = vec![
-        PointXYZI {
-            x: 0.0,
-            y: 1.0,
-            z: 5.0,
-            intensity: 0.0,
-        },
-        PointXYZI {
-            x: 1.0,
-            y: 1.5,
-            z: 5.0,
-            intensity: 1.0,
-        },
-        PointXYZI {
-            x: 1.3,
-            y: 1.6,
-            z: 5.7,
-            intensity: 2.0,
-        },
-        PointXYZI {
-            x: f32::MAX,
-            y: f32::MIN,
-            z: f32::MAX,
-            intensity: f32::MAX,
-        },
+        PointXYZI::new(0.0, 1.0, 5.0, 0.0),
+        PointXYZI::new(1.0, 1.5, 5.0, 1.0),
+        PointXYZI::new(1.3, 1.6, 5.7, 2.0),
+        PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
     ];
 
     let read_cloud = [
-        PointXYZ {
-            x: 0.0,
-            y: 1.0,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.0,
-            y: 1.5,
-            z: 5.0,
-        },
-        PointXYZ {
-            x: 1.3,
-            y: 1.6,
-            z: 5.7,
-        },
-        PointXYZ {
-            x: f32::MAX,
-            y: f32::MIN,
-            z: f32::MAX,
-        },
+        PointXYZ::new(0.0, 1.0, 5.0),
+        PointXYZ::new(1.0, 1.5, 5.0),
+        PointXYZ::new(1.3, 1.6, 5.7),
+        PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
     ];
 
     convert_from_into_in_out_cloud!(write_cloud, PointXYZI, read_cloud, PointXYZ);
 }
 
+#[cfg(feature = "derive")]
 #[test]
 fn write_less_than_available() {
     #[derive(Debug, PartialEq, Clone, Default, TypeLayout)]
diff --git a/tests/r2r_msg_test.rs b/tests/r2r_msg_test.rs
index 912a4ba..78b924a 100644
--- a/tests/r2r_msg_test.rs
+++ b/tests/r2r_msg_test.rs
@@ -1,7 +1,7 @@
 #[cfg(feature = "r2r_msg")]
 #[test]
 fn convertxyz_r2r_msg() {
-    use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
+    use ros_pointcloud2::{points::PointXYZ, PointCloud2Msg};
 
     use r2r::sensor_msgs::msg::PointCloud2;
 
@@ -23,7 +23,7 @@ fn convertxyz_r2r_msg() {
         },
     ];
     let copy = cloud.clone();
-    let internal_cloud = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let internal_cloud = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let r2r_msg_cloud: PointCloud2 = internal_cloud.into();
     let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into();
     let to_convert = convert_back_internal.try_into_iter().unwrap();
diff --git a/tests/rosrust_msg_test.rs b/tests/rosrust_msg_test.rs
index 30e0bae..2e7c795 100644
--- a/tests/rosrust_msg_test.rs
+++ b/tests/rosrust_msg_test.rs
@@ -1,7 +1,7 @@
 #[cfg(feature = "rosrust_msg")]
 #[test]
 fn convertxyz_rosrust_msg() {
-    use ros_pointcloud2::{pcl_utils::PointXYZ, PointCloud2Msg};
+    use ros_pointcloud2::{points::PointXYZ, PointCloud2Msg};
 
     let cloud = vec![
         PointXYZ {
@@ -21,7 +21,7 @@ fn convertxyz_rosrust_msg() {
         },
     ];
     let copy = cloud.clone();
-    let internal_cloud = PointCloud2Msg::try_from_iterable(cloud).unwrap();
+    let internal_cloud = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into();
     let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into();
     let to_convert = convert_back_internal.try_into_iter().unwrap();

From aa3ca8be8faca034c48b9c35196443392e28e172 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 7 May 2024 22:52:40 +0200
Subject: [PATCH 08/10] tidy

---
 README.md                            |  28 ++-
 benches/roundtrip.rs                 |  49 ++++-
 examples/custom_enum_field_filter.rs |   8 +-
 examples/rayon_bench.rs              |  10 +-
 examples/simple_distance_filter.rs   |   2 +-
 rpcl2_derive/src/lib.rs              |   6 +-
 src/convert.rs                       |  10 +-
 src/iterator.rs                      |   6 +-
 src/lib.rs                           |  73 ++++---
 src/points.rs                        | 282 +++++++++++++--------------
 src/prelude.rs                       |   2 +-
 tests/e2e_test.rs                    |  26 ++-
 12 files changed, 264 insertions(+), 238 deletions(-)

diff --git a/README.md b/README.md
index ef290db..4b1a85f 100644
--- a/README.md
+++ b/README.md
@@ -1,33 +1,29 @@
+## !! Note !!
+This library is currently in development for v1.0.0, for the documentation of v0.4.0 on crates.io, visit the [docs](https://docs.rs/ros_pointcloud2/0.4.0/ros_pointcloud2/).
+
 <p align="center">
   <h3 align="center">ROS PointCloud2</h3>
-  <p align="center">Customizable conversions to and from the PointCloud2 ROS message.</p>
+  <p align="center">A complete and versatile abstraction of PointCloud2.</p>
   <p align="center"><a href="https://crates.io/crates/ros_pointcloud2"><img src="https://img.shields.io/crates/v/ros_pointcloud2.svg" alt=""></a> <a href="https://github.com/stelzo/ros_pointcloud2/tree/main/tests"><img src="https://github.com/stelzo/ros_pointcloud2/actions/workflows/tests.yml/badge.svg" alt=""></a>
   </p>
 </p>
 
-A complete implementation of `sensor_msgs/PointCloud2` conversions with focus on ease of use and maximum throughput.
-
-Providing an easy to use, generics defined, point-wise iterator abstraction over the byte buffer in `PointCloud2` to minimize iterations in your processing pipeline.
-
-To keep the crate a general purpose library for the problem, it uses its own type for the message `PointCloud2Msg`. ROS1 and ROS2 support is added with feature flags.
-
-## Performance
-
-The library compares the similarity of the ROS message point type with your given Point, allowing it to select the fastest strategy.
+To keep the crate a general purpose library for the problem, it uses its own type for the message `PointCloud2Msg`.
+ROS1 and ROS2 is supported with feature flags.
 
-Since PCL is the most used library for this work in C++, the predefined types in ros_pointcloud2 use C padding to be exact copies for maximum performance between C++ and Rust nodes at the cost of a slightly larger message sizes.
+Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/1.0.0/ros_pointcloud2/) for a complete guide.
 
-Since ros_pointcloud2 optimizes the performance dynamically, it typically outperforms PCL conversions in all tasks.
+## Quickstart
 
-## Quickstart (Iterator)
+The following example uses the iterator APIs. For memory bound pipelines, you may also try the `try_from_vec` or `try_into_vec` functions by enabling the `derive` feature.
 
 ```rust
 use ros_pointcloud2::prelude::*;
 
-// Your points (here using a predefined type PointXYZ).
+// PointXYZ (and many others) are provided by the crate.
 let cloud_points = vec![
-  PointXYZ {x: 91.486, y: -4.1, z: 42.0001,},
-  PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,},
+  PointXYZ::new(91.486, -4.1, 42.0001),
+  PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
 ];
 
 // Give the Vec or anything that implements `IntoIterator`.
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
index 9c60355..c0bb7ea 100644
--- a/benches/roundtrip.rs
+++ b/benches/roundtrip.rs
@@ -1,5 +1,3 @@
-#![cfg(feature = "derive")]
-
 use criterion::{black_box, criterion_group, criterion_main, Criterion};
 use ros_pointcloud2::prelude::*;
 
@@ -19,7 +17,8 @@ pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<
     pointcloud
 }
 
-fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
+#[cfg(feature = "derive")]
+fn roundtrip_vec(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
     let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
     let total = internal_msg
@@ -29,7 +28,18 @@ fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
     orig_len == total.len()
 }
 
-fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
+fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+    let total = internal_msg
+        .try_into_iter()
+        .unwrap()
+        .collect::<Vec<PointXYZ>>();
+    orig_len == total.len()
+}
+
+#[cfg(feature = "derive")]
+fn roundtrip_filter_vec(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
     let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
     let total = internal_msg
@@ -46,6 +56,23 @@ fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
     orig_len == total.x as usize
 }
 
+fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
+    let orig_len = cloud.len();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+    let total = internal_msg
+        .try_into_iter()
+        .unwrap()
+        .filter(|point: &PointXYZ| {
+            (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+        })
+        .fold(PointXYZ::default(), |acc, point| PointXYZ {
+            x: acc.x + point.x,
+            y: acc.y + point.y,
+            z: acc.z + point.z,
+        });
+    orig_len == total.x as usize
+}
+
 #[cfg(feature = "rayon")]
 fn roundtrip_par(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
@@ -95,6 +122,13 @@ fn roundtrip_benchmark(c: &mut Criterion) {
         })
     });
 
+    #[cfg(feature = "derive")]
+    c.bench_function("roundtrip_vec 500k", |b| {
+        b.iter(|| {
+            black_box(roundtrip_vec(cloud_points_500k.clone()));
+        })
+    });
+
     c.bench_function("roundtrip_filter 500k", |b| {
         b.iter(|| {
             roundtrip_filter(black_box(cloud_points_500k.clone()));
@@ -120,6 +154,13 @@ fn roundtrip_benchmark(c: &mut Criterion) {
             black_box(roundtrip_filter_par(cloud_points_1_5m.clone()));
         })
     });
+
+    #[cfg(feature = "derive")]
+    c.bench_function("roundtrip_filter_vec 1.5m", |b| {
+        b.iter(|| {
+            roundtrip_filter_vec(black_box(cloud_points_1_5m.clone()));
+        })
+    });
 }
 
 criterion_group!(benches, roundtrip_benchmark);
diff --git a/examples/custom_enum_field_filter.rs b/examples/custom_enum_field_filter.rs
index 05d7ef7..ff0b356 100644
--- a/examples/custom_enum_field_filter.rs
+++ b/examples/custom_enum_field_filter.rs
@@ -67,9 +67,9 @@ impl From<u8> for Label {
 // - coordinate type
 // - dimension (xyz = 3)
 // - metadata dimension (intensity + my_custom_label = 2)
-impl From<CustomPoint> for Point<5> {
+impl From<CustomPoint> for RPCL2Point<5> {
     fn from(point: CustomPoint) -> Self {
-        Point {
+        RPCL2Point {
             fields: [
                 point.x.into(),
                 point.y.into(),
@@ -81,8 +81,8 @@ impl From<CustomPoint> for Point<5> {
     }
 }
 
-impl From<Point<5>> for CustomPoint {
-    fn from(point: Point<5>) -> Self {
+impl From<RPCL2Point<5>> for CustomPoint {
+    fn from(point: RPCL2Point<5>) -> Self {
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
diff --git a/examples/rayon_bench.rs b/examples/rayon_bench.rs
index c3aa9eb..02d615c 100644
--- a/examples/rayon_bench.rs
+++ b/examples/rayon_bench.rs
@@ -1,13 +1,11 @@
+use rand::Rng;
 /// This example implements a naive benchmark for the library so you can evaluate the use of rayon for parallel processing.
 /// It generates a random point cloud and measures the time it takes to iterate over it.
-/// The code works mainly as a showcase. For real benchmarks, check the `benches` directory.
-/// It also implements the same benchmark shown here.
+/// The code works mainly as a showcase. For actual benchmarks, check the `benches` directory or run `cargo bench`.
 use std::time::Duration;
 
 use ros_pointcloud2::prelude::*;
 
-use rand::Rng;
-
 pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
     let mut rng = rand::thread_rng();
     let mut pointcloud = Vec::with_capacity(num_points);
@@ -24,7 +22,7 @@ pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<
 
 fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
-    let internal_msg = PointCloud2Msg::try_from_iter(cloud.into_iter()).unwrap();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let total = internal_msg
         .try_into_iter()
         .unwrap()
@@ -34,7 +32,7 @@ fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
 
 fn roundtrip_filter(cloud: Vec<PointXYZ>) -> bool {
     let orig_len = cloud.len();
-    let internal_msg = PointCloud2Msg::try_from_iter(cloud.into_iter()).unwrap();
+    let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
     let total = internal_msg
         .try_into_iter()
         .unwrap()
diff --git a/examples/simple_distance_filter.rs b/examples/simple_distance_filter.rs
index 59c6948..d594765 100644
--- a/examples/simple_distance_filter.rs
+++ b/examples/simple_distance_filter.rs
@@ -14,7 +14,7 @@ fn main() {
 
     println!("Original cloud: {:?}", cloud);
 
-    let msg = PointCloud2Msg::try_from_iter(cloud.clone().into_iter()).unwrap();
+    let msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
 
     println!("filtering by distance < 1.9m");
     let out = msg
diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs
index b1ce4a5..7f5e968 100644
--- a/rpcl2_derive/src/lib.rs
+++ b/rpcl2_derive/src/lib.rs
@@ -116,8 +116,8 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
     }).collect::<Vec<_>>();
 
     let from_my_point =  quote! {
-        impl From<ros_pointcloud2::Point<#field_len_token>> for #name {
-            fn from(point: ros_pointcloud2::Point<#field_len_token>) -> Self {
+        impl From<ros_pointcloud2::RPCL2Point<#field_len_token>> for #name {
+            fn from(point: ros_pointcloud2::RPCL2Point<#field_len_token>) -> Self {
                 Self {
                     #(#field_names_get,)*
                 }
@@ -131,7 +131,7 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
     }).collect::<Vec<_>>();
 
     let from_custom_point = quote! {
-        impl From<#name> for ros_pointcloud2::Point<#field_len_token> {
+        impl From<#name> for ros_pointcloud2::RPCL2Point<#field_len_token> {
             fn from(point: #name) -> Self {
                 ros_pointcloud2::Point {
                     fields: [ #(#field_names_into,)* ]
diff --git a/src/convert.rs b/src/convert.rs
index 14aaf1e..fc6edf3 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -148,14 +148,14 @@ impl From<FieldDatatype> for u8 {
     }
 }
 
-/// Matching field names from each meta data per point to the PointField name.
+/// 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::{Point, PointConvertible, MetaNames, size_of};
+/// use ros_pointcloud2::prelude::*;
 ///
 /// #[derive(Clone, Debug, PartialEq, Copy)]
 /// pub struct MyPointXYZI {
@@ -165,9 +165,9 @@ impl From<FieldDatatype> for u8 {
 ///     pub intensity: f32,
 /// }
 ///
-/// impl MetaNames<1> for MyPointXYZI {
-///    fn meta_names() -> [&'static str; 1] {
-///       ["intensity"]
+/// impl Fields<4> for MyPointXYZI {
+///    fn field_names_ordered() -> [&'static str; 4] {
+///       ["x", "y", "z", "intensity"]
 ///   }
 /// }
 /// ```
diff --git a/src/iterator.rs b/src/iterator.rs
index fdbcc22..99135de 100644
--- a/src/iterator.rs
+++ b/src/iterator.rs
@@ -1,6 +1,6 @@
 use crate::{
     convert::{Endianness, FieldDatatype},
-    Fields, MsgConversionError, Point, PointCloud2Msg, PointConvertible, PointMeta,
+    Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointMeta, RPCL2Point,
 };
 
 /// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg.
@@ -192,7 +192,7 @@ impl<const N: usize> ByteBufferView<N> {
     }
 
     #[inline(always)]
-    fn point_at(&self, idx: usize) -> Point<N> {
+    fn point_at(&self, idx: usize) -> RPCL2Point<N> {
         let offset = (self.start_point_idx + idx) * self.point_step_size;
 
         // TODO memcpy entire point at once, then extract fields?
@@ -209,7 +209,7 @@ impl<const N: usize> ByteBufferView<N> {
                 );
             });
 
-        Point { fields: meta }
+        RPCL2Point { fields: meta }
     }
 
     #[inline]
diff --git a/src/lib.rs b/src/lib.rs
index 3ef4075..599fe14 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -21,12 +21,12 @@
 //!
 //! # Example PointXYZ
 //! ```
-//! use ros_pointcloud2::{PointCloud2Msg, pcl_utils::PointXYZ};
+//! use ros_pointcloud2::prelude::*;
 //!
 //! // PointXYZ is predefined
 //! let cloud_points = vec![
-//!   PointXYZ {x: 9.0006, y: 42.0, z: -6.2,},
-//!   PointXYZ {x: f32::MAX, y: f32::MIN,z: f32::MAX,},
+//!   PointXYZ::new(9.0006, 42.0, -6.2),
+//!   PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
 //! ];
 //!
 //! // For equality test later
@@ -164,7 +164,7 @@ impl PointCloud2Msg {
     where
         C: PointConvertible<N>,
     {
-        let point: Point<N> = C::default().into();
+        let point: RPCL2Point<N> = C::default().into();
         debug_assert!(point.fields.len() == N);
 
         let meta_names = C::field_names_ordered();
@@ -206,7 +206,7 @@ impl PointCloud2Msg {
     where
         C: PointConvertible<N>,
     {
-        let point: Point<N> = C::default().into();
+        let point: RPCL2Point<N> = C::default().into();
         debug_assert!(point.fields.len() == N);
 
         let meta_names = C::field_names_ordered();
@@ -253,7 +253,7 @@ impl PointCloud2Msg {
     where
         C: PointConvertible<N>,
     {
-        let point: Point<N> = C::default().into();
+        let point: RPCL2Point<N> = C::default().into();
         debug_assert!(point.fields.len() == N);
 
         let meta_names = C::field_names_ordered();
@@ -295,13 +295,11 @@ impl PointCloud2Msg {
     ///
     /// # Example
     /// ```
-    /// use ros_pointcloud2::{
-    ///     pcl_utils::PointXYZ, PointCloud2Msg,
-    /// };
+    /// use ros_pointcloud2::prelude::*;
     ///
     /// let cloud_points: Vec<PointXYZ> = vec![
-    ///     PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
-    ///     PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
+    ///     PointXYZ::new(1.0, 2.0, 3.0),
+    ///     PointXYZ::new(4.0, 5.0, 6.0),
     /// ];
     ///
     // let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
@@ -315,7 +313,7 @@ impl PointCloud2Msg {
         let mut cloud = Self::prepare::<N, C>()?;
 
         iterable.into_iter().for_each(|coords| {
-            let point: Point<N> = coords.into();
+            let point: RPCL2Point<N> = coords.into();
 
             point.fields.iter().for_each(|meta| {
                 let truncated_bytes = unsafe {
@@ -410,19 +408,33 @@ impl PointCloud2Msg {
 /// Implement the `From` traits for your point type to use the conversion.
 ///
 /// See the [`ros_pointcloud2::PointConvertible`] trait for more information.
-pub struct Point<const N: usize> {
+pub struct RPCL2Point<const N: usize> {
     pub fields: [PointMeta; N],
 }
 
+impl<const N: usize> Default for RPCL2Point<N> {
+    fn default() -> Self {
+        Self {
+            fields: [PointMeta::default(); N],
+        }
+    }
+}
+
+impl<const N: usize> From<[PointMeta; N]> for RPCL2Point<N> {
+    fn from(fields: [PointMeta; N]) -> Self {
+        Self { fields }
+    }
+}
+
 /// Trait to enable point conversions on the fly while iterating.
 ///
 /// Implement this trait for your custom point you want to read or write in the message.
 ///
 /// # Example
 /// ```
-/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
+/// use ros_pointcloud2::prelude::*;
 ///
-/// #[derive(Clone, Debug, PartialEq, Copy)]
+/// #[derive(Clone, Debug, PartialEq, Copy, Default)]
 /// pub struct MyPointXYZI {
 ///     pub x: f32,
 ///     pub y: f32,
@@ -430,43 +442,42 @@ pub struct Point<const N: usize> {
 ///     pub intensity: f32,
 /// }
 ///
-/// impl From<MyPointXYZI> for Point<f32, 3, 1> {
+/// impl From<MyPointXYZI> for RPCL2Point<4> {
 ///     fn from(point: MyPointXYZI) -> Self {
-///         Point {
-///             coords: [point.x, point.y, point.z],
-///             meta: [point.intensity.into()],
+///         RPCL2Point {
+///             fields: [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()],
 ///         }
 ///     }
 /// }
 ///
-/// impl From<Point<f32, 3, 1>> for MyPointXYZI {
-///     fn from(point: Point<f32, 3, 1>) -> Self {
+/// impl From<RPCL2Point<4>> for MyPointXYZI {
+///     fn from(point: RPCL2Point<4>) -> Self {
 ///         Self {
-///             x: point.coords[0],
-///             y: point.coords[1],
-///             z: point.coords[2],
-///             intensity: point.meta[0].get(),
+///             x: point.fields[0].get(),
+///             y: point.fields[1].get(),
+///             z: point.fields[2].get(),
+///             intensity: point.fields[3].get(),
 ///         }
 ///     }
 /// }
 ///
-/// impl MetaNames<1> for MyPointXYZI {
-///    fn meta_names() -> [&'static str; 1] {
-///       ["intensity"]
+/// impl Fields<4> for MyPointXYZI {
+///    fn field_names_ordered() -> [&'static str; 4] {
+///       ["x", "y", "z", "intensity"]
 ///   }
 /// }
 ///
-/// impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for MyPointXYZI {}
+/// impl PointConvertible<4> for MyPointXYZI {}
 /// ```
 #[cfg(not(feature = "derive"))]
 pub trait PointConvertible<const N: usize>:
-    From<Point<N>> + Into<Point<N>> + Fields<N> + Clone + 'static + Default
+    From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Clone + 'static + Default
 {
 }
 
 #[cfg(feature = "derive")]
 pub trait PointConvertible<const N: usize>:
-    type_layout::TypeLayout + From<Point<N>> + Into<Point<N>> + Fields<N> + 'static + Default
+    type_layout::TypeLayout + From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + 'static + Default
 {
 }
 
diff --git a/src/points.rs b/src/points.rs
index cba5cd3..aa0b7aa 100644
--- a/src/points.rs
+++ b/src/points.rs
@@ -1,4 +1,4 @@
-use crate::{Fields, Point, PointConvertible};
+use crate::{Fields, PointConvertible, RPCL2Point};
 
 #[cfg(feature = "derive")]
 use type_layout::TypeLayout;
@@ -147,21 +147,19 @@ impl Fields<3> for PointXYZ {
     }
 }
 
-impl From<Point<3>> for PointXYZ {
-    fn from(point: Point<3>) -> Self {
-        Self {
-            x: point.fields[0].get(),
-            y: point.fields[1].get(),
-            z: point.fields[2].get(),
-        }
+impl From<RPCL2Point<3>> for PointXYZ {
+    fn from(point: RPCL2Point<3>) -> Self {
+        Self::new(
+            point.fields[0].get(),
+            point.fields[1].get(),
+            point.fields[2].get(),
+        )
     }
 }
 
-impl From<PointXYZ> for Point<3> {
+impl From<PointXYZ> for RPCL2Point<3> {
     fn from(point: PointXYZ) -> Self {
-        Point {
-            fields: [point.x.into(), point.y.into(), point.z.into()],
-        }
+        [point.x.into(), point.y.into(), point.z.into()].into()
     }
 }
 
@@ -199,27 +197,26 @@ impl Fields<4> for PointXYZI {
     }
 }
 
-impl From<Point<4>> for PointXYZI {
-    fn from(point: Point<4>) -> Self {
-        Self {
-            x: point.fields[0].get(),
-            y: point.fields[1].get(),
-            z: point.fields[2].get(),
-            intensity: point.fields[3].get(),
-        }
+impl From<RPCL2Point<4>> for PointXYZI {
+    fn from(point: RPCL2Point<4>) -> Self {
+        Self::new(
+            point.fields[0].get(),
+            point.fields[1].get(),
+            point.fields[2].get(),
+            point.fields[3].get(),
+        )
     }
 }
 
-impl From<PointXYZI> for Point<4> {
+impl From<PointXYZI> for RPCL2Point<4> {
     fn from(point: PointXYZI) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                point.intensity.into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            point.intensity.into(),
+        ]
+        .into()
     }
 }
 
@@ -238,13 +235,8 @@ pub struct PointXYZL {
 }
 
 impl PointXYZL {
-    pub fn new(x: f32, y: f32, z: f32, intensity: f32) -> Self {
-        Self {
-            x,
-            y,
-            z,
-            label: intensity as u32,
-        }
+    pub fn new(x: f32, y: f32, z: f32, label: u32) -> Self {
+        Self { x, y, z, label }
     }
 
     #[cfg(feature = "nalgebra")]
@@ -262,27 +254,26 @@ impl Fields<4> for PointXYZL {
     }
 }
 
-impl From<Point<4>> for PointXYZL {
-    fn from(point: Point<4>) -> Self {
-        Self {
-            x: point.fields[0].get(),
-            y: point.fields[1].get(),
-            z: point.fields[2].get(),
-            label: point.fields[3].get(),
-        }
+impl From<RPCL2Point<4>> for PointXYZL {
+    fn from(point: RPCL2Point<4>) -> Self {
+        Self::new(
+            point.fields[0].get(),
+            point.fields[1].get(),
+            point.fields[2].get(),
+            point.fields[3].get(),
+        )
     }
 }
 
-impl From<PointXYZL> for Point<4> {
+impl From<PointXYZL> for RPCL2Point<4> {
     fn from(point: PointXYZL) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                point.label.into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            point.label.into(),
+        ]
+        .into()
     }
 }
 
@@ -333,8 +324,8 @@ impl Fields<4> for PointXYZRGB {
     }
 }
 
-impl From<Point<4>> for PointXYZRGB {
-    fn from(point: Point<4>) -> Self {
+impl From<RPCL2Point<4>> for PointXYZRGB {
+    fn from(point: RPCL2Point<4>) -> Self {
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
@@ -344,16 +335,15 @@ impl From<Point<4>> for PointXYZRGB {
     }
 }
 
-impl From<PointXYZRGB> for Point<4> {
+impl From<PointXYZRGB> for RPCL2Point<4> {
     fn from(point: PointXYZRGB) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                f32::from(point.rgb).into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            f32::from(point.rgb).into(),
+        ]
+        .into()
     }
 }
 
@@ -406,8 +396,8 @@ impl Fields<5> for PointXYZRGBA {
     }
 }
 
-impl From<Point<5>> for PointXYZRGBA {
-    fn from(point: Point<5>) -> Self {
+impl From<RPCL2Point<5>> for PointXYZRGBA {
+    fn from(point: RPCL2Point<5>) -> Self {
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
@@ -418,17 +408,16 @@ impl From<Point<5>> for PointXYZRGBA {
     }
 }
 
-impl From<PointXYZRGBA> for Point<5> {
+impl From<PointXYZRGBA> for RPCL2Point<5> {
     fn from(point: PointXYZRGBA) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                f32::from(point.rgb).into(),
-                point.a.into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            f32::from(point.rgb).into(),
+            point.a.into(),
+        ]
+        .into()
     }
 }
 
@@ -454,14 +443,11 @@ impl PointXYZRGBNormal {
         x: f32,
         y: f32,
         z: f32,
-        r: u8,
-        g: u8,
-        b: u8,
+        rgb: RGB,
         normal_x: f32,
         normal_y: f32,
         normal_z: f32,
     ) -> Self {
-        let rgb = RGB::new(r, g, b);
         Self {
             x,
             y,
@@ -500,8 +486,8 @@ impl Fields<7> for PointXYZRGBNormal {
     }
 }
 
-impl From<Point<7>> for PointXYZRGBNormal {
-    fn from(point: Point<7>) -> Self {
+impl From<RPCL2Point<7>> for PointXYZRGBNormal {
+    fn from(point: RPCL2Point<7>) -> Self {
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
@@ -514,19 +500,18 @@ impl From<Point<7>> for PointXYZRGBNormal {
     }
 }
 
-impl From<PointXYZRGBNormal> for Point<7> {
+impl From<PointXYZRGBNormal> for RPCL2Point<7> {
     fn from(point: PointXYZRGBNormal) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                f32::from(point.rgb).into(),
-                point.normal_x.into(),
-                point.normal_y.into(),
-                point.normal_z.into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            f32::from(point.rgb).into(),
+            point.normal_x.into(),
+            point.normal_y.into(),
+            point.normal_z.into(),
+        ]
+        .into()
     }
 }
 
@@ -583,33 +568,32 @@ impl Fields<7> for PointXYZINormal {
     }
 }
 
-impl From<Point<7>> for PointXYZINormal {
-    fn from(point: Point<7>) -> Self {
-        Self {
-            x: point.fields[0].get(),
-            y: point.fields[1].get(),
-            z: point.fields[2].get(),
-            intensity: point.fields[3].get(),
-            normal_x: point.fields[4].get(),
-            normal_y: point.fields[5].get(),
-            normal_z: point.fields[6].get(),
-        }
+impl From<RPCL2Point<7>> for PointXYZINormal {
+    fn from(point: RPCL2Point<7>) -> Self {
+        Self::new(
+            point.fields[0].get(),
+            point.fields[1].get(),
+            point.fields[2].get(),
+            point.fields[3].get(),
+            point.fields[4].get(),
+            point.fields[5].get(),
+            point.fields[6].get(),
+        )
     }
 }
 
-impl From<PointXYZINormal> for Point<7> {
+impl From<PointXYZINormal> for RPCL2Point<7> {
     fn from(point: PointXYZINormal) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                point.intensity.into(),
-                point.normal_x.into(),
-                point.normal_y.into(),
-                point.normal_z.into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            point.intensity.into(),
+            point.normal_x.into(),
+            point.normal_y.into(),
+            point.normal_z.into(),
+        ]
+        .into()
     }
 }
 
@@ -667,8 +651,8 @@ impl Fields<5> for PointXYZRGBL {
     }
 }
 
-impl From<Point<5>> for PointXYZRGBL {
-    fn from(point: Point<5>) -> Self {
+impl From<RPCL2Point<5>> for PointXYZRGBL {
+    fn from(point: RPCL2Point<5>) -> Self {
         Self {
             x: point.fields[0].get(),
             y: point.fields[1].get(),
@@ -679,17 +663,16 @@ impl From<Point<5>> for PointXYZRGBL {
     }
 }
 
-impl From<PointXYZRGBL> for Point<5> {
+impl From<PointXYZRGBL> for RPCL2Point<5> {
     fn from(point: PointXYZRGBL) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                f32::from(point.rgb).into(),
-                point.label.into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            f32::from(point.rgb).into(),
+            point.label.into(),
+        ]
+        .into()
     }
 }
 
@@ -736,31 +719,30 @@ impl Fields<6> for PointXYZNormal {
     }
 }
 
-impl From<Point<6>> for PointXYZNormal {
-    fn from(point: Point<6>) -> Self {
-        Self {
-            x: point.fields[0].get(),
-            y: point.fields[1].get(),
-            z: point.fields[2].get(),
-            normal_x: point.fields[3].get(),
-            normal_y: point.fields[4].get(),
-            normal_z: point.fields[5].get(),
-        }
+impl From<RPCL2Point<6>> for PointXYZNormal {
+    fn from(point: RPCL2Point<6>) -> Self {
+        Self::new(
+            point.fields[0].get(),
+            point.fields[1].get(),
+            point.fields[2].get(),
+            point.fields[3].get(),
+            point.fields[4].get(),
+            point.fields[5].get(),
+        )
     }
 }
 
-impl From<PointXYZNormal> for Point<6> {
+impl From<PointXYZNormal> for RPCL2Point<6> {
     fn from(point: PointXYZNormal) -> Self {
-        Point {
-            fields: [
-                point.x.into(),
-                point.y.into(),
-                point.z.into(),
-                point.normal_x.into(),
-                point.normal_y.into(),
-                point.normal_z.into(),
-            ],
-        }
+        [
+            point.x.into(),
+            point.y.into(),
+            point.z.into(),
+            point.normal_x.into(),
+            point.normal_y.into(),
+            point.normal_z.into(),
+        ]
+        .into()
     }
 }
 
diff --git a/src/prelude.rs b/src/prelude.rs
index cce24f7..4097dbc 100644
--- a/src/prelude.rs
+++ b/src/prelude.rs
@@ -4,7 +4,7 @@ pub use crate::points::*;
 #[cfg(feature = "rayon")]
 pub use rayon::prelude::*;
 
-pub use crate::{Fields, MsgConversionError, Point, PointCloud2Msg, PointConvertible};
+pub use crate::{Fields, MsgConversionError, PointCloud2Msg, PointConvertible, RPCL2Point};
 
 #[cfg(feature = "derive")]
 pub use type_layout::TypeLayout;
diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs
index 44718da..d559fe9 100644
--- a/tests/e2e_test.rs
+++ b/tests/e2e_test.rs
@@ -54,7 +54,7 @@ fn write_cloud() {
         PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
     ];
 
-    let msg = PointCloud2Msg::try_from_iter(cloud.into_iter());
+    let msg = PointCloud2Msg::try_from_iter(cloud);
     assert!(msg.is_ok());
 }
 
@@ -256,16 +256,14 @@ fn converterxyzrgbnormal() {
     convert_from_into!(
         PointXYZRGBNormal,
         vec![
-            PointXYZRGBNormal::new(0.0, 1.0, 5.0, 0, 0, 0, 0.0, 0.0, 0.0),
-            PointXYZRGBNormal::new(1.0, 1.5, 5.0, 1, 1, 1, 1.0, 1.0, 1.0),
-            PointXYZRGBNormal::new(1.3, 1.6, 5.7, 2, 2, 2, 2.0, 2.0, 2.0),
+            PointXYZRGBNormal::new(0.0, 1.0, 5.0, RGB::new(0, 0, 0), 0.0, 0.0, 0.0),
+            PointXYZRGBNormal::new(1.0, 1.5, 5.0, RGB::new(1, 1, 1), 1.0, 1.0, 1.0),
+            PointXYZRGBNormal::new(1.3, 1.6, 5.7, RGB::new(2, 2, 2), 2.0, 2.0, 2.0),
             PointXYZRGBNormal::new(
                 f32::MAX,
                 f32::MIN,
                 f32::MAX,
-                u8::MAX,
-                u8::MAX,
-                u8::MAX,
+                RGB::new(u8::MAX, u8::MAX, u8::MAX),
                 f32::MAX,
                 f32::MAX,
                 f32::MAX,
@@ -339,10 +337,10 @@ fn converterxyzl() {
     convert_from_into!(
         PointXYZL,
         vec![
-            PointXYZL::new(0.0, 1.0, 5.0, 0.0),
-            PointXYZL::new(1.0, 1.5, 5.0, 1.0),
-            PointXYZL::new(1.3, 1.6, 5.7, 2.0),
-            PointXYZL::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
+            PointXYZL::new(0.0, 1.0, 5.0, 0),
+            PointXYZL::new(1.0, 1.5, 5.0, 1),
+            PointXYZL::new(1.3, 1.6, 5.7, 2),
+            PointXYZL::new(f32::MAX, f32::MIN, f32::MAX, u32::MAX),
         ]
     );
 }
@@ -391,8 +389,8 @@ fn write_less_than_available() {
         dummy: f32,
     }
 
-    impl From<Point<3>> for CustomPoint {
-        fn from(point: Point<3>) -> Self {
+    impl From<RPCL2Point<3>> for CustomPoint {
+        fn from(point: RPCL2Point<3>) -> Self {
             Self {
                 x: point.fields[0].get(),
                 y: point.fields[1].get(),
@@ -402,7 +400,7 @@ fn write_less_than_available() {
         }
     }
 
-    impl From<CustomPoint> for Point<3> {
+    impl From<CustomPoint> for RPCL2Point<3> {
         fn from(point: CustomPoint) -> Self {
             Point {
                 fields: [point.x.into(), point.y.into(), point.z.into()],

From d2c863e208341559e41e668a349fedb343744134 Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 7 May 2024 23:14:03 +0200
Subject: [PATCH 09/10] test with all feats

---
 .github/workflows/r2r_galactic.yml   |  2 +-
 .github/workflows/r2r_humble.yml     |  2 +-
 .github/workflows/r2r_iron.yml       |  2 +-
 .github/workflows/rclrs_humble.yml   |  2 +-
 .github/workflows/rclrs_iron.yml     |  2 +-
 .github/workflows/rosrust_noetic.yml |  2 +-
 .github/workflows/tests.yml          |  2 +-
 Cargo.toml                           |  2 ++
 benches/roundtrip.rs                 | 13 +++++--------
 examples/custom_enum_field_filter.rs | 14 +-------------
 examples/rayon_bench.rs              | 13 +++++--------
 rpcl2_derive/src/lib.rs              |  2 +-
 src/lib.rs                           |  6 +++---
 tests/e2e_test.rs                    |  4 +---
 14 files changed, 25 insertions(+), 43 deletions(-)

diff --git a/.github/workflows/r2r_galactic.yml b/.github/workflows/r2r_galactic.yml
index 5c5f3d1..45a609f 100644
--- a/.github/workflows/r2r_galactic.yml
+++ b/.github/workflows/r2r_galactic.yml
@@ -18,4 +18,4 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - run: docker build . --file ./tests/Dockerfile_r2r_galactic --tag r2r_galactic
-    - run: docker run r2r_galactic cargo test --features r2r_msg
+    - run: docker run r2r_galactic cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/r2r_humble.yml b/.github/workflows/r2r_humble.yml
index 5373957..a88752c 100644
--- a/.github/workflows/r2r_humble.yml
+++ b/.github/workflows/r2r_humble.yml
@@ -18,4 +18,4 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - run: docker build . --file ./tests/Dockerfile_r2r_humble --tag r2r_humble
-    - run: docker run r2r_humble cargo test --features r2r_msg
+    - run: docker run r2r_humble cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/r2r_iron.yml b/.github/workflows/r2r_iron.yml
index 5de642d..ef3ab9d 100644
--- a/.github/workflows/r2r_iron.yml
+++ b/.github/workflows/r2r_iron.yml
@@ -18,4 +18,4 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - run: docker build . --file ./tests/Dockerfile_r2r_iron --tag r2r_iron
-    - run: docker run r2r_iron cargo test --features r2r_msg
+    - run: docker run r2r_iron cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/rclrs_humble.yml b/.github/workflows/rclrs_humble.yml
index 7ac4349..1c0f88b 100644
--- a/.github/workflows/rclrs_humble.yml
+++ b/.github/workflows/rclrs_humble.yml
@@ -18,4 +18,4 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble
-    - run: docker run rclrs_humble cargo test
+    - run: docker run rclrs_humble cargo test --features derive,nalgebra,rayon
diff --git a/.github/workflows/rclrs_iron.yml b/.github/workflows/rclrs_iron.yml
index d9ac7cf..390419b 100644
--- a/.github/workflows/rclrs_iron.yml
+++ b/.github/workflows/rclrs_iron.yml
@@ -18,4 +18,4 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - run: docker build . --file ./tests/Dockerfile_rclrs_iron --tag rclrs_iron
-    - run: docker run rclrs_iron cargo test
+    - run: docker run rclrs_iron cargo test --features derive,nalgebra,rayon
diff --git a/.github/workflows/rosrust_noetic.yml b/.github/workflows/rosrust_noetic.yml
index 4662df4..40a5c12 100644
--- a/.github/workflows/rosrust_noetic.yml
+++ b/.github/workflows/rosrust_noetic.yml
@@ -105,4 +105,4 @@ jobs:
       - name: test
         run: |
           source /opt/ros/$ROS_DISTRO/setup.bash
-          cargo test --features rosrust_msg
+          cargo test --features rosrust_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index 7f940b7..04c5cee 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -25,4 +25,4 @@ jobs:
       - name: Linting
         run: cargo clippy --all-targets -- -D warnings
       - name: Tests
-        run: cargo test
+        run: cargo test --features derive,nalgebra,rayon
diff --git a/Cargo.toml b/Cargo.toml
index 89425e1..c347ffd 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -50,3 +50,5 @@ r2r_msg = ["dep:r2r"]
 rayon = ["dep:rayon"]
 derive = ["dep:rpcl2_derive", "dep:type-layout"]
 nalgebra = ["dep:nalgebra"]
+
+default = ["rayon", "derive", "nalgebra"]
\ No newline at end of file
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
index c0bb7ea..cf4945b 100644
--- a/benches/roundtrip.rs
+++ b/benches/roundtrip.rs
@@ -94,14 +94,11 @@ fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
         .filter(|point: &PointXYZ| {
             (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
         })
-        .reduce(
-            || PointXYZ::default(),
-            |acc, point| PointXYZ {
-                x: acc.x + point.x,
-                y: acc.y + point.y,
-                z: acc.z + point.z,
-            },
-        );
+        .reduce(PointXYZ::default, |acc, point| PointXYZ {
+            x: acc.x + point.x,
+            y: acc.y + point.y,
+            z: acc.z + point.z,
+        });
     orig_len == total.x as usize
 }
 
diff --git a/examples/custom_enum_field_filter.rs b/examples/custom_enum_field_filter.rs
index ff0b356..264422b 100644
--- a/examples/custom_enum_field_filter.rs
+++ b/examples/custom_enum_field_filter.rs
@@ -27,19 +27,7 @@ struct CustomPoint {
     my_custom_label: Label,
 }
 
-// For us as convenience
-impl CustomPoint {
-    fn new(x: f32, y: f32, z: f32, intensity: f32, my_custom_label: Label) -> Self {
-        Self {
-            x,
-            y,
-            z,
-            intensity,
-            my_custom_label,
-        }
-    }
-}
-
+// For our convenience
 impl From<Label> for u8 {
     fn from(label: Label) -> Self {
         match label {
diff --git a/examples/rayon_bench.rs b/examples/rayon_bench.rs
index 02d615c..ef21641 100644
--- a/examples/rayon_bench.rs
+++ b/examples/rayon_bench.rs
@@ -68,14 +68,11 @@ fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> bool {
         .filter(|point: &PointXYZ| {
             (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
         })
-        .reduce(
-            || PointXYZ::default(),
-            |acc, point| PointXYZ {
-                x: acc.x + point.x,
-                y: acc.y + point.y,
-                z: acc.z + point.z,
-            },
-        );
+        .reduce(PointXYZ::default, |acc, point| PointXYZ {
+            x: acc.x + point.x,
+            y: acc.y + point.y,
+            z: acc.z + point.z,
+        });
     orig_len == total.x as usize
 }
 
diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs
index 7f5e968..a03e5bc 100644
--- a/rpcl2_derive/src/lib.rs
+++ b/rpcl2_derive/src/lib.rs
@@ -133,7 +133,7 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
     let from_custom_point = quote! {
         impl From<#name> for ros_pointcloud2::RPCL2Point<#field_len_token> {
             fn from(point: #name) -> Self {
-                ros_pointcloud2::Point {
+                ros_pointcloud2::RPCL2Point {
                     fields: [ #(#field_names_into,)* ]
                 }
             }
diff --git a/src/lib.rs b/src/lib.rs
index 599fe14..9956fc4 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -336,7 +336,7 @@ impl PointCloud2Msg {
         C: PointConvertible<N>,
     {
         match system_endian() {
-            Endianness::Big => Self::try_from_iter(vec.into_iter()),
+            Endianness::Big => Self::try_from_iter(vec),
             Endianness::Little => {
                 let mut cloud = Self::prepare_direct_copy::<N, C>()?;
 
@@ -503,10 +503,10 @@ impl TryFrom<type_layout::Field> for PointField {
     fn try_from(f: type_layout::Field) -> Result<Self, Self::Error> {
         match f {
             type_layout::Field::Field { name, ty, size } => {
-                let typename: String = ty.to_owned().into();
+                let typename: String = ty.into_owned();
                 let datatype = FieldDatatype::try_from(typename)?;
                 Ok(Self::Field {
-                    name: name.to_owned().into(),
+                    name: name.into_owned(),
                     size: size as u32,
                     datatype: datatype.into(),
                 })
diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs
index d559fe9..ffbc5e5 100644
--- a/tests/e2e_test.rs
+++ b/tests/e2e_test.rs
@@ -402,9 +402,7 @@ fn write_less_than_available() {
 
     impl From<CustomPoint> for RPCL2Point<3> {
         fn from(point: CustomPoint) -> Self {
-            Point {
-                fields: [point.x.into(), point.y.into(), point.z.into()],
-            }
+            [point.x.into(), point.y.into(), point.z.into()].into()
         }
     }
 

From 8739658e2f81249bab80471a387238b661172a7b Mon Sep 17 00:00:00 2001
From: stelzo <stelzo@steado.de>
Date: Tue, 7 May 2024 23:17:12 +0200
Subject: [PATCH 10/10] indent

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 4b1a85f..7143c6c 100644
--- a/README.md
+++ b/README.md
@@ -37,7 +37,7 @@ let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
 
 let new_pcl = in_msg.try_into_iter().unwrap()
   .map(|point: PointXYZ| { // Define the type of point here.
-    // Some logic here ...
+      // Some logic here ...
 
       point
     })