diff --git a/src/iterator.rs b/src/iterator.rs index 65f7c9c..4c47fbc 100644 --- a/src/iterator.rs +++ b/src/iterator.rs @@ -28,7 +28,7 @@ where iteration: usize, iteration_back: usize, data: ByteBufferView, - phantom_c: std::marker::PhantomData, // internally used for meta names array + phantom_c: std::marker::PhantomData, // internally used for pdata names array } #[cfg(feature = "rayon")] @@ -133,7 +133,6 @@ where } } -/// Implementation of the iterator trait. impl Iterator for PointCloudIterator where C: PointConvertible, @@ -162,7 +161,7 @@ struct ByteBufferView { end_point_idx: usize, point_step_size: usize, offsets: [usize; N], - meta: Vec<(String, FieldDatatype)>, + pdata: Vec<(String, FieldDatatype)>, endian: Endian, } @@ -173,7 +172,7 @@ impl ByteBufferView { start_point_idx: usize, end_point_idx: usize, offsets: [usize; N], - meta: Vec<(String, FieldDatatype)>, + pdata: Vec<(String, FieldDatatype)>, endian: Endian, ) -> Self { Self { @@ -182,7 +181,7 @@ impl ByteBufferView { end_point_idx, point_step_size, offsets, - meta, + pdata, endian, } } @@ -195,22 +194,21 @@ impl ByteBufferView { #[inline(always)] fn point_at(&self, idx: usize) -> RPCL2Point { let offset = (self.start_point_idx + idx) * self.point_step_size; - - // TODO memcpy entire point at once, then extract fields? - let mut meta = [PointData::default(); N]; - meta.iter_mut() + let mut pdata = [PointData::default(); N]; + pdata + .iter_mut() .zip(self.offsets.iter()) - .zip(self.meta.iter()) - .for_each(|((p_meta, in_point_offset), (_, meta_type))| { - *p_meta = PointData::from_buffer( + .zip(self.pdata.iter()) + .for_each(|((pdata_entry, in_point_offset), (_, pdata_type))| { + *pdata_entry = PointData::from_buffer( &self.data, offset + in_point_offset, - *meta_type, + *pdata_type, self.endian, ); }); - RPCL2Point { fields: meta } + pdata.into() } #[inline] @@ -221,7 +219,7 @@ impl ByteBufferView { end_point_idx: start + size - 1, point_step_size: self.point_step_size, offsets: self.offsets, - meta: self.meta.clone(), + pdata: self.pdata.clone(), endian: self.endian, } } @@ -231,9 +229,8 @@ impl ByteBufferView { let left_start = self.start_point_idx; let left_size = point_index; - let right_start = point_index; + let right_start = self.start_point_idx + 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), @@ -248,12 +245,12 @@ where 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. + /// Converting a PointCloud2Msg into an iterator is a fallible operation since the message could contain 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 { - let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N]; + let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N]; let not_found_fieldnames = C::field_names_ordered() .into_iter() @@ -273,7 +270,7 @@ where } let ordered_fieldnames = C::field_names_ordered(); - for (field, with_offset) in cloud.fields.iter().zip(meta_with_offsets.iter_mut()) { + for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) { if ordered_fieldnames.contains(&field.name.as_str()) { *with_offset = ( field.name.clone(), @@ -283,25 +280,26 @@ where } } - meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2)); + pdata_with_offsets + .sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2)); debug_assert!( - meta_with_offsets.len() == N, + pdata_with_offsets.len() == N, "Not all fields were found in the message. Expected {} but found {}.", N, - meta_with_offsets.len() + pdata_with_offsets.len() ); let mut offsets = [usize::default(); N]; - let mut meta = vec![(String::default(), FieldDatatype::default()); N]; + let mut pdata = vec![(String::default(), FieldDatatype::default()); N]; - meta_with_offsets + pdata_with_offsets .into_iter() - .zip(meta.iter_mut()) + .zip(pdata.iter_mut()) .zip(offsets.iter_mut()) - .for_each(|(((name, datatype, offset), meta), meta_offset)| { - *meta = (name, datatype); - *meta_offset = offset; + .for_each(|(((name, datatype, offset), pdata), pdata_offset)| { + *pdata = (name, datatype); + *pdata_offset = offset; }); let point_step_size = cloud.point_step as usize; @@ -312,9 +310,9 @@ where let last_offset = offsets.last().expect("Dimensionality is 0."); - 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 { + let last_pdata = pdata.last().expect("Dimensionality is 0."); + let size_with_last_pdata = last_offset + last_pdata.1.size(); + if size_with_last_pdata > point_step_size { return Err(MsgConversionError::DataLengthMismatch); } @@ -326,7 +324,7 @@ where 0, cloud_length - 1, offsets, - meta, + pdata, cloud.endian, ); diff --git a/src/lib.rs b/src/lib.rs index 90a7bd9..92c32a2 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -72,7 +72,7 @@ //! pub x: f32, //! pub y: f32, //! pub z: f32, -//! #[cfg_attr(feature = "derive", rpcl2(name = "i"))] +//! #[cfg_attr(feature = "derive", rpcl2(rename("i")))] //! pub intensity: f32, //! } //! @@ -121,7 +121,7 @@ //! pub x: f32, //! pub y: f32, //! pub z: f32, -//! #[rpcl2(name = "i")] +//! #[rpcl2(rename("i"))] //! pub intensity: f32, //! } //! ``` @@ -457,33 +457,33 @@ impl PointCloud2Msg { let field_names = C::field_names_ordered(); debug_assert!(field_names.len() == N); - let mut meta_offsets_acc: u32 = 0; + let mut pdata_offsets_acc: u32 = 0; let mut fields = vec![PointFieldMsg::default(); N]; let field_count: u32 = 1; - for ((meta_value, field_name), field_val) in point + for ((pdata_entry, field_name), field_val) in point .fields .into_iter() .zip(field_names.into_iter()) .zip(fields.iter_mut()) { - let datatype_code = meta_value.datatype.into(); + let datatype_code = pdata_entry.datatype.into(); let _ = FieldDatatype::try_from(datatype_code)?; *field_val = PointFieldMsg { name: field_name.into(), - offset: meta_offsets_acc, + offset: pdata_offsets_acc, datatype: datatype_code, count: 1, }; - meta_offsets_acc += field_count * meta_value.datatype.size() as u32; + pdata_offsets_acc += field_count * pdata_entry.datatype.size() as u32; } ( PointCloud2MsgBuilder::new() .fields(fields) - .point_step(meta_offsets_acc), - meta_offsets_acc, + .point_step(pdata_offsets_acc), + pdata_offsets_acc, ) }; let mut cloud_width = 0; @@ -491,9 +491,9 @@ impl PointCloud2Msg { iterable.into_iter().for_each(|pointdata| { let point: RPCL2Point = pointdata.into(); - point.fields.iter().for_each(|meta| { + point.fields.iter().for_each(|pdata| { let truncated_bytes = unsafe { - std::slice::from_raw_parts(meta.bytes.as_ptr(), meta.datatype.size()) + std::slice::from_raw_parts(pdata.bytes.as_ptr(), pdata.datatype.size()) }; cloud.data.extend_from_slice(truncated_bytes); }); @@ -706,7 +706,7 @@ impl PointCloud2Msg { } } -/// Internal point representation. It is used to store the coordinates and meta data of a point. +/// Internal point representation. It is used to store the point data entries. /// /// 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. @@ -779,7 +779,7 @@ impl From<[PointData; N]> for RPCL2Point { /// ``` #[cfg(not(feature = "derive"))] pub trait PointConvertible: - From> + Into> + Fields + Clone + 'static + Default + From> + Into> + Fields + Clone + Default { } @@ -840,7 +840,7 @@ pub trait PointConvertible: /// ``` #[cfg(feature = "derive")] pub trait PointConvertible: - type_layout::TypeLayout + From> + Into> + Fields + 'static + Default + type_layout::TypeLayout + From> + Into> + Fields + Default { } @@ -926,8 +926,8 @@ impl TryFrom for TypeLayoutInfo { /// use ros_pointcloud2::PointData; /// /// let original_data: f64 = 1.0; -/// let meta = PointData::new(original_data); -/// let my_data: f64 = meta.get(); +/// let pdata = PointData::new(original_data); +/// let my_data: f64 = pdata.get(); /// ``` #[derive(Debug, Clone, Copy)] pub struct PointData { @@ -951,7 +951,7 @@ impl PointData { /// /// # Example /// ``` - /// let meta = ros_pointcloud2::PointData::new(1.0); + /// let pdata = ros_pointcloud2::PointData::new(1.0); /// ``` #[inline(always)] pub fn new(value: T) -> Self { @@ -984,8 +984,8 @@ impl PointData { /// # Example /// ``` /// let original_data: f64 = 1.0; - /// let meta = ros_pointcloud2::PointData::new(original_data); - /// let my_data: f64 = meta.get(); + /// let pdata = ros_pointcloud2::PointData::new(original_data); + /// let my_data: f64 = pdata.get(); /// ``` pub fn get(&self) -> T { match self.endian { @@ -1408,7 +1408,7 @@ mod tests { #[derive(Fields)] struct TestStruct { field1: String, - #[rpcl2(name = "renamed_field")] + #[rpcl2(rename("renamed_field"))] field2: i32, field3: f64, field4: bool, diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index 3606bbd..69fb781 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -70,9 +70,6 @@ fn write_cloud_from_vec() { let msg = PointCloud2Msg::try_from_vec(cloud); assert!(msg.is_ok()); - - let msg = msg.unwrap(); - println!("{:?}", msg); } #[test] @@ -92,6 +89,47 @@ fn write_empty_cloud_iter() { assert!(msg.unwrap().data.is_empty()); } +#[test] +#[cfg(all(feature = "derive", feature = "rayon"))] +fn conv_cloud_par_iter() { + let cloud = vec![ + PointXYZ::new(0.0, 1.0, 5.0), + PointXYZ::new(1.0, 1.5, 5.0), + PointXYZ::new(1.3, 1.6, 5.7), + ]; + let copy = cloud.clone(); + + let msg: Result = PointCloud2Msg::try_from_vec(cloud); + assert!(msg.is_ok()); + let msg = msg.unwrap(); + let to_p_type = msg.try_into_par_iter(); + assert!(to_p_type.is_ok()); + let to_p_type = to_p_type.unwrap(); + let back_to_type = to_p_type.collect::>(); + assert_eq!(copy, back_to_type); +} + +#[test] +#[cfg(all(feature = "derive", feature = "rayon"))] +fn conv_cloud_par_par_iter() { + let cloud = vec![ + 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 copy = cloud.clone(); + + let msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter()); + assert!(msg.is_ok()); + let msg = msg.unwrap(); + let to_p_type = msg.try_into_par_iter(); + assert!(to_p_type.is_ok()); + let to_p_type = to_p_type.unwrap(); + let back_to_type = to_p_type.collect::>(); + assert_eq!(copy, back_to_type); +} + #[test] #[cfg(feature = "derive")] fn custom_xyz_f32() {