fix subtype iterator mapping

stelzo · Oct 4, 2024 · a02016e · a02016e
1 parent e6b7649
commit a02016e
Show file tree

Hide file tree

Showing 2 changed files with 203 additions and 10 deletions.
diff --git a/src/iterator.rs b/src/iterator.rs
@@ -260,8 +260,6 @@ 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> {
-        let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];
-
         let fields_only = crate::ordered_field_names::<N, C>();
 
         let not_found_fieldnames = fields_only
@@ -281,13 +279,14 @@ where
             return Err(MsgConversionError::FieldsNotFound(names_not_found));
         }
 
-        for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) {
+        let mut pdata_with_offsets = Vec::with_capacity(N);
+        for field in cloud.fields.iter() {
             if fields_only.contains(&field.name) {
-                *with_offset = (
+                pdata_with_offsets.push((
                     field.name.clone(),
                     field.datatype.try_into()?,
                     field.offset as usize,
-                );
+                ));
             }
         }
 

diff --git a/src/lib.rs b/src/lib.rs
@@ -32,17 +32,17 @@
 //!     PointXYZI::new(46.0, 5.47, 0.5, 0.1),
 //! ];
 //! let cloud_copy = cloud_points.clone(); // For equality test later.
-//!     
+//!
 //! let out_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
-//!     
+//!
 //! // Convert to your ROS crate message type.
 //! // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
 //! // Publish ...
 //!
 //! // ... now incoming from a topic.
 //! // let in_msg: PointCloud2Msg = msg.into();
 //! let in_msg = out_msg;
-//!     
+//!
 //! let processed_cloud = in_msg.try_into_iter().unwrap()
 //!     .map(|point: PointXYZ| { // Define the data you want from the point.
 //!         // Some logic here.
@@ -501,7 +501,7 @@ impl PointCloud2Msg {
         let target_layout = KnownLayoutInfo::try_from(C::layout())?;
 
         debug_assert!(field_names.len() <= target_layout.fields.len());
-        debug_assert!(self.fields.len() <= target_layout.fields.len());
+        debug_assert!(self.fields.len() >= target_layout.fields.len());
 
         let mut offset: u32 = 0;
         let mut field_counter = 0;
@@ -1459,4 +1459,198 @@ impl FromBytes for u8 {
     fn from_le_bytes(bytes: PointDataBuffer) -> Self {
         Self::from_le_bytes([bytes[0]])
     }
-}
+}
+
+mod test {
+    #![allow(clippy::unwrap_used)]
+    use crate::prelude::*;
+    #[derive(Debug, Default, Clone, PartialEq)]
+    #[repr(C)]
+    struct PointA {
+        x: f32,
+        y: f32,
+        z: f32,
+        intensity: f32,
+        t: u32,
+        reflectivity: u16,
+        ring: u16,
+        ambient: u16,
+        range: u32,
+    }
+    impl From<RPCL2Point<9>> for PointA {
+        fn from(point: RPCL2Point<9>) -> Self {
+            Self::new(point[0].get(), point[1].get(), point[2].get())
+        }
+    }
+    impl From<PointA> for RPCL2Point<9> {
+        fn from(point: PointA) -> Self {
+            [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.intensity.into(),
+                point.t.into(),
+                point.reflectivity.into(),
+                point.ring.into(),
+                point.ambient.into(),
+                point.range.into(),
+            ]
+            .into()
+        }
+    }
+    unsafe impl PointConvertible<9> for PointA {
+        fn layout() -> LayoutDescription {
+            LayoutDescription::new(&[
+                LayoutField::new("x", "f32", 4),
+                LayoutField::new("y", "f32", 4),
+                LayoutField::new("z", "f32", 4),
+                LayoutField::new("intensity", "f32", 4),
+                LayoutField::new("t", "u32", 4),
+                LayoutField::new("reflectivity", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("ring", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("ambient", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("range", "u32", 4),
+            ])
+        }
+    }
+    impl PointA {
+        fn new(x: f32, y: f32, z: f32) -> Self {
+            Self {
+                x,
+                y,
+                z,
+                intensity: 0.0,
+                t: 0,
+                reflectivity: 0,
+                ring: 0,
+                ambient: 0,
+                range: 0,
+            }
+        }
+    }
+    #[derive(Debug, Clone, Default, PartialEq)]
+    #[repr(C)]
+    struct PointB {
+        pub x: f32,
+        pub y: f32,
+        pub z: f32,
+        pub t: u32,
+    }
+    impl PointB {
+        fn new(x: f32, y: f32, z: f32) -> Self {
+            Self { x, y, z, t: 0 }
+        }
+    }
+    impl From<RPCL2Point<4>> for PointB {
+        fn from(point: RPCL2Point<4>) -> Self {
+            Self::new(point[0].get(), point[1].get(), point[2].get())
+        }
+    }
+    impl From<PointB> for RPCL2Point<4> {
+        fn from(point: PointB) -> Self {
+            [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.t.into(),
+            ]
+            .into()
+        }
+    }
+    unsafe impl PointConvertible<4> for PointB {
+        fn layout() -> LayoutDescription {
+            LayoutDescription::new(&[
+                LayoutField::new("x", "f32", 4),
+                LayoutField::new("y", "f32", 4),
+                LayoutField::new("z", "f32", 4),
+                LayoutField::new("t", "u32", 4),
+            ])
+        }
+    }
+    #[derive(Debug, Clone, Default, PartialEq)]
+    #[repr(C)]
+    struct PointD {
+        x: f32,
+        y: f32,
+        z: f32,
+        t: u32,
+        ring: u16,
+        range: u32,
+        signal: u16,
+        reflectivity: u16,
+        near_ir: u16,
+    }
+    impl From<RPCL2Point<9>> for PointD {
+        fn from(point: RPCL2Point<9>) -> Self {
+            Self::new(point[0].get(), point[1].get(), point[2].get())
+        }
+    }
+    impl From<PointD> for RPCL2Point<9> {
+        fn from(point: PointD) -> Self {
+            [
+                point.x.into(),
+                point.y.into(),
+                point.z.into(),
+                point.t.into(),
+                point.ring.into(),
+                point.range.into(),
+                point.signal.into(),
+                point.reflectivity.into(),
+                point.near_ir.into(),
+            ]
+            .into()
+        }
+    }
+    unsafe impl PointConvertible<9> for PointD {
+        fn layout() -> LayoutDescription {
+            LayoutDescription::new(&[
+                LayoutField::new("x", "f32", 4),
+                LayoutField::new("y", "f32", 4),
+                LayoutField::new("z", "f32", 4),
+                LayoutField::new("t", "u32", 4),
+                LayoutField::new("ring", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("range", "u32", 4),
+                LayoutField::new("signal", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("reflectivity", "u16", 2),
+                LayoutField::padding(2),
+                LayoutField::new("near_ir", "u16", 2),
+                LayoutField::padding(2),
+            ])
+        }
+    }
+    impl PointD {
+        fn new(x: f32, y: f32, z: f32) -> Self {
+            Self {
+                x,
+                y,
+                z,
+                t: 0,
+                ring: 0,
+                range: 0,
+                signal: 0,
+                reflectivity: 0,
+                near_ir: 0,
+            }
+        }
+    }
+    #[test]
+    fn subtype_iterator_fallback() {
+        let cloud_a = PointCloud2Msg::try_from_iter(vec![
+            PointA::new(1.0, 2.0, 3.0),
+            PointA::new(4.0, 5.0, 6.0),
+            PointA::new(7.0, 8.0, 9.0),
+        ])
+        .unwrap();
+        let cloud_c: PointB = cloud_a.clone().try_into_iter().unwrap().next().unwrap();
+        assert_eq!(cloud_c, PointB::new(1.0, 2.0, 3.0));
+        let cloud_b: Vec<PointB> = cloud_a.try_into_vec().unwrap();
+        assert_eq!(cloud_b[0], PointB::new(1.0, 2.0, 3.0));
+        assert_eq!(cloud_b[1], PointB::new(4.0, 5.0, 6.0));
+        assert_eq!(cloud_b[2], PointB::new(7.0, 8.0, 9.0));
+    }
+}