merge main
This commit is contained in:
stelzo
parent
f69dfe93f2
commit
d7d51e0cc2
|
|
@ -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 }
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
};
|
||||
|
||||
|
|
@ -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")]
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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()?,
|
||||
|
|
@ -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]
|
||||
|
|
@ -358,7 +359,7 @@ where
|
|||
iteration: 0,
|
||||
iteration_back: data.len() - 1,
|
||||
data,
|
||||
phantom_c: core::marker::PhantomData,
|
||||
_phantom: core::marker::PhantomData,
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -495,4 +496,4 @@ mod test {
|
|||
let first_right = right.next();
|
||||
assert!(first_right.is_none());
|
||||
}
|
||||
}
|
||||
}
|
||||
384
src/lib.rs
384
src/lib.rs
|
|
@ -3,8 +3,8 @@
|
|||
//! The library provides the [`PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators.
|
||||
//!
|
||||
//! Vector conversions are optimized for direct copy. They are useful when you just move similar data around. They are usually a good default.
|
||||
//! - [`try_from_vec`](PointCloud2Msg::try_from_vec) requires `derive` feature (enabled by default)
|
||||
//! - [`try_into_vec`](PointCloud2Msg::try_into_vec) requires `derive` feature (enabled by default)
|
||||
//! - [`try_from_vec`](PointCloud2Msg::try_from_vec)
|
||||
//! - [`try_into_vec`](PointCloud2Msg::try_into_vec)
|
||||
//!
|
||||
//! You can use the iterator functions for more control over the conversion process.
|
||||
//! - [`try_from_iter`](PointCloud2Msg::try_from_iter)
|
||||
|
|
@ -64,16 +64,27 @@
|
|||
//! # Custom Points
|
||||
//! Implement [`PointConvertible`] for your point with the `derive` feature or manually.
|
||||
//!
|
||||
//! ## Derive (recommended)
|
||||
//! ```ignore
|
||||
//! #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible)]
|
||||
//! pub struct MyPointXYZI {
|
||||
//! pub x: f32,
|
||||
//! pub y: f32,
|
||||
//! pub z: f32,
|
||||
//! #[rpcl2(rename("i"))]
|
||||
//! pub intensity: f32,
|
||||
//! }
|
||||
//! ```
|
||||
//!
|
||||
//! ## Manual
|
||||
//! ```
|
||||
//! use ros_pointcloud2::prelude::*;
|
||||
//!
|
||||
//! #[cfg_attr(feature = "derive", derive(PointConvertible, TypeLayout))]
|
||||
//! #[derive(Clone, Debug, PartialEq, Copy, Default)]
|
||||
//! pub struct MyPointXYZI {
|
||||
//! pub x: f32,
|
||||
//! pub y: f32,
|
||||
//! pub z: f32,
|
||||
//! #[cfg_attr(feature = "derive", rpcl2(rename("i")))]
|
||||
//! pub intensity: f32,
|
||||
//! }
|
||||
//!
|
||||
|
|
@ -83,30 +94,28 @@
|
|||
//! }
|
||||
//! }
|
||||
//!
|
||||
//! // Manual implementation of PointConvertible without derive.
|
||||
//! #[cfg(not(feature = "derive"))]
|
||||
//! impl Fields<4> for MyPointXYZI {
|
||||
//! fn field_names_ordered() -> [&'static str; 4] {
|
||||
//! ["x", "y", "z", "i"] // Note the different field name for intensity.
|
||||
//! }
|
||||
//! }
|
||||
//!
|
||||
//! #[cfg(not(feature = "derive"))]
|
||||
//! impl From<RPCL2Point<4>> for MyPointXYZI {
|
||||
//! fn from(point: RPCL2Point<4>) -> Self {
|
||||
//! Self::new(point[0].get(), point[1].get(), point[2].get(), point[3].get())
|
||||
//! }
|
||||
//! }
|
||||
//!
|
||||
//! #[cfg(not(feature = "derive"))]
|
||||
//! impl From<MyPointXYZI> for RPCL2Point<4> {
|
||||
//! fn from(point: MyPointXYZI) -> Self {
|
||||
//! [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
|
||||
//! }
|
||||
//! }
|
||||
//!
|
||||
//! #[cfg(not(feature = "derive"))]
|
||||
//! impl PointConvertible<4> for MyPointXYZI {}
|
||||
//! impl PointConvertible<4> for MyPointXYZI {
|
||||
//! 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),
|
||||
//! ])
|
||||
//! }
|
||||
//! }
|
||||
//!
|
||||
//! let first_p = MyPointXYZI::new(1.0, 2.0, 3.0, 0.5);
|
||||
//! let cloud_points = vec![first_p, MyPointXYZI::new(4.0, 5.0, 6.0, 0.5)];
|
||||
|
|
@ -114,21 +123,9 @@
|
|||
//! let cloud_points_out: Vec<MyPointXYZI> = msg_out.try_into_iter().unwrap().collect();
|
||||
//! assert_eq!(first_p, *cloud_points_out.first().unwrap());
|
||||
//! ```
|
||||
//!
|
||||
//! An example without `#[cfg_attr]` looks like this:
|
||||
//! ```ignore
|
||||
//! #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)]
|
||||
//! pub struct MyPointXYZI {
|
||||
//! pub x: f32,
|
||||
//! pub y: f32,
|
||||
//! pub z: f32,
|
||||
//! #[rpcl2(rename("i"))]
|
||||
//! pub intensity: f32,
|
||||
//! }
|
||||
//! ```
|
||||
#![crate_type = "lib"]
|
||||
#![cfg_attr(docsrs, feature(doc_cfg))]
|
||||
#![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.0-rc.1")]
|
||||
#![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.0-rc.2")]
|
||||
#![warn(clippy::print_stderr)]
|
||||
#![warn(clippy::print_stdout)]
|
||||
#![warn(clippy::unwrap_used)]
|
||||
|
|
@ -139,14 +136,6 @@
|
|||
#![cfg_attr(not(feature = "std"), no_std)]
|
||||
// Setup an allocator with #[global_allocator]
|
||||
// see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html
|
||||
#![warn(clippy::unwrap_used)]
|
||||
#![warn(clippy::cargo)]
|
||||
#![warn(clippy::std_instead_of_core)]
|
||||
#![warn(clippy::alloc_instead_of_core)]
|
||||
#![warn(clippy::std_instead_of_alloc)]
|
||||
#![cfg_attr(not(feature = "std"), no_std)]
|
||||
// Setup an allocator with #[global_allocator]
|
||||
// see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html
|
||||
|
||||
pub mod points;
|
||||
pub mod prelude;
|
||||
|
|
@ -157,6 +146,9 @@ pub mod iterator;
|
|||
use crate::ros::{HeaderMsg, PointFieldMsg};
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
#[doc(hidden)]
|
||||
pub use memoffset;
|
||||
|
||||
use core::str::FromStr;
|
||||
|
||||
#[macro_use]
|
||||
|
|
@ -230,7 +222,6 @@ impl std::error::Error for MsgConversionError {
|
|||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
fn system_endian() -> Endian {
|
||||
if cfg!(target_endian = "big") {
|
||||
Endian::Big
|
||||
|
|
@ -241,6 +232,43 @@ fn system_endian() -> Endian {
|
|||
}
|
||||
}
|
||||
|
||||
/// Description of the memory layout of a type with named fields.
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct LayoutDescription(Vec<LayoutField>);
|
||||
|
||||
impl LayoutDescription {
|
||||
pub fn new(fields: &[LayoutField]) -> Self {
|
||||
Self(fields.into())
|
||||
}
|
||||
}
|
||||
|
||||
/// Enum to describe the field type and size in a padded or unpadded layout.
|
||||
#[derive(Clone, Debug)]
|
||||
pub enum LayoutField {
|
||||
Field {
|
||||
name: alloc::borrow::Cow<'static, str>,
|
||||
ty: alloc::borrow::Cow<'static, str>,
|
||||
size: usize,
|
||||
},
|
||||
Padding {
|
||||
size: usize,
|
||||
},
|
||||
}
|
||||
|
||||
impl LayoutField {
|
||||
pub fn new(name: &'static str, ty: &'static str, size: usize) -> Self {
|
||||
LayoutField::Field {
|
||||
name: name.into(),
|
||||
ty: ty.into(),
|
||||
size,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn padding(size: usize) -> Self {
|
||||
LayoutField::Padding { size }
|
||||
}
|
||||
}
|
||||
|
||||
/// The intermediate point cloud type for ROS integrations.
|
||||
///
|
||||
/// To assert consistency, the type should be build with the [`PointCloud2MsgBuilder`].
|
||||
|
|
@ -273,7 +301,6 @@ pub enum Denseness {
|
|||
Sparse,
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
enum ByteSimilarity {
|
||||
Equal,
|
||||
Overlapping,
|
||||
|
|
@ -428,8 +455,32 @@ pub struct CloudDimensions {
|
|||
pub height: u32,
|
||||
}
|
||||
|
||||
fn ordered_field_names<const N: usize, C: PointConvertible<N>>() -> Vec<String> {
|
||||
C::layout()
|
||||
.0
|
||||
.iter()
|
||||
.filter(|field| {
|
||||
matches!(
|
||||
field,
|
||||
LayoutField::Field {
|
||||
name: _,
|
||||
ty: _,
|
||||
size: _,
|
||||
}
|
||||
)
|
||||
})
|
||||
.map(|field| match field {
|
||||
LayoutField::Field {
|
||||
name,
|
||||
ty: _,
|
||||
size: _,
|
||||
} => name.to_string(),
|
||||
_ => unreachable!("Fields must be filtered before."),
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
impl PointCloud2Msg {
|
||||
#[cfg(feature = "derive")]
|
||||
#[inline]
|
||||
fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError>
|
||||
where
|
||||
|
|
@ -438,26 +489,26 @@ impl PointCloud2Msg {
|
|||
let point: RPCL2Point<N> = C::default().into();
|
||||
debug_assert!(point.fields.len() == N);
|
||||
|
||||
let field_names = C::field_names_ordered();
|
||||
let field_names = ordered_field_names::<N, C>();
|
||||
debug_assert!(field_names.len() == N);
|
||||
|
||||
let layout = TypeLayoutInfo::try_from(C::type_layout())?;
|
||||
debug_assert!(field_names.len() == layout.fields.len());
|
||||
let layout = KnownLayoutInfo::try_from(C::layout())?;
|
||||
debug_assert!(field_names.len() <= layout.fields.len());
|
||||
|
||||
let mut offset: u32 = 0;
|
||||
let mut field_counter = 0;
|
||||
for (f, msg_f) in layout.fields.iter().zip(self.fields.iter()) {
|
||||
for f in layout.fields.iter() {
|
||||
match f {
|
||||
PointField::Field {
|
||||
datatype,
|
||||
size,
|
||||
count,
|
||||
} => {
|
||||
let f_translated = String::from_str(field_names[field_counter])
|
||||
.expect("Field name is not a valid string.");
|
||||
let msg_f = &self.fields[field_counter];
|
||||
let f_translated = &field_names[field_counter];
|
||||
field_counter += 1;
|
||||
|
||||
if msg_f.name != f_translated
|
||||
if msg_f.name != *f_translated
|
||||
|| msg_f.offset != offset
|
||||
|| msg_f.datatype != *datatype
|
||||
|| msg_f.count != 1
|
||||
|
|
@ -503,7 +554,7 @@ impl PointCloud2Msg {
|
|||
let point: RPCL2Point<N> = C::default().into();
|
||||
debug_assert!(point.fields.len() == N);
|
||||
|
||||
let field_names = C::field_names_ordered();
|
||||
let field_names = crate::ordered_field_names::<N, C>();
|
||||
debug_assert!(field_names.len() == N);
|
||||
|
||||
let mut pdata_offsets_acc: u32 = 0;
|
||||
|
|
@ -519,7 +570,7 @@ impl PointCloud2Msg {
|
|||
let _ = FieldDatatype::try_from(datatype_code)?;
|
||||
|
||||
*field_val = PointFieldMsg {
|
||||
name: field_name.into(),
|
||||
name: field_name,
|
||||
offset: pdata_offsets_acc,
|
||||
datatype: datatype_code,
|
||||
count: 1,
|
||||
|
|
@ -557,8 +608,8 @@ impl PointCloud2Msg {
|
|||
}
|
||||
|
||||
/// Create a PointCloud2Msg from a parallel iterator. Requires the `rayon` and `derive` feature to be enabled.
|
||||
#[cfg(all(feature = "rayon", feature = "derive"))]
|
||||
#[cfg_attr(docsrs, doc(cfg(all(feature = "rayon", feature = "derive"))))]
|
||||
#[cfg(feature = "rayon")]
|
||||
#[cfg_attr(docsrs, doc(cfg(feature = "rayon")))]
|
||||
pub fn try_from_par_iter<const N: usize, C>(
|
||||
iterable: impl rayon::iter::ParallelIterator<Item = C>,
|
||||
) -> Result<Self, MsgConversionError>
|
||||
|
|
@ -585,8 +636,6 @@ impl PointCloud2Msg {
|
|||
///
|
||||
/// # Errors
|
||||
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
|
||||
#[cfg(feature = "derive")]
|
||||
#[cfg_attr(docsrs, doc(cfg(feature = "derive")))]
|
||||
pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
|
|
@ -597,17 +646,15 @@ impl PointCloud2Msg {
|
|||
let point: RPCL2Point<N> = C::default().into();
|
||||
debug_assert!(point.fields.len() == N);
|
||||
|
||||
let field_names = C::field_names_ordered();
|
||||
let field_names = crate::ordered_field_names::<N, C>();
|
||||
debug_assert!(field_names.len() == N);
|
||||
|
||||
let layout = TypeLayoutInfo::try_from(C::type_layout())?;
|
||||
debug_assert!(field_names.len() == layout.fields.len());
|
||||
let layout = KnownLayoutInfo::try_from(C::layout())?;
|
||||
debug_assert!(field_names.len() <= layout.fields.len());
|
||||
|
||||
let mut offset = 0;
|
||||
let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(layout.fields.len());
|
||||
let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(field_names.len());
|
||||
for f in layout.fields.into_iter() {
|
||||
let f_translated = String::from_str(field_names[fields.len()])
|
||||
.expect("Field name is not a valid string.");
|
||||
match f {
|
||||
PointField::Field {
|
||||
datatype,
|
||||
|
|
@ -615,7 +662,7 @@ impl PointCloud2Msg {
|
|||
count,
|
||||
} => {
|
||||
fields.push(PointFieldMsg {
|
||||
name: f_translated,
|
||||
name: field_names[fields.len()].clone(),
|
||||
offset,
|
||||
datatype,
|
||||
..Default::default()
|
||||
|
|
@ -674,8 +721,6 @@ impl PointCloud2Msg {
|
|||
///
|
||||
/// # Errors
|
||||
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
|
||||
#[cfg(feature = "derive")]
|
||||
#[cfg_attr(docsrs, doc(cfg(feature = "derive")))]
|
||||
pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
|
|
@ -778,14 +823,6 @@ pub struct RPCL2Point<const N: usize> {
|
|||
fields: [PointData; N],
|
||||
}
|
||||
|
||||
impl<const N: usize> Default for RPCL2Point<N> {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
fields: [PointData::default(); N],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<const N: usize> core::ops::Index<usize> for RPCL2Point<N> {
|
||||
type Output = PointData;
|
||||
|
||||
|
|
@ -802,157 +839,97 @@ impl<const N: usize> From<[PointData; N]> for RPCL2Point<N> {
|
|||
|
||||
/// Trait to enable point conversions on the fly.
|
||||
///
|
||||
/// # Example
|
||||
/// Implement this trait for your custom point you want to read or write in the message.
|
||||
/// It is strongly recommended to enable the `derive` feature and use the `#[derive(PointConvertible)]` macro.
|
||||
/// This prevents common errors when implementing this trait by hand.
|
||||
///
|
||||
/// Be aware that Rust does not guarantee the memory layout of structs. Learn more [here](https://doc.rust-lang.org/reference/type-layout.html).
|
||||
/// To make layouting more predictable and thus faster for C++ node interactions, use the `#[repr(C)]` attribute on your struct.
|
||||
/// An example for diverging point layouts with padding can be seen in the source code of [this](points::PointXYZRGBA::layout) implementation.
|
||||
///
|
||||
/// The generic parameter `N` is the number of fields in the point type. There can be more (hidden) fields that pad the layout but they do not count for the N.
|
||||
/// For
|
||||
///
|
||||
/// # Derive
|
||||
/// ```ignore
|
||||
/// use ros_pointcloud2::prelude::*;
|
||||
///
|
||||
/// #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible)]
|
||||
/// #[repr(C, align(4))]
|
||||
/// pub struct MyPointXYZL {
|
||||
/// pub x: f32,
|
||||
/// pub y: f32,
|
||||
/// pub z: f32,
|
||||
/// #[rpcl2(rename("l"))]
|
||||
/// pub label: u8,
|
||||
/// }
|
||||
/// ```
|
||||
///
|
||||
/// # Manual
|
||||
/// ```
|
||||
/// use ros_pointcloud2::prelude::*;
|
||||
///
|
||||
/// #[derive(Clone, Debug, PartialEq, Copy, Default)]
|
||||
/// pub struct MyPointXYZI {
|
||||
/// #[repr(C, align(4))]
|
||||
/// pub struct MyPointXYZL {
|
||||
/// pub x: f32,
|
||||
/// pub y: f32,
|
||||
/// pub z: f32,
|
||||
/// pub intensity: f32,
|
||||
/// pub label: u8,
|
||||
/// }
|
||||
///
|
||||
/// impl From<MyPointXYZI> for RPCL2Point<4> {
|
||||
/// fn from(point: MyPointXYZI) -> Self {
|
||||
/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
|
||||
/// impl From<MyPointXYZL> for RPCL2Point<4> {
|
||||
/// fn from(point: MyPointXYZL) -> Self {
|
||||
/// [point.x.into(), point.y.into(), point.z.into(), point.label.into()].into()
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl From<RPCL2Point<4>> for MyPointXYZI {
|
||||
/// impl From<RPCL2Point<4>> for MyPointXYZL {
|
||||
/// fn from(point: RPCL2Point<4>) -> Self {
|
||||
/// Self {
|
||||
/// x: point[0].get(),
|
||||
/// y: point[1].get(),
|
||||
/// z: point[2].get(),
|
||||
/// intensity: point[3].get(),
|
||||
/// label: point[3].get(),
|
||||
/// }
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl Fields<4> for MyPointXYZI {
|
||||
/// fn field_names_ordered() -> [&'static str; 4] {
|
||||
/// ["x", "y", "z", "intensity"]
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl PointConvertible<4> for MyPointXYZI {}
|
||||
/// ```
|
||||
#[cfg_attr(docsrs, doc(cfg(not(feature = "derive"))))]
|
||||
#[cfg(not(feature = "derive"))]
|
||||
pub trait PointConvertible<const N: usize>:
|
||||
From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Clone + Default
|
||||
{
|
||||
}
|
||||
|
||||
/// Trait to enable point conversions on the fly.
|
||||
///
|
||||
/// Implement this trait for your custom point you want to read or write in the message.
|
||||
/// For a more convenient way to implement this trait, enable the `derive` feature and use the `#[derive(PointConvertible, TypeLayout)]` macro.
|
||||
///
|
||||
/// # Derive Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::prelude::*;
|
||||
///
|
||||
/// #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)]
|
||||
/// pub struct MyPointXYZI {
|
||||
/// pub x: f32,
|
||||
/// pub y: f32,
|
||||
/// pub z: f32,
|
||||
/// pub intensity: f32,
|
||||
/// }
|
||||
/// ```
|
||||
///
|
||||
/// # Manual Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::prelude::*;
|
||||
///
|
||||
/// #[derive(Clone, Debug, PartialEq, Copy, Default, TypeLayout)]
|
||||
/// pub struct MyPointXYZI {
|
||||
/// pub x: f32,
|
||||
/// pub y: f32,
|
||||
/// pub z: f32,
|
||||
/// pub intensity: f32,
|
||||
/// }
|
||||
///
|
||||
/// impl From<MyPointXYZI> for RPCL2Point<4> {
|
||||
/// fn from(point: MyPointXYZI) -> Self {
|
||||
/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
|
||||
/// impl PointConvertible<4> for MyPointXYZL {
|
||||
/// fn layout() -> LayoutDescription {
|
||||
/// LayoutDescription::new(&[
|
||||
/// LayoutField::new("x", "f32", 4),
|
||||
/// LayoutField::new("y", "f32", 4),
|
||||
/// LayoutField::new("z", "f32", 4),
|
||||
/// LayoutField::new("l", "u8", 1),
|
||||
/// LayoutField::padding(3),
|
||||
/// ])
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl From<RPCL2Point<4>> for MyPointXYZI {
|
||||
/// fn from(point: RPCL2Point<4>) -> Self {
|
||||
/// Self {
|
||||
/// x: point[0].get(),
|
||||
/// y: point[1].get(),
|
||||
/// z: point[2].get(),
|
||||
/// intensity: point[3].get(),
|
||||
/// }
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl Fields<4> for MyPointXYZI {
|
||||
/// fn field_names_ordered() -> [&'static str; 4] {
|
||||
/// ["x", "y", "z", "intensity"]
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl PointConvertible<4> for MyPointXYZI {}
|
||||
/// ```
|
||||
#[cfg_attr(docsrs, doc(cfg(feature = "derive")))]
|
||||
#[cfg(feature = "derive")]
|
||||
pub trait PointConvertible<const N: usize>:
|
||||
type_layout::TypeLayout + From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Default
|
||||
From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Default + Sized
|
||||
{
|
||||
fn layout() -> LayoutDescription;
|
||||
}
|
||||
|
||||
/// 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::prelude::*;
|
||||
///
|
||||
/// #[derive(Clone, Debug, PartialEq, Copy)]
|
||||
/// pub struct MyPointXYZI {
|
||||
/// pub x: f32,
|
||||
/// pub y: f32,
|
||||
/// pub z: f32,
|
||||
/// pub intensity: f32,
|
||||
/// }
|
||||
///
|
||||
/// impl Fields<4> for MyPointXYZI {
|
||||
/// fn field_names_ordered() -> [&'static str; 4] {
|
||||
/// ["x", "y", "z", "intensity"]
|
||||
/// }
|
||||
/// }
|
||||
/// ```
|
||||
pub trait Fields<const N: usize> {
|
||||
fn field_names_ordered() -> [&'static str; N];
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
#[derive(Debug)]
|
||||
enum PointField {
|
||||
Padding(u32),
|
||||
Field { size: u32, datatype: u8, count: u32 },
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
struct TypeLayoutInfo {
|
||||
#[derive(Debug)]
|
||||
struct KnownLayoutInfo {
|
||||
fields: Vec<PointField>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
impl TryFrom<type_layout::Field> for PointField {
|
||||
impl TryFrom<LayoutField> for PointField {
|
||||
type Error = MsgConversionError;
|
||||
|
||||
fn try_from(f: type_layout::Field) -> Result<Self, Self::Error> {
|
||||
fn try_from(f: LayoutField) -> Result<Self, Self::Error> {
|
||||
match f {
|
||||
type_layout::Field::Field { name: _, ty, size } => {
|
||||
LayoutField::Field { name: _, ty, size } => {
|
||||
let typename: String = ty.into_owned().to_lowercase();
|
||||
let datatype = FieldDatatype::from_str(typename.as_str())?;
|
||||
Ok(Self::Field {
|
||||
|
|
@ -961,21 +938,19 @@ impl TryFrom<type_layout::Field> for PointField {
|
|||
count: 1,
|
||||
})
|
||||
}
|
||||
type_layout::Field::Padding { size } => Ok(Self::Padding(size.try_into()?)),
|
||||
LayoutField::Padding { size } => Ok(Self::Padding(size.try_into()?)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
impl TryFrom<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
|
||||
impl TryFrom<LayoutDescription> for KnownLayoutInfo {
|
||||
type Error = MsgConversionError;
|
||||
|
||||
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<_>, _>>()?;
|
||||
fn try_from(t: LayoutDescription) -> Result<Self, Self::Error> {
|
||||
let fields: Vec<PointField> =
|
||||
t.0.into_iter()
|
||||
.map(PointField::try_from)
|
||||
.collect::<Result<Vec<_>, _>>()?;
|
||||
Ok(Self { fields })
|
||||
}
|
||||
}
|
||||
|
|
@ -1470,29 +1445,4 @@ impl FromBytes for u8 {
|
|||
fn from_le_bytes(bytes: PointDataBuffer) -> Self {
|
||||
Self::from_le_bytes([bytes[0]])
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
#[cfg(feature = "derive")]
|
||||
mod tests {
|
||||
use super::Fields;
|
||||
use rpcl2_derive::Fields;
|
||||
|
||||
use alloc::string::String;
|
||||
|
||||
#[allow(dead_code)]
|
||||
#[derive(Fields)]
|
||||
struct TestStruct {
|
||||
field1: String,
|
||||
#[rpcl2(rename("renamed_field"))]
|
||||
field2: i32,
|
||||
field3: f64,
|
||||
field4: bool,
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_struct_names() {
|
||||
let names = TestStruct::field_names_ordered();
|
||||
assert_eq!(names, ["field1", "renamed_field", "field3", "field4"]);
|
||||
}
|
||||
}
|
||||
}
|
||||
207
src/points.rs
207
src/points.rs
|
|
@ -1,18 +1,12 @@
|
|||
//! Predefined point types commonly used in ROS.
|
||||
use crate::{Fields, PointConvertible, RPCL2Point};
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
use type_layout::TypeLayout;
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
use alloc::vec::Vec;
|
||||
use crate::{LayoutDescription, LayoutField, PointConvertible, RPCL2Point};
|
||||
|
||||
/// A packed RGB color encoding as used in ROS tools.
|
||||
#[derive(Clone, Copy)]
|
||||
#[repr(C)]
|
||||
#[repr(C, align(4))]
|
||||
pub union RGB {
|
||||
packed: f32,
|
||||
unpacked: [u8; 4], // 1 byte padding
|
||||
unpacked: [u8; 4], // Padding
|
||||
}
|
||||
|
||||
unsafe impl Send for RGB {}
|
||||
|
|
@ -112,8 +106,7 @@ impl From<f32> for RGB {
|
|||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZ {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -155,12 +148,6 @@ impl PointXYZ {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<3>> for PointXYZ {
|
||||
fn from(point: RPCL2Point<3>) -> Self {
|
||||
Self::new(point[0].get(), point[1].get(), point[2].get())
|
||||
|
|
@ -173,13 +160,21 @@ impl From<PointXYZ> for RPCL2Point<3> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<3> for PointXYZ {}
|
||||
impl PointConvertible<3> for PointXYZ {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::padding(4),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZI {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -203,12 +198,6 @@ impl PointXYZI {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<4>> for PointXYZI {
|
||||
fn from(point: RPCL2Point<4>) -> Self {
|
||||
Self::new(
|
||||
|
|
@ -232,13 +221,21 @@ impl From<PointXYZI> for RPCL2Point<4> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<4> for PointXYZI {}
|
||||
impl PointConvertible<4> for PointXYZI {
|
||||
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),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZL {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -262,12 +259,6 @@ impl PointXYZL {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<4>> for PointXYZL {
|
||||
fn from(point: RPCL2Point<4>) -> Self {
|
||||
Self::new(
|
||||
|
|
@ -291,13 +282,21 @@ impl From<PointXYZL> for RPCL2Point<4> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<4> for PointXYZL {}
|
||||
impl PointConvertible<4> for PointXYZL {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("label", "u32", 4),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZRGB {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -338,12 +337,6 @@ impl PointXYZRGB {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<4>> for PointXYZRGB {
|
||||
fn from(point: RPCL2Point<4>) -> Self {
|
||||
Self {
|
||||
|
|
@ -367,14 +360,22 @@ impl From<PointXYZRGB> for RPCL2Point<4> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<4> for PointXYZRGB {}
|
||||
impl PointConvertible<4> for PointXYZRGB {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("rgb", "RGB", 4),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZRGBA {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -416,12 +417,6 @@ impl PointXYZRGBA {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<5>> for PointXYZRGBA {
|
||||
fn from(point: RPCL2Point<5>) -> Self {
|
||||
Self {
|
||||
|
|
@ -447,13 +442,23 @@ impl From<PointXYZRGBA> for RPCL2Point<5> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<5> for PointXYZRGBA {}
|
||||
impl PointConvertible<5> for PointXYZRGBA {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("rgb", "RGB", 4),
|
||||
LayoutField::new("a", "u8", 1),
|
||||
LayoutField::padding(15),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZRGBNormal {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -512,12 +517,6 @@ impl PointXYZRGBNormal {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<7>> for PointXYZRGBNormal {
|
||||
fn from(point: RPCL2Point<7>) -> Self {
|
||||
Self {
|
||||
|
|
@ -547,13 +546,25 @@ impl From<PointXYZRGBNormal> for RPCL2Point<7> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<7> for PointXYZRGBNormal {}
|
||||
impl PointConvertible<7> for PointXYZRGBNormal {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("rgb", "RGB", 4),
|
||||
LayoutField::new("normal_x", "f32", 4),
|
||||
LayoutField::new("normal_y", "f32", 4),
|
||||
LayoutField::new("normal_z", "f32", 4),
|
||||
LayoutField::padding(4),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZINormal {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -597,12 +608,6 @@ impl PointXYZINormal {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<7>> for PointXYZINormal {
|
||||
fn from(point: RPCL2Point<7>) -> Self {
|
||||
Self::new(
|
||||
|
|
@ -632,13 +637,25 @@ impl From<PointXYZINormal> for RPCL2Point<7> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<7> for PointXYZINormal {}
|
||||
impl PointConvertible<7> for PointXYZINormal {
|
||||
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("normal_x", "f32", 4),
|
||||
LayoutField::new("normal_y", "f32", 4),
|
||||
LayoutField::new("normal_z", "f32", 4),
|
||||
LayoutField::padding(4),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZRGBL {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -686,12 +703,6 @@ impl PointXYZRGBL {
|
|||
}
|
||||
}
|
||||
|
||||
impl Fields<5> for PointXYZRGBL {
|
||||
fn field_names_ordered() -> [&'static str; 5] {
|
||||
["x", "y", "z", "rgb", "label"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<5>> for PointXYZRGBL {
|
||||
fn from(point: RPCL2Point<5>) -> Self {
|
||||
Self {
|
||||
|
|
@ -717,13 +728,23 @@ impl From<PointXYZRGBL> for RPCL2Point<5> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<5> for PointXYZRGBL {}
|
||||
impl PointConvertible<5> for PointXYZRGBL {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("rgb", "RGB", 4),
|
||||
LayoutField::new("label", "u32", 4),
|
||||
LayoutField::padding(12),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
/// 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)]
|
||||
#[repr(C, align(16))]
|
||||
pub struct PointXYZNormal {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
|
|
@ -757,12 +778,6 @@ impl PointXYZNormal {
|
|||
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"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<6>> for PointXYZNormal {
|
||||
fn from(point: RPCL2Point<6>) -> Self {
|
||||
Self::new(
|
||||
|
|
@ -790,4 +805,16 @@ impl From<PointXYZNormal> for RPCL2Point<6> {
|
|||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<6> for PointXYZNormal {}
|
||||
impl PointConvertible<6> for PointXYZNormal {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("normal_x", "f32", 4),
|
||||
LayoutField::new("normal_y", "f32", 4),
|
||||
LayoutField::new("normal_z", "f32", 4),
|
||||
LayoutField::padding(8),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
|
@ -1,7 +1,7 @@
|
|||
//! Commonly used types and traits for predefined and custom point conversions.
|
||||
pub use crate::{
|
||||
FieldDatatype, Fields, FromBytes, GetFieldDatatype, MsgConversionError, PointCloud2Msg,
|
||||
PointConvertible, PointDataBuffer, RPCL2Point,
|
||||
FieldDatatype, FromBytes, GetFieldDatatype, LayoutDescription, LayoutField, MsgConversionError,
|
||||
PointCloud2Msg, PointConvertible, PointDataBuffer, RPCL2Point,
|
||||
};
|
||||
|
||||
pub use crate::points::*;
|
||||
|
|
@ -11,7 +11,4 @@ pub use crate::ros::*;
|
|||
pub use rayon::prelude::*;
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
pub use type_layout::TypeLayout;
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
pub use rpcl2_derive::*;
|
||||
pub use rpcl2_derive::*;
|
||||
|
|
@ -9,7 +9,6 @@ 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);
|
||||
|
|
@ -30,7 +29,6 @@ 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());
|
||||
|
|
@ -59,7 +57,6 @@ fn write_cloud() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn write_cloud_from_vec() {
|
||||
let cloud = vec![
|
||||
PointXYZ::new(0.0, 1.0, 5.0),
|
||||
|
|
@ -73,7 +70,6 @@ fn write_cloud_from_vec() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn write_empty_cloud_vec() {
|
||||
let cloud: Vec<PointXYZ> = vec![];
|
||||
let msg = PointCloud2Msg::try_from_vec(cloud);
|
||||
|
|
@ -90,7 +86,7 @@ fn write_empty_cloud_iter() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(all(feature = "derive", feature = "rayon"))]
|
||||
#[cfg(feature = "rayon")]
|
||||
fn conv_cloud_par_iter() {
|
||||
let cloud = vec![
|
||||
PointXYZ::new(0.0, 1.0, 5.0),
|
||||
|
|
@ -110,7 +106,7 @@ fn conv_cloud_par_iter() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(all(feature = "derive", feature = "rayon"))]
|
||||
#[cfg(feature = "rayon")]
|
||||
fn conv_cloud_par_par_iter() {
|
||||
let cloud = vec![
|
||||
PointXYZ::new(0.0, 1.0, 5.0),
|
||||
|
|
@ -133,7 +129,7 @@ fn conv_cloud_par_par_iter() {
|
|||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn custom_xyz_f32() {
|
||||
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
|
||||
#[derive(Debug, PartialEq, Clone, Default)]
|
||||
#[repr(C)]
|
||||
struct CustomPoint {
|
||||
x: f32,
|
||||
|
|
@ -141,6 +137,32 @@ fn custom_xyz_f32() {
|
|||
z: f32,
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<3>> for CustomPoint {
|
||||
fn from(point: RPCL2Point<3>) -> Self {
|
||||
Self {
|
||||
x: point[0].get(),
|
||||
y: point[1].get(),
|
||||
z: point[2].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<CustomPoint> for RPCL2Point<3> {
|
||||
fn from(point: CustomPoint) -> Self {
|
||||
[point.x.into(), point.y.into(), point.z.into()].into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<3> for CustomPoint {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
convert_from_into!(
|
||||
CustomPoint,
|
||||
vec![
|
||||
|
|
@ -164,7 +186,6 @@ fn custom_xyz_f32() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn custom_xyzi_f32() {
|
||||
let cloud: Vec<CustomPointXYZI> = vec![
|
||||
CustomPointXYZI {
|
||||
|
|
@ -193,7 +214,7 @@ fn custom_xyzi_f32() {
|
|||
},
|
||||
];
|
||||
|
||||
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
|
||||
#[derive(Debug, PartialEq, Clone, Default)]
|
||||
#[repr(C)]
|
||||
struct CustomPointXYZI {
|
||||
x: f32,
|
||||
|
|
@ -202,13 +223,48 @@ fn custom_xyzi_f32() {
|
|||
i: u8,
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<4>> for CustomPointXYZI {
|
||||
fn from(point: RPCL2Point<4>) -> Self {
|
||||
Self {
|
||||
x: point[0].get(),
|
||||
y: point[1].get(),
|
||||
z: point[2].get(),
|
||||
i: point[3].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<CustomPointXYZI> for RPCL2Point<4> {
|
||||
fn from(point: CustomPointXYZI) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
point.i.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<4> for CustomPointXYZI {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("i", "u8", 1),
|
||||
LayoutField::padding(3),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
convert_from_into!(CustomPointXYZI, cloud);
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn custom_rgba_f32() {
|
||||
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
|
||||
#[derive(Debug, PartialEq, Clone, Default)]
|
||||
#[repr(C)]
|
||||
struct CustomPoint {
|
||||
x: f32,
|
||||
|
|
@ -220,6 +276,53 @@ fn custom_rgba_f32() {
|
|||
a: u8,
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<7>> for CustomPoint {
|
||||
fn from(point: RPCL2Point<7>) -> Self {
|
||||
Self {
|
||||
x: point[0].get(),
|
||||
y: point[1].get(),
|
||||
z: point[2].get(),
|
||||
r: point[3].get(),
|
||||
g: point[4].get(),
|
||||
b: point[5].get(),
|
||||
a: point[6].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<CustomPoint> for RPCL2Point<7> {
|
||||
fn from(point: CustomPoint) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
point.r.into(),
|
||||
point.g.into(),
|
||||
point.b.into(),
|
||||
point.a.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<7> for CustomPoint {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
LayoutField::new("r", "u8", 1),
|
||||
LayoutField::padding(3),
|
||||
LayoutField::new("g", "u8", 1),
|
||||
LayoutField::padding(3),
|
||||
LayoutField::new("b", "u8", 1),
|
||||
LayoutField::padding(3),
|
||||
LayoutField::new("a", "u8", 1),
|
||||
LayoutField::padding(3),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
let cloud = vec![
|
||||
CustomPoint {
|
||||
x: 0.0,
|
||||
|
|
@ -375,7 +478,6 @@ fn converterxyzrgb() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn converterxyzrgb_from_vec() {
|
||||
convert_from_into_vec!(
|
||||
PointXYZRGB,
|
||||
|
|
@ -433,7 +535,6 @@ fn write_xyzi_read_xyz() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn write_xyzi_read_xyz_vec() {
|
||||
let write_cloud = vec![
|
||||
PointXYZI::new(0.0, 1.0, 5.0, 0.0),
|
||||
|
|
@ -453,9 +554,8 @@ fn write_xyzi_read_xyz_vec() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn write_less_than_available() {
|
||||
#[derive(Debug, PartialEq, Clone, Default, TypeLayout)]
|
||||
#[derive(Debug, PartialEq, Clone, Default)]
|
||||
#[repr(C)]
|
||||
struct CustomPoint {
|
||||
x: f32,
|
||||
|
|
@ -481,14 +581,16 @@ fn write_less_than_available() {
|
|||
}
|
||||
}
|
||||
|
||||
impl Fields<3> for CustomPoint {
|
||||
fn field_names_ordered() -> [&'static str; 3] {
|
||||
["x", "y", "z"]
|
||||
impl PointConvertible<3> for CustomPoint {
|
||||
fn layout() -> LayoutDescription {
|
||||
LayoutDescription::new(&[
|
||||
LayoutField::new("x", "f32", 4),
|
||||
LayoutField::new("y", "f32", 4),
|
||||
LayoutField::new("z", "f32", 4),
|
||||
])
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<3> for CustomPoint {}
|
||||
|
||||
let write_cloud = vec![
|
||||
CustomPoint {
|
||||
x: 1.0,
|
||||
|
|
@ -532,37 +634,4 @@ fn write_less_than_available() {
|
|||
];
|
||||
|
||||
convert_from_into_in_out_cloud!(write_cloud, CustomPoint, read_cloud, CustomPoint);
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
#[allow(unused_variables)]
|
||||
fn readme() {
|
||||
use ros_pointcloud2::prelude::*;
|
||||
|
||||
// PointXYZ (and many others) are provided by the crate.
|
||||
let cloud_points = vec![
|
||||
PointXYZI::new(91.486, -4.1, 42.0001, 0.1),
|
||||
PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MIN),
|
||||
];
|
||||
|
||||
let out_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
|
||||
|
||||
// Convert the ROS crate message type, we will use r2r here.
|
||||
// let msg: r2r::sensor_msgs::msg::PointCloud2 = out_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 info you want to have from the Msg.
|
||||
// Some logic here ...
|
||||
|
||||
point
|
||||
})
|
||||
.collect::<Vec<_>>();
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue