Skip to content

Commit

Permalink
merge main
Browse files Browse the repository at this point in the history
  • Loading branch information
stelzo committed May 19, 2024
1 parent f69dfe9 commit d7d51e0
Show file tree
Hide file tree
Showing 6 changed files with 414 additions and 370 deletions.
2 changes: 1 addition & 1 deletion Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ rosrust = { version = "0.9.11", optional = true }
r2r = { version = "0.8.4", optional = true }
rayon = { version = "1", optional = true }
nalgebra = { version = "0.32.5", optional = true, default-features = false }
rpcl2-derive = { version = "0.2.0", optional = true, path = "../rpcl2-derive" }
rpcl2-derive = { version = "0.2.0", optional = true }
memoffset = { version = "0.9", optional = true }

sensor_msgs = { version = "*", optional = true }
Expand Down
25 changes: 13 additions & 12 deletions src/iterator.rs
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
//! Iterator implementations for [`PointCloud2Msg`] including a parallel iterator for rayon.
use crate::{
Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData,
Endian, FieldDatatype, MsgConversionError, PointCloud2Msg, PointConvertible, PointData,
RPCL2Point,
};

Expand All @@ -27,12 +27,12 @@ use alloc::vec::Vec;
///
pub struct PointCloudIterator<const N: usize, C>
where
C: Fields<N>,
C: PointConvertible<N>,
{
iteration: usize,
iteration_back: usize,
data: ByteBufferView<N>,
phantom_c: core::marker::PhantomData<C>, // internally used for pdata names array
_phantom: core::marker::PhantomData<C>,
}

#[cfg(feature = "rayon")]
Expand Down Expand Up @@ -250,7 +250,7 @@ impl<const N: usize> ByteBufferView<N> {

impl<const N: usize, C> TryFrom<PointCloud2Msg> for PointCloudIterator<N, C>
where
C: Fields<N>,
C: PointConvertible<N>,
{
type Error = MsgConversionError;

Expand All @@ -262,8 +262,10 @@ where
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];

let not_found_fieldnames = C::field_names_ordered()
.into_iter()
let fields_only = crate::ordered_field_names::<N, C>();

let not_found_fieldnames = fields_only
.iter()
.map(|name| {
let found = cloud.fields.iter().any(|field| field.name == *name);
(name, found)
Expand All @@ -279,9 +281,8 @@ where
return Err(MsgConversionError::FieldsNotFound(names_not_found));
}

let ordered_fieldnames = C::field_names_ordered();
for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) {
if ordered_fieldnames.contains(&field.name.as_str()) {
if fields_only.contains(&field.name) {
*with_offset = (
field.name.clone(),
field.datatype.try_into()?,
Expand Down Expand Up @@ -342,14 +343,14 @@ where
iteration: 0,
iteration_back: cloud_length - 1,
data,
phantom_c: core::marker::PhantomData,
_phantom: core::marker::PhantomData,
})
}
}

impl<const N: usize, C> PointCloudIterator<N, C>
where
C: Fields<N>,
C: PointConvertible<N>,
{
#[inline]
#[must_use]
Expand All @@ -358,7 +359,7 @@ where
iteration: 0,
iteration_back: data.len() - 1,
data,
phantom_c: core::marker::PhantomData,
_phantom: core::marker::PhantomData,
}
}

Expand Down Expand Up @@ -495,4 +496,4 @@ mod test {
let first_right = right.next();
assert!(first_right.is_none());
}
}
}
Loading

0 comments on commit d7d51e0

Please sign in to comment.