merge main

README.md

@@ -10,8 +10,6 @@ Providing an easy to use, generics defined, point-wise iterator abstraction over
 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`.
 
 ## Examples
-
-Using a `Vec<>`.
 ```rust
 use ros_pointcloud2::{
   pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
@@ -19,24 +17,15 @@ use ros_pointcloud2::{
 
 // Your points (here using the predefined type PointXYZ).
 let cloud_points = vec![
-  PointXYZ {
+  PointXYZ {x: 91.486, y: -4.1, z: 42.0001,},
-    x: 91.486,
+  PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,},
-    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 `IntoIter`.
+// 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();
@@ -49,7 +38,7 @@ let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
 // ... now incoming from a topic.
 // let internal_msg: PointCloud2Msg = msg.into();
 
-// Message -> Reader. The Reader implements the Iterator trait.
+// 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| {

src/convert.rs

@@ -8,10 +8,8 @@ pub enum FieldDatatype {
     I32,
     U8,
     U16,
-
     #[default]
     U32,
-
     I8,
     I16,
 }
@@ -135,41 +133,32 @@ pub(crate) fn check_coord(
 }
 
 /// Matching field names from each meta data per point to the PointField name.
-/// Always make sure to use the same order as in your Into<> implementation to have a correct mapping.
+/// 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 for full point conversion.
+/// ```
+/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
+///
+/// #[derive(Clone, Debug, PartialEq, Copy)]
+/// pub struct MyPointXYZI {
+///     pub x: f32,
+///     pub y: f32,
+///     pub z: f32,
+///     pub intensity: f32,
+/// }
+///
+/// impl MetaNames<1> for MyPointXYZI {
+///    fn meta_names() -> [&'static str; 1] {
+///       ["intensity"]
+///   }
+/// }
+/// ```
 pub trait MetaNames<const METADIM: usize> {
     fn meta_names() -> [&'static str; METADIM];
 }
 
-#[inline(always)]
-pub(crate) 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)
-}
-
 /// This trait is used to convert a byte slice to a primitive type.
 /// All PointField types are supported.
 pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype {

src/lib.rs

@@ -1,6 +1,6 @@
 //! 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
+//! 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.
 //!
 //! For ROS interoperability, the message type generated by the respective crate must be converted to
@@ -23,25 +23,15 @@
 //!
 //! // Your points (here using the predefined type PointXYZ).
 //! let cloud_points = vec![
-//!   PointXYZ {
+//!   PointXYZ {x: 9.0006, y: 42.0, z: -6.2,},
-//!     x: 9.0006,
+//!   PointXYZ {x: f32::MAX, y: f32::MIN,z: f32::MAX,},
-//!     y: 42.0,
-//!     z: -6.2,
-//!   },
-//!
-//!   PointXYZ {
-//!     x: f32::MAX,
-//!     y: f32::MIN,
-//!     z: f32::MAX,
-//!   },
 //! ];
 //!
 //! // For equality test later
 //! let cloud_copy = cloud_points.clone();
 //!
 //! // Vector -> Writer -> Message.
-//! // Vector -> Writer -> Message.
+//! // You can also just give the Vec or anything that implements `IntoIterator`.
-//! // You can also just give the Vec or anything that implements `IntoIter`.
 //! let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
 //!     .try_into() // iterating points here O(n)
 //!     .unwrap();
@@ -54,7 +44,7 @@
 //! // ... now incoming from a topic.
 //! // let internal_msg: PointCloud2Msg = msg.into();
 //!
-//! // Message -> Reader. The Reader implements the Iterator trait.
+//! // 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| {
@@ -91,10 +81,14 @@ macro_rules! size_of {
 pub mod reader;
 pub mod writer;
 
+pub use convert::MetaNames;
 pub use ros_types::PointCloud2Msg;
 
-use crate::convert::*;
+use crate::convert::{
-use crate::pcl_utils::*;
+    FieldDatatype,
+    FromBytes,
+};
+
 use crate::ros_types::PointFieldMsg;
 
 /// All errors that can occur for creating Reader and Writer.
@@ -119,6 +113,42 @@ pub enum ConversionError {
     DataLengthMismatch,
 }
 
+/// 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;
+///
+/// #[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(),
+///         }
+///     }
+/// }
+/// ```
 pub struct Point<T, const DIM: usize, const METADIM: usize> {
     pub coords: [T; DIM],
     pub meta: [PointMeta; METADIM],
@@ -126,6 +156,47 @@ pub struct Point<T, const DIM: usize, const METADIM: usize> {
 
 /// Trait to convert a point to a tuple of coordinates and meta data.
 /// Implement this trait for your point type to use the conversion.
+/// 
+/// # Example
+/// ```
+/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
+///
+/// #[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(),
+///         }
+///     }
+/// }
+///
+/// impl MetaNames<1> for MyPointXYZI {
+///    fn meta_names() -> [&'static str; 1] {
+///       ["intensity"]
+///   }
+/// }
+///
+/// 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
@@ -155,14 +226,14 @@ pub struct PointMeta {
 impl Default for PointMeta {
     fn default() -> Self {
         Self {
-            bytes: [0; std::mem::size_of::<f64>()],
+            bytes: [u8::default(); std::mem::size_of::<f64>()],
             datatype: FieldDatatype::F32,
         }
     }
 }
 
 impl PointMeta {
-    /// Create a new PointMeta from a value
+    /// Create a new PointMeta from a value.
     ///
     /// # Example
     /// ```

src/reader.rs

@@ -1,4 +1,19 @@
-use crate::*;
+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> =
@@ -124,7 +139,7 @@ pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>;
 /// ## Example
 /// ```
 /// use ros_pointcloud2::{
-///     reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta,
+///     reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
 /// };
 ///
 /// type Xyz = f32; // coordinate type

src/ros_types.rs

@@ -142,75 +142,16 @@ impl From<PointCloud2Msg> for rosrust_msg::sensor_msgs::PointCloud2 {
     fn from(msg: PointCloud2Msg) -> Self {
         rosrust_msg::sensor_msgs::PointCloud2 {
             header: rosrust_msg::std_msgs::Header {
-                seq: msg.header.seq,
+                seq: self.header.seq,
-                stamp: TimeMsg {
+                stamp: rosrust::Time {
-                    sec: msg.header.stamp.sec,
+                    sec: self.header.stamp.sec,
-                    nsec: msg.header.stamp.nsec,
+                    nsec: self.header.stamp.nsec,
                 },
-                frame_id: msg.header.frame_id,
-            },
-            height: msg.height,
-            width: msg.width,
-            fields: msg
-                .fields
-                .into_iter()
-                .map(|field| rosrust_msg::sensor_msgs::PointField {
-                    name: field.name,
-                    offset: field.offset,
-                    datatype: field.datatype,
-                    count: field.count,
-                })
-                .collect(),
-            is_bigendian: msg.is_bigendian,
-            point_step: msg.point_step,
-            row_step: msg.row_step,
-            data: msg.data,
-            is_dense: msg.is_dense,
-        }
-    }
-}
-
-#[cfg(feature = "rclrs_msg")]
-impl From<sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
-    fn from(msg: sensor_msgs::msg::PointCloud2) -> Self {
-        Self {
-            header: HeaderMsg {
-                seq: 0,
-                stamp: msg.header.stamp,
-                frame_id: msg.header.frame_id,
-            },
-            height: msg.height,
-            width: msg.width,
-            fields: msg
-                .fields
-                .into_iter()
-                .map(|field| PointFieldMsg {
-                    name: field.name,
-                    offset: field.offset,
-                    datatype: field.datatype,
-                    count: field.count,
-                })
-                .collect(),
-            is_bigendian: msg.is_bigendian,
-            point_step: msg.point_step,
-            row_step: msg.row_step,
-            data: msg.data,
-            is_dense: msg.is_dense,
-        }
-    }
-}
-
-#[cfg(feature = "rclrs_msg")]
-impl Into<sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
-    fn into(self) -> sensor_msgs::msg::PointCloud2 {
-        sensor_msgs::msg::PointCloud2 {
-            header: std_msgs::msg::Header {
-                stamp: self.header.stamp,
                 frame_id: self.header.frame_id,
             },
-            height: self.height,
+            height: msg.height,
-            width: self.width,
+            width: msg.width,
-            fields: self
+            fields: msg
                 .fields
                 .into_iter()
                 .map(|field| sensor_msgs::msg::PointField {
@@ -220,11 +161,11 @@ impl Into<sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
                     count: field.count,
                 })
                 .collect(),
-            is_bigendian: self.is_bigendian,
+            is_bigendian: msg.is_bigendian,
-            point_step: self.point_step,
+            point_step: msg.point_step,
-            row_step: self.row_step,
+            row_step: msg.row_step,
-            data: self.data,
+            data: msg.data,
-            is_dense: self.is_dense,
+            is_dense: msg.is_dense,
         }
     }
 }

src/writer.rs

@@ -1,4 +1,15 @@
-use crate::*;
+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> =
@@ -105,7 +116,7 @@ pub type WriterXYZL = WriterF32<3, 1, PointXYZL>;
 /// ## Example
 /// ```
 /// use ros_pointcloud2::{
-///     reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta,
+///     reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
 /// };
 ///
 /// type Xyz = f32; // coordinate type
@@ -303,3 +314,33 @@ where
         }
     }
 }
+
+#[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)
+}

tests/e2e_test.rs

@@ -65,7 +65,7 @@ fn custom_xyz_f32() {
         }
     }
 
-    impl convert::MetaNames<METADIM> for CustomPoint {
+    impl MetaNames<METADIM> for CustomPoint {
         fn meta_names() -> [&'static str; METADIM] {
             []
         }
@@ -130,7 +130,7 @@ fn custom_xyzi_f32() {
         }
     }
 
-    impl convert::MetaNames<METADIM> for CustomPoint {
+    impl MetaNames<METADIM> for CustomPoint {
         fn meta_names() -> [&'static str; METADIM] {
             ["intensity"]
         }
@@ -215,7 +215,7 @@ fn custom_rgba_f32() {
         }
     }
 
-    impl convert::MetaNames<METADIM> for CustomPoint {
+    impl MetaNames<METADIM> for CustomPoint {
         fn meta_names() -> [&'static str; METADIM] {
             ["r", "g", "b", "a"]
         }
@@ -741,7 +741,7 @@ fn write_less_than_available() {
         }
     }
 
-    impl convert::MetaNames<METADIM> for CustomPoint {
+    impl MetaNames<METADIM> for CustomPoint {
         fn meta_names() -> [&'static str; METADIM] {
             []
         }