diff --git a/Cargo.toml b/Cargo.toml index f5de2b8..a70ded8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,9 +14,6 @@ homepage = "https://github.com/stelzo/ros_pointcloud2" exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**/build/**", "**/dist/**", "**/docs/**", "**/doc/**"] [dependencies] -mem_macros = "1.0.1" -num-traits = "0.2.15" -fallible-iterator = "0.3.0" rosrust_msg = { version = "0.1", optional = true } rosrust = { version = "0.9.11", optional = true } r2r = { version = "0.8.4", optional = true } diff --git a/README.md b/README.md index 01d5766..4bcd698 100644 --- a/README.md +++ b/README.md @@ -5,58 +5,48 @@

-Providing a memory efficient way for message conversion while allowing user defined types without the cost of iterations. -Instead of converting the entire cloud into a `Vec`, you get an iterable type that converts each point from the message on the fly. +Providing an easy to use, generics defined, point-wise iterator abstraction over the byte buffer in `PointCloud2` to minimize iterations in your processing pipeline. -To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `ros_types::PointCloud2Msg`. +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 ```rust use ros_pointcloud2::{ - fallible_iterator::FallibleIterator, - pcl_utils::PointXYZ, - ros_types::PointCloud2Msg, - ConvertXYZ, + pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, }; // Your points (here using the predefined type PointXYZ). let cloud_points = vec![ - PointXYZ { - x: 1.3, - y: 1.6, - z: 5.7, - }, - PointXYZ { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - }, + PointXYZ {x: 91.486, y: -4.1, z: 42.0001,}, + PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,}, ]; -let cloud_copy = cloud_points.clone(); // For checking equality later. +// For equality test later +let cloud_copy = cloud_points.clone(); -// Vector -> Converter -> Message -let internal_msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points) - .unwrap() - .try_into() - .unwrap(); +// Vector -> Writer -> Message. +// 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(); -// Convert to your favorite ROS crate message type, we will use rosrust here. -// let msg: rosrust_msg::sensor_msgs::PointCloud2 = internal_msg.into(); +// Convert to your ROS crate message type, we will use r2r here. +// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); -// Back to this crate's message type. +// Publish ... + +// ... now incoming from a topic. // let internal_msg: PointCloud2Msg = msg.into(); -// Message -> Converter -> Vector -let convert: ConvertXYZ = ConvertXYZ::try_from(internal_msg).unwrap(); -let new_cloud_points = convert - .map(|point: PointXYZ| { - // Insert your point business logic here - // or use other methods like .for_each(). - - // We are using a fallible iterator so we need to return a Result. - Ok(point) - }) - .collect::>() - .unwrap(); // Handle point conversion or byte errors here. +// 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| { + // Some logic here + + point + }) + .collect::>(); // iterating points here O(n) assert_eq!(new_cloud_points, cloud_copy); ``` @@ -95,141 +85,13 @@ Also, indicate the following dependencies to your linker inside the `package.xml builtin_interfaces ``` -### Others -To use `ros_pointcloud2` somewhere else, you can also implement the `Into` and `From` traits for `PointCloud2Msg`. - -Try to avoid cloning the `data: Vec` field. -```rust -use ros_pointcloud2::ros_types::PointCloud2Msg; - -struct YourROSPointCloud2 {} // Likely to be generated by your ROS crate. - -impl Into for PointCloud2Msg { - fn into(self) -> YourROSPointCloud2 { - todo!() - } -} - -impl From for PointCloud2Msg { - fn from(msg: YourROSPointCloud2) -> Self { - todo!() - } -} -``` - Please open an issue or PR if you want to see support for other crates. -## Features - -- Easy to integrate into your favorite ROS1 or ROS2 crate -- Custom point types -- Predefined types for common conversions (compared to PCL) - - PointXYZ - - PointXYZI - - PointXYZL - - PointXYZRGB - - PointXYZRGBA - - PointXYZRGBL - - PointXYZNormal - - PointXYZINormal - - PointXYZRGBNormal -- 2D and 3D - -## Custom Types - -You can freely convert to your own point types without reiterating the entire cloud. - -You just need to implement some traits. -```rust -use ros_pointcloud2::mem_macros::size_of; -use ros_pointcloud2::ros_types::PointCloud2Msg; -use ros_pointcloud2::{ConversionError, Convert, MetaNames, PointConvertible, PointMeta}; - -const DIM: usize = 3; -const METADIM: usize = 4; - -#[derive(Debug, PartialEq, Clone)] -struct CustomPoint { - x: f32, // DIM 1 - y: f32, // DIM 2 - z: f32, // DIM 3 - r: u8, // METADIM 1 - g: u8, // METADIM 2 - b: u8, // METADIM 3 - a: u8, // METADIM 4 -} - -// Converting your custom point to the crate's internal representation -impl From for ([f32; DIM], [PointMeta; METADIM]) { - fn from(point: CustomPoint) -> Self { - ( - [point.x, point.y, point.z], - [ - PointMeta::new(point.r), - PointMeta::new(point.g), - PointMeta::new(point.b), - PointMeta::new(point.a), - ], - ) - } -} - -// The mappings for index of meta idx to field names. Example: 0 -> "r", 1 -> "g", 2 -> "b", 3 -> "a" -impl MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { - ["r", "g", "b", "a"].map(|s| s.to_string()) - } -} - -// Converting crate's internal representation to your custom point -impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - r: data.1[0].get()?, - g: data.1[1].get()?, - b: data.1[2].get()?, - a: data.1[3].get()?, - }) - } -} - -impl PointConvertible for CustomPoint {} - -type MyConverter = Convert; - -// Your custom cloud (Vector) -let custom_cloud = vec![ - CustomPoint { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - r: u8::MAX, - g: u8::MAX, - b: u8::MAX, - a: u8::MAX, - }, -]; - -// Cloud -> ROS message -let custom_msg: PointCloud2Msg = MyConverter::try_from(custom_cloud) - .unwrap() - .try_into() - .unwrap(); - -// ROS message -> Cloud -let to_custom_type = MyConverter::try_from(custom_msg).unwrap(); -``` - ## Future Work - Benchmark vs PCL -- Proper error passing to the iterator result (currently merged into `PointConversionError`) -- remove allocations -- introduce no-panic for maximum stability - Add more predefined types +- Optional derive macros for custom point implementations + ## License [MIT](https://choosealicense.com/licenses/mit/) diff --git a/src/convert.rs b/src/convert.rs new file mode 100644 index 0000000..c5d055f --- /dev/null +++ b/src/convert.rs @@ -0,0 +1,361 @@ +use crate::*; + +/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html). +#[derive(Default, Clone, Debug, PartialEq, Copy)] +pub enum FieldDatatype { + F32, + F64, + I32, + U8, + U16, + #[default] + U32, + I8, + I16, +} + +impl FieldDatatype { + pub fn size(&self) -> usize { + match self { + FieldDatatype::U8 => 1, + FieldDatatype::U16 => 2, + FieldDatatype::U32 => 4, + FieldDatatype::I8 => 1, + FieldDatatype::I16 => 2, + FieldDatatype::I32 => 4, + FieldDatatype::F32 => 4, + FieldDatatype::F64 => 8, + } + } +} + +/// Getter trait for the datatype of a field value. +pub trait GetFieldDatatype { + fn field_datatype() -> FieldDatatype; +} + +impl GetFieldDatatype for f32 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::F32 + } +} + +impl GetFieldDatatype for f64 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::F64 + } +} + +impl GetFieldDatatype for i32 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::I32 + } +} + +impl GetFieldDatatype for u8 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::U8 + } +} + +impl GetFieldDatatype for u16 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::U16 + } +} + +impl GetFieldDatatype for u32 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::U32 + } +} + +impl GetFieldDatatype for i8 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::I8 + } +} + +impl GetFieldDatatype for i16 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::I16 + } +} + +impl TryFrom for FieldDatatype { + type Error = ConversionError; + + fn try_from(value: u8) -> Result { + match value { + 1 => Ok(FieldDatatype::I8), + 2 => Ok(FieldDatatype::U8), + 3 => Ok(FieldDatatype::I16), + 4 => Ok(FieldDatatype::U16), + 5 => Ok(FieldDatatype::I32), + 6 => Ok(FieldDatatype::U32), + 7 => Ok(FieldDatatype::F32), + 8 => Ok(FieldDatatype::F64), + _ => Err(ConversionError::UnsupportedFieldType), + } + } +} + +impl From for u8 { + fn from(val: FieldDatatype) -> Self { + match val { + FieldDatatype::I8 => 1, + FieldDatatype::U8 => 2, + FieldDatatype::I16 => 3, + FieldDatatype::U16 => 4, + FieldDatatype::I32 => 5, + FieldDatatype::U32 => 6, + FieldDatatype::F32 => 7, + FieldDatatype::F64 => 8, + } + } +} + +pub(crate) fn check_coord( + coord: Option, + fields: &[PointFieldMsg], + xyz_field_type: &FieldDatatype, +) -> Result { + match coord { + Some(y_idx) => { + let field = &fields[y_idx]; + if field.datatype != u8::from(*xyz_field_type) { + return Err(ConversionError::InvalidFieldFormat); + } + Ok(field.clone()) + } + None => Err(ConversionError::NotEnoughFields), + } +} + +/// Matching field names from each meta data per point to the PointField name. +/// 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 { + fn meta_names() -> [&'static str; METADIM]; +} + +/// 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 { + fn from_be_bytes(bytes: &[u8]) -> Self; + fn from_le_bytes(bytes: &[u8]) -> Self; + + fn bytes(_: &Self) -> Vec; +} + +impl FromBytes for i8 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0]]) + } + + #[inline] + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0]]) + } + + #[inline] + fn bytes(x: &i8) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for i16 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1]]) + } + + #[inline] + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1]]) + } + + #[inline] + fn bytes(x: &i16) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for u16 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1]]) + } + + #[inline] + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1]]) + } + + #[inline] + fn bytes(x: &u16) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for u32 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + #[inline] + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + #[inline] + fn bytes(x: &u32) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for f32 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + #[inline] + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + #[inline] + fn bytes(x: &f32) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for i32 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + fn bytes(x: &i32) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for f64 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([ + bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], + ]) + } + + #[inline] + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([ + bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], + ]) + } + + #[inline] + fn bytes(x: &f64) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for u8 { + #[inline] + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0]]) + } + + #[inline] + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0]]) + } + + #[inline] + fn bytes(x: &u8) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +#[derive(Default, Clone, Debug, PartialEq)] +pub(crate) enum Endianness { + Big, + + #[default] + Little, +} + +#[inline(always)] +pub(crate) fn load_loadable( + start_idx: usize, + data: &[u8], + endian: &Endianness, +) -> T { + match endian { + Endianness::Big => T::from_be_bytes(load_bytes::(start_idx, data).as_slice()), + Endianness::Little => T::from_le_bytes(load_bytes::(start_idx, data).as_slice()), + } +} + +/// Note: check if the data slice is long enough to load the bytes beforehand! Uses unsafe indexing. +#[inline(always)] +fn load_bytes(start_idx: usize, data: &[u8]) -> [u8; S] { + let mut target = [u8::default(); S]; + + debug_assert!(target.len() == S); + debug_assert!(data.len() >= start_idx + S); + + let source = unsafe { data.get_unchecked(start_idx..start_idx + S) }; + target + .iter_mut() + .zip(source.iter()) + .for_each(|(target, source)| { + *target = *source; + }); + target +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn from_bytes() { + i8::bytes(&1); + u8::bytes(&1); + i16::bytes(&1); + u16::bytes(&1); + i32::bytes(&1); + u32::bytes(&1); + f32::bytes(&1.0); + f64::bytes(&1.0); + } +} diff --git a/src/lib.rs b/src/lib.rs index eff935d..dd868a1 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,261 +1,210 @@ -#![doc = include_str!("../README.md")] +//! 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 +//! 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 +//! the [`ros_pointcloud2::PointCloud2Msg`] using the `From` trait, +//! which is mostly a ownership transfer without copying the point data. +//! +//! There are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust_msg` message types +//! available in the feature flags. If you miss a message type, please open an issue or a PR. +//! See the [`ros_pointcloud2::ros_types`] module for more information. +//! +//! Common point types like `PointXYZ` are provided in the [`ros_pointcloud2::pcl_utils`] module. You can implement any custom point type +//! that can be described by the specification. +//! See the [`ros_pointcloud2::Reader`] or [`ros_pointcloud2::Writer`] struct documentation for an example. +//! +//! # Example +//! ``` +//! use ros_pointcloud2::{ +//! pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, +//! }; +//! +//! // Your points (here using the predefined type PointXYZ). +//! let cloud_points = vec![ +//! PointXYZ {x: 9.0006, 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. +//! // 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(); +//! +//! // Convert to your ROS crate message type, we will use r2r here. +//! // let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); +//! +//! // Publish ... +//! +//! // ... now incoming from a topic. +//! // let internal_msg: PointCloud2Msg = msg.into(); +//! +//! // 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| { +//! // Some logic here +//! +//! point +//! }) +//! .collect::>(); // iterating points here O(n) +//! +//! assert_eq!(new_cloud_points, cloud_copy); +//! ``` + +pub mod convert; pub mod pcl_utils; pub mod ros_types; -use crate::pcl_utils::*; -use crate::ros_types::{PointCloud2Msg, PointFieldMsg}; -use num_traits::Zero; +/// Macro to get the size of a type at compile time. This is a convenience macro to avoid writing out the full std::mem::size_of::(). +/// Use it for your custom Reader and Writer implementations. +/// # Example +/// ``` +/// use ros_pointcloud2::{ +/// writer::Writer, size_of, pcl_utils::PointXYZ, +/// }; +/// +/// type MyWriterXYZ = Writer; +/// ``` +#[macro_export] +macro_rules! size_of { + ($t:ty) => { + std::mem::size_of::<$t>() + }; +} -#[macro_use] -pub extern crate mem_macros; +pub mod reader; +pub mod writer; -pub extern crate fallible_iterator; +pub use convert::MetaNames; +pub use ros_types::PointCloud2Msg; -/// Errors that can occur when converting between PointCloud2 and a custom type. +use crate::convert::{ + FieldDatatype, + FromBytes, +}; + +use crate::ros_types::PointFieldMsg; + +/// All errors that can occur for creating Reader and Writer. #[derive(Debug)] pub enum ConversionError { + /// The coordinate field does not match the expected datatype. InvalidFieldFormat, + + /// Not enough meta or dimensional fields in the PointCloud2 message. NotEnoughFields, + + /// The dimensionality of the point inside the message is too high. TooManyDimensions, + + /// The field type is not supported by the ROS message description. UnsupportedFieldType, + + /// There are no points in the point cloud. NoPoints, + + /// The length of the byte buffer in the message does not match the expected length computed from the fields. DataLengthMismatch, - MetaIndexLengthMismatch, - EndOfBuffer, - PointConversionError, - MetaDatatypeMismatch, +} + +/// 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 for Point { +/// fn from(point: MyPointXYZI) -> Self { +/// Point { +/// coords: [point.x, point.y, point.z], +/// meta: [point.intensity.into()], +/// } +/// } +/// } +/// +/// impl From> for MyPointXYZI { +/// fn from(point: Point) -> Self { +/// Self { +/// x: point.coords[0], +/// y: point.coords[1], +/// z: point.coords[2], +/// intensity: point.meta[0].get(), +/// } +/// } +/// } +/// ``` +pub struct Point { + pub coords: [T; DIM], + pub meta: [PointMeta; METADIM], } /// Trait to convert a point to a tuple of coordinates and meta data. /// Implement this trait for your point type to use the conversion. -pub trait PointConvertible: - TryInto<([T; DIM], [PointMeta; METADIM])> - + TryFrom<([T; DIM], [PointMeta; METADIM])> - + MetaNames - + Clone -where - T: FromBytes, -{ -} - -/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html). -#[derive(Clone, Debug, PartialEq, Copy)] -pub enum FieldDatatype { - F32, - F64, - I32, - U8, - U16, - U32, - I8, - I16, -} - -/// Getter trait for the datatype of a field value. -pub trait GetFieldDatatype { - fn field_datatype() -> FieldDatatype; -} - -impl GetFieldDatatype for f32 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::F32 - } -} - -impl GetFieldDatatype for f64 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::F64 - } -} - -impl GetFieldDatatype for i32 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::I32 - } -} - -impl GetFieldDatatype for u8 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::U8 - } -} - -impl GetFieldDatatype for u16 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::U16 - } -} - -impl GetFieldDatatype for u32 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::U32 - } -} - -impl GetFieldDatatype for i8 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::I8 - } -} - -impl GetFieldDatatype for i16 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::I16 - } -} - -fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 { - match geo_type { - FieldDatatype::I8 => 1, - FieldDatatype::U8 => 2, - FieldDatatype::I16 => 3, - FieldDatatype::U16 => 4, - FieldDatatype::I32 => 5, - FieldDatatype::U32 => 6, - FieldDatatype::F32 => 7, - FieldDatatype::F64 => 8, - } -} - -fn convert_msg_code_to_type(code: u8) -> Result { - match code { - 7 => Ok(FieldDatatype::F32), - 8 => Ok(FieldDatatype::F64), - 5 => Ok(FieldDatatype::I32), - 2 => Ok(FieldDatatype::U8), - 4 => Ok(FieldDatatype::U16), - 6 => Ok(FieldDatatype::U32), - 1 => Ok(FieldDatatype::I8), - 3 => Ok(FieldDatatype::I16), - _ => Err(ConversionError::UnsupportedFieldType), - } -} - -fn check_coord( - coord: Option, - fields: &[PointFieldMsg], - xyz_field_type: &FieldDatatype, -) -> Result { - match coord { - Some(y_idx) => { - let field = &fields[y_idx]; - if field.datatype != convert_to_msg_code(xyz_field_type) { - return Err(ConversionError::InvalidFieldFormat); - } - Ok(field.clone()) - } - None => Err(ConversionError::NotEnoughFields), - } -} - -/// 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. -pub trait MetaNames { - fn meta_names() -> [String; METADIM]; -} - -/// The Convert struct is used to convert between a PointCloud2 message and a custom type. -/// A custom type must implement the FromBytes trait and the Into trait. -/// The Into trait is used to convert the custom type into a tuple of the coordinates and the meta data. -/// The FromBytes trait is used to convert the bytes from the PointCloud2 message into the custom type. -/// +/// /// # Example /// ``` -/// use ros_pointcloud2::mem_macros::size_of; -/// use ros_pointcloud2::{Convert, PointMeta}; -/// use ros_pointcloud2::ros_types::PointCloud2Msg; -/// const DIM : usize = 3; // how many dimensions your point has (e.g. x, y, z) -/// const METADIM : usize = 4; // how many meta fields you have (e.g. r, g, b, a) -/// type MyConverter = Convert; +/// 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 for Point { +/// fn from(point: MyPointXYZI) -> Self { +/// Point { +/// coords: [point.x, point.y, point.z], +/// meta: [point.intensity.into()], +/// } +/// } +/// } +/// +/// impl From> for MyPointXYZI { +/// fn from(point: Point) -> 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 for MyPointXYZI {} /// ``` -pub struct Convert +pub trait PointConvertible: + From> + Into> + MetaNames + Clone + 'static where T: FromBytes, - C: PointConvertible, { - iteration: usize, - coordinates: Vec, - phantom_t: std::marker::PhantomData, - data: Vec, - point_step_size: usize, - cloud_length: usize, - offsets: Vec, - big_endian: Endianness, - xyz_field_type: FieldDatatype, - meta: Vec<(String, FieldDatatype)>, } -pub type ConvertXYZ = Convert; -pub type ConvertXYZI = Convert; -pub type ConvertXYZNormal = Convert; -pub type ConvertXYZRGB = Convert; -pub type ConvertXYZRGBL = Convert; -pub type ConvertXYZRGBA = Convert; -pub type ConvertXYZRGBNormal = Convert; -pub type ConvertXYZINormal = Convert; -pub type ConvertXYZL = Convert; - -impl TryFrom> - for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Error = ConversionError; - - /// Converts a vector of custom types into a Convert struct that implements the Iterator trait. - /// - /// # Example - /// ``` - /// use ros_pointcloud2::{ConvertXYZ, ConversionError}; - /// use ros_pointcloud2::pcl_utils::PointXYZ; - /// - /// let cloud_points: Vec = vec![ - /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, - /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, - /// ]; - /// let convert: Result = ConvertXYZ::try_from(cloud_points); - /// ``` - fn try_from(cloud: Vec) -> Result { - let length = cloud.len(); - - let mut meta: Vec<(String, FieldDatatype)> = vec![]; - let first_point = cloud.first().ok_or(ConversionError::NoPoints)?; - let point_meta: ([T; DIM], [PointMeta; METADIM]) = match first_point.clone().try_into() { - Ok(point_meta) => point_meta, - Err(_) => { - return Err(ConversionError::PointConversionError); - } - }; - let meta_names = C::meta_names(); - let mut point_step_size = DIM * SIZE; - for (idx, value) in point_meta.1.iter().enumerate() { - meta.push(( - meta_names - .get(idx) - .ok_or(ConversionError::MetaIndexLengthMismatch)? - .clone(), - value.datatype, - )); - point_step_size += datatype_size(&value.datatype); - } - - Ok(Self { - phantom_t: std::marker::PhantomData, - iteration: usize::zero(), - coordinates: cloud, - data: Vec::new(), - point_step_size, - cloud_length: length, - offsets: Vec::new(), - big_endian: Endianness::Little, - xyz_field_type: T::field_datatype(), - meta, - }) - } -} - -/// Meta data representation for a point +/// Metadata representation for a point. /// /// This struct is used to store meta data in a fixed size byte buffer along the with the /// datatype that is encoded so that it can be decoded later. @@ -266,586 +215,122 @@ where /// /// let original_data: f64 = 1.0; /// let meta = PointMeta::new(original_data); -/// let my_data: f64 = meta.get().unwrap(); +/// let my_data: f64 = meta.get(); /// ``` #[derive(Debug, Clone, Copy)] pub struct PointMeta { - bytes: [u8; size_of!(f64)], + bytes: [u8; std::mem::size_of::()], datatype: FieldDatatype, } impl Default for PointMeta { fn default() -> Self { Self { - bytes: [0; size_of!(f64)], + bytes: [u8::default(); std::mem::size_of::()], datatype: FieldDatatype::F32, } } } impl PointMeta { - /// Create a new PointMeta from a value + /// Create a new PointMeta from a value. /// /// # Example /// ``` /// let meta = ros_pointcloud2::PointMeta::new(1.0); /// ``` + #[inline(always)] pub fn new(value: T) -> Self { let raw_bytes = T::bytes(&value); - let mut bytes = [0; size_of!(f64)]; - for (idx, byte) in raw_bytes.iter().enumerate() { - bytes[idx] = *byte; + let mut bytes = [0; std::mem::size_of::()]; + for (byte, save_byte) in raw_bytes.into_iter().zip(bytes.iter_mut()) { + *save_byte = byte; } + Self { bytes, datatype: T::field_datatype(), } } - fn new_from_buffer( - data: &[u8], - offset: usize, - datatype: &FieldDatatype, - ) -> Result { - let size = datatype_size(datatype); - let bytes = data - .get(offset..offset + size) - .ok_or(ConversionError::DataLengthMismatch)?; - let mut bytes_array = [0; size_of!(f64)]; // 8 bytes as f64 is the largest type - for (idx, byte) in bytes.iter().enumerate() { - bytes_array[idx] = *byte; + #[inline(always)] + fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self { + debug_assert!(data.len() >= offset + datatype.size()); + + let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) }; + let mut bytes_array = [0; std::mem::size_of::()]; // 8 bytes as f64 is the largest type + for (byte, save_byte) in bytes.iter().zip(bytes_array.iter_mut()) { + *save_byte = *byte; } - Ok(Self { + + Self { bytes: bytes_array, datatype: *datatype, - }) + } } - /// Get the value from the PointMeta. It will return None if the datatype does not match. + /// Get the numeric value from the PointMeta description. /// /// # Example /// ``` /// let original_data: f64 = 1.0; /// let meta = ros_pointcloud2::PointMeta::new(original_data); - /// let my_data: f64 = meta.get().unwrap(); + /// let my_data: f64 = meta.get(); /// ``` - pub fn get(&self) -> Result { - if self.datatype != T::field_datatype() { - return Err(ConversionError::MetaDatatypeMismatch); - } - let size = datatype_size(&T::field_datatype()); - if let Some(bytes) = self.bytes.get(0..size) { - Ok(T::from_le_bytes(bytes)) - } else { - Err(ConversionError::DataLengthMismatch) // self.bytes are not long enough, already handled while parsing - } + pub fn get(&self) -> T { + let size = T::field_datatype().size(); + let bytes = self + .bytes + .get(0..size) + .expect("Exceeds bounds of f64, which is the largest type"); + T::from_le_bytes(bytes) } } -impl TryFrom - for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Error = ConversionError; - - /// Converts a ROS PointCloud2 message into a Convert struct that implements the Iterator trait. - /// Iterate over the struct to get or use the points. - /// - /// # Example - /// Since we do not have a ROS node here, we first create a PointCloud2 message and then convert back to a Convert struct. - /// ``` - /// use ros_pointcloud2::ros_types::PointCloud2Msg; - /// use ros_pointcloud2::ConvertXYZ; - /// use ros_pointcloud2::pcl_utils::PointXYZ; - /// - /// let cloud_points: Vec = vec![ - /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, - /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, - /// ]; - /// let msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap(); - /// - /// let convert: ConvertXYZ = ConvertXYZ::try_from(msg).unwrap(); // parse message - /// ``` - fn try_from(cloud: PointCloud2Msg) -> Result { - if cloud.fields.len() < DIM { - return Err(ConversionError::NotEnoughFields); - } - - let xyz_field_type = T::field_datatype(); - - let mut has_x: Option = None; - let mut has_y: Option = None; - let mut has_z: Option = None; - - let mut meta_with_offsets: Vec<(String, FieldDatatype, usize)> = - Vec::with_capacity(METADIM); - - let lower_meta_names = C::meta_names() - .iter() - .map(|x| x.to_lowercase()) - .collect::>(); - for (idx, field) in cloud.fields.iter().enumerate() { - let lower_field_name = field.name.to_lowercase(); - match lower_field_name.as_str() { - "x" => has_x = Some(idx), - "y" => has_y = Some(idx), - "z" => has_z = Some(idx), - _ => { - if lower_meta_names.contains(&lower_field_name) { - meta_with_offsets.push(( - field.name.clone(), - convert_msg_code_to_type(field.datatype)?, - field.offset as usize, - )); - } - } - } - } - - meta_with_offsets.sort_by(|a, b| a.2.cmp(&b.2)); - let meta_offsets: Vec = meta_with_offsets.iter().map(|x| x.2).collect(); - let meta: Vec<(String, FieldDatatype)> = meta_with_offsets - .iter() - .map(|x| (x.0.clone(), x.1)) - .collect(); - - let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?; - let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?; - - let mut offsets = vec![x_field.offset as usize, y_field.offset as usize]; - - let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type); - match z_field { - Ok(z_field) => { - offsets.push(z_field.offset as usize); - } - Err(err) => match err { - ConversionError::NotEnoughFields => { - if DIM == 3 { - return Err(ConversionError::NotEnoughFields); - } - } - _ => return Err(err), - }, - } - - let endian = if cloud.is_bigendian { - Endianness::Big - } else { - Endianness::Little - }; - - if offsets.len() != DIM { - return Err(ConversionError::NotEnoughFields); - } - - offsets.extend(meta_offsets); - - if offsets.len() != DIM + METADIM { - return Err(ConversionError::NotEnoughFields); - } - - let point_step_size = cloud.point_step as usize; - if point_step_size * cloud.width as usize * cloud.height as usize != cloud.data.len() { - return Err(ConversionError::DataLengthMismatch); - } - - if let Some(max_offset) = offsets.last() { - if let Some(last_meta) = meta.last() { - let size_with_last_meta = max_offset + datatype_size(&last_meta.1); - if size_with_last_meta > point_step_size { - return Err(ConversionError::DataLengthMismatch); - } - } - } - - Ok(Self { - phantom_t: std::marker::PhantomData, - iteration: usize::zero(), - coordinates: Vec::new(), - data: cloud.data, - point_step_size, - cloud_length: cloud.width as usize * cloud.height as usize, - offsets, - big_endian: endian, - xyz_field_type: T::field_datatype(), - meta, - }) +impl From for PointMeta { + fn from(value: f32) -> Self { + Self::new(value) } } -fn datatype_size(datatype: &FieldDatatype) -> usize { - match datatype { - FieldDatatype::U8 => 1, - FieldDatatype::U16 => 2, - FieldDatatype::U32 => 4, - FieldDatatype::I8 => 1, - FieldDatatype::I16 => 2, - FieldDatatype::I32 => 4, - FieldDatatype::F32 => 4, - FieldDatatype::F64 => 8, +impl From for PointMeta { + fn from(value: f64) -> Self { + Self::new(value) } } -impl TryInto - for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Error = ConversionError; - - /// Convert the point cloud to a ROS message. - /// First use the `try_from` method for initializing the conversion and parsing meta data. - /// Then use the `try_into` method to do the actual conversion. - /// - /// # Example - /// ``` - /// use ros_pointcloud2::ros_types::PointCloud2Msg; - /// use ros_pointcloud2::ConvertXYZ; - /// use ros_pointcloud2::pcl_utils::PointXYZ; - /// - /// let cloud_points: Vec = vec![ - /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, - /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, - /// ]; - /// let msg_out: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap(); - /// ``` - fn try_into(self) -> Result { - let mut cloud = PointCloud2Msg::default(); - - // Define the message fields - let mut fields = Vec::new(); - if DIM > 3 { - return Err(ConversionError::TooManyDimensions); - } - - let datatype: u8 = convert_to_msg_code(&self.xyz_field_type); - - if DIM > 1 { - fields.push(PointFieldMsg { - name: "x".to_string(), - offset: 0, - datatype, - count: 1, - }); - fields.push(PointFieldMsg { - name: "y".to_string(), - offset: SIZE as u32, - datatype, - count: 1, - }); - } - - if DIM == 3 { - fields.push(PointFieldMsg { - name: "z".to_string(), - offset: 2 * SIZE as u32, - datatype, - count: 1, - }); - } - - // meta data fields - let first_point = self.coordinates.first().ok_or(ConversionError::NoPoints)?; - let meta: ([T; DIM], [PointMeta; METADIM]) = match first_point.clone().try_into() { - Ok(meta) => meta, - Err(_) => return Err(ConversionError::PointConversionError), - }; - - let meta_names = C::meta_names(); - - let sized_dim = DIM as u32 * SIZE as u32; - - for (idx, value) in meta.1.iter().enumerate() { - let datatype: u8 = convert_to_msg_code(&value.datatype); - let mut offset = sized_dim; - for i in 0..idx { - let datatype: u8 = convert_to_msg_code(&meta.1[i].datatype); - let field_type = convert_msg_code_to_type(datatype)?; - let field_size = datatype_size(&field_type); - offset += field_size as u32; - } - fields.push(PointFieldMsg { - name: meta_names[idx].to_string(), - offset, - datatype, - count: 1, - }); - } - - // calc all meta data points step size - let mut step_size = 0; - for field in fields.iter() { - let field_type = convert_msg_code_to_type(field.datatype)?; - let field_size = datatype_size(&field_type); - step_size += field.count * field_size as u32; - } - - cloud.fields = fields; - cloud.point_step = step_size; - cloud.row_step = self.coordinates.len() as u32 * cloud.point_step; - cloud.height = 1; - cloud.width = self.coordinates.len() as u32; - cloud.is_bigendian = false; - cloud.is_dense = true; - - for coords in self.coordinates { - let coords_data: ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() { - Ok(meta) => meta, - Err(_) => return Err(ConversionError::PointConversionError), - }; - coords_data - .0 - .iter() - .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); - coords_data.1.iter().for_each(|meta| { - let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)]; - cloud.data.extend_from_slice(truncated_bytes); - }); - } - - Ok(cloud) +impl From for PointMeta { + fn from(value: i32) -> Self { + Self::new(value) } } -impl - Convert -where - T: FromBytes, - C: PointConvertible, -{ - /// Convenience getter for the number of points in the cloud. - pub fn len(&self) -> usize { - self.cloud_length - } - - pub fn is_empty(&self) -> bool { - self.cloud_length == 0 +impl From for PointMeta { + fn from(value: u8) -> Self { + Self::new(value) } } -impl - fallible_iterator::FallibleIterator for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Item = C; - type Error = ConversionError; - - /// Iterate over the data buffer and interpret the data as a point. - fn next(&mut self) -> Result, Self::Error> { - if self.iteration >= self.cloud_length { - return Ok(None); // iteration finished - } - - let mut xyz: [T; DIM] = [T::zero(); DIM]; - let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM]; - for (idx, in_point_offset) in self.offsets.iter().enumerate() { - if idx < DIM { - match load_loadable::( - (self.iteration * self.point_step_size) + in_point_offset, - &self.data, - self.big_endian.clone(), - ) { - Some(c) => xyz[idx] = c, - None => return Err(ConversionError::EndOfBuffer), - } - } else { - let meta_idx = idx - DIM; - let meta_type = self.meta[meta_idx].1; - let start = (self.iteration * self.point_step_size) + in_point_offset; - if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) { - meta[meta_idx] = m; - } else { - return Err(ConversionError::MetaIndexLengthMismatch); // since we can not return an error in the iterator, we finish the iteration here early since the last point is not valid. This case is not expected since we catch it while parsing the file. - } - } - } - - self.iteration += 1; - let conv = C::try_from((xyz, meta)); // try convert the point - match conv { - Err(_) => Err(ConversionError::PointConversionError), - Ok(tuple) => Ok(Some(tuple)), - } +impl From for PointMeta { + fn from(value: u16) -> Self { + Self::new(value) } } -/// This trait is used to convert a byte slice to a primitive type. -/// All PointField types are supported. -pub trait FromBytes: Zero + Sized + Copy + GetFieldDatatype { - fn from_be_bytes(bytes: &[u8]) -> Self; - fn from_le_bytes(bytes: &[u8]) -> Self; - - fn bytes(_: &Self) -> Vec; -} - -impl FromBytes for i8 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0]]) - } - - fn bytes(x: &i8) -> Vec { - Vec::from(x.to_le_bytes()) +impl From for PointMeta { + fn from(value: u32) -> Self { + Self::new(value) } } -impl FromBytes for i16 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1]]) - } - - fn bytes(x: &i16) -> Vec { - Vec::from(x.to_le_bytes()) +impl From for PointMeta { + fn from(value: i8) -> Self { + Self::new(value) } } -impl FromBytes for u16 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1]]) - } - - fn bytes(x: &u16) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for u32 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn bytes(x: &u32) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for f32 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn bytes(x: &f32) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for i32 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn bytes(x: &i32) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for f64 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([ - bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], - ]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([ - bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], - ]) - } - - fn bytes(x: &f64) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for u8 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0]]) - } - - fn bytes(x: &u8) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -#[derive(Clone, Debug, PartialEq)] -enum Endianness { - Big, - Little, -} - -fn load_loadable( - start_idx: usize, - data: &[u8], - endian: Endianness, -) -> Option { - match endian { - Endianness::Big => Some(T::from_be_bytes( - load_bytes::(start_idx, data)?.as_slice(), - )), - Endianness::Little => Some(T::from_le_bytes( - load_bytes::(start_idx, data)?.as_slice(), - )), - } -} - -fn load_bytes(start_idx: usize, data: &[u8]) -> Option<[u8; S]> { - if start_idx + S > data.len() { - return None; - } - let mut buff: [u8; S] = [u8::zero(); S]; - for (byte, buff_val) in buff.iter_mut().enumerate().take(S) { - let raw_byte = data.get(start_idx + byte); - match raw_byte { - None => { - return None; - } - Some(some_byte) => { - *buff_val = *some_byte; - } - } - } - - Some(buff) -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn from_bytes() { - i8::bytes(&1); - u8::bytes(&1); - i16::bytes(&1); - u16::bytes(&1); - i32::bytes(&1); - u32::bytes(&1); - f32::bytes(&1.0); - f64::bytes(&1.0); +impl From for PointMeta { + fn from(value: i16) -> Self { + Self::new(value) } } diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs index 359651b..2439e9d 100644 --- a/src/pcl_utils.rs +++ b/src/pcl_utils.rs @@ -1,11 +1,13 @@ -use crate::{ConversionError, MetaNames, PointConvertible, PointMeta}; +use crate::{MetaNames, Point, PointConvertible}; +/// Pack an RGB color into a single f32 value as used in ROS with PCL for RViz usage. #[inline] fn pack_rgb(r: u8, g: u8, b: u8) -> f32 { let packed = ((r as u32) << 16) + ((g as u32) << 8) + (b as u32); f32::from_bits(packed) } +/// Unpack an RGB color from a single f32 value as used in ROS with PCL for RViz usage. #[inline] fn unpack_rgb(rgb: f32) -> [u8; 3] { let packed: u32 = rgb.to_bits(); @@ -24,33 +26,32 @@ pub struct PointXYZ { pub z: f32, } -impl TryInto<([f32; 3], [PointMeta; 0])> for PointXYZ { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 0]), Self::Error> { - Ok(([self.x, self.y, self.z], [])) +impl From> for PointXYZ { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + } } } -impl TryFrom<([f32; 3], [PointMeta; 0])> for PointXYZ { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 0])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - }) +impl From for Point { + fn from(point: PointXYZ) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [], + } } } impl MetaNames<0> for PointXYZ { - fn meta_names() -> [String; 0] { + fn meta_names() -> [&'static str; 0] { [] } } -impl PointConvertible for PointXYZ {} +impl PointConvertible() }, 3, 0> for PointXYZ {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an intensity value. @@ -62,34 +63,33 @@ pub struct PointXYZI { pub intensity: f32, } -impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZI { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { - Ok(([self.x, self.y, self.z], [PointMeta::new(self.intensity)])) +impl From for Point { + fn from(point: PointXYZI) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [point.intensity.into()], + } } } -impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZI { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - intensity: data.1[0].get()?, - }) +impl From> for PointXYZI { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + intensity: point.meta[0].get(), + } } } impl MetaNames<1> for PointXYZI { - fn meta_names() -> [String; 1] { - ["intensity"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 1] { + ["intensity"] } } -impl PointConvertible for PointXYZI {} +impl PointConvertible() }, 3, 1> for PointXYZI {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an RGB color value. @@ -103,39 +103,37 @@ pub struct PointXYZRGB { pub b: u8, } -impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(([self.x, self.y, self.z], [PointMeta::new(rgb)])) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZRGB { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGB { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], r: rgb_unpacked[0], g: rgb_unpacked[1], b: rgb_unpacked[2], - }) + } + } +} + +impl From for Point { + fn from(point: PointXYZRGB) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [pack_rgb(point.r, point.g, point.b).into()], + } } } impl MetaNames<1> for PointXYZRGB { - fn meta_names() -> [String; 1] { - ["rgb"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 1] { + ["rgb"] } } -impl PointConvertible for PointXYZRGB {} +impl PointConvertible() }, 3, 1> for PointXYZRGB {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an RGBA color value. @@ -151,43 +149,39 @@ pub struct PointXYZRGBA { pub a: u8, } -impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(( - [self.x, self.y, self.z], - [PointMeta::new(rgb), PointMeta::new(self.a)], - )) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBA { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGBA { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], r: rgb_unpacked[0], g: rgb_unpacked[1], b: rgb_unpacked[2], - a: data.1[1].get()?, - }) + a: point.meta[1].get(), + } + } +} + +impl From for Point { + fn from(point: PointXYZRGBA) -> Self { + let rgb = pack_rgb(point.r, point.g, point.b); + Point { + coords: [point.x, point.y, point.z], + meta: [rgb.into(), point.a.into()], + } } } impl MetaNames<2> for PointXYZRGBA { - fn meta_names() -> [String; 2] { - ["rgb", "a"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 2] { + ["rgb", "a"] } } -impl PointConvertible for PointXYZRGBA {} +impl PointConvertible() }, 3, 2> for PointXYZRGBA {} /// 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. @@ -204,50 +198,46 @@ pub struct PointXYZRGBNormal { pub normal_z: f32, } -impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(( - [self.x, self.y, self.z], - [ - PointMeta::new(rgb), - PointMeta::new(self.normal_x), - PointMeta::new(self.normal_y), - PointMeta::new(self.normal_z), - ], - )) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGBNormal { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], r: rgb_unpacked[0], g: rgb_unpacked[1], b: rgb_unpacked[2], - normal_x: data.1[1].get()?, - normal_y: data.1[2].get()?, - normal_z: data.1[3].get()?, - }) + normal_x: point.meta[1].get(), + normal_y: point.meta[2].get(), + normal_z: point.meta[3].get(), + } + } +} + +impl From for Point { + fn from(point: PointXYZRGBNormal) -> Self { + let rgb = pack_rgb(point.r, point.g, point.b); + Point { + coords: [point.x, point.y, point.z], + meta: [ + rgb.into(), + point.normal_x.into(), + point.normal_y.into(), + point.normal_z.into(), + ], + } } } impl MetaNames<4> for PointXYZRGBNormal { - fn meta_names() -> [String; 4] { - ["rgb", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 4] { + ["rgb", "normal_x", "normal_y", "normal_z"] } } -impl PointConvertible for PointXYZRGBNormal {} +impl PointConvertible() }, 3, 4> for PointXYZRGBNormal {} /// 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. @@ -262,45 +252,41 @@ pub struct PointXYZINormal { pub normal_z: f32, } -impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZINormal { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { - Ok(( - [self.x, self.y, self.z], - [ - PointMeta::new(self.intensity), - PointMeta::new(self.normal_x), - PointMeta::new(self.normal_y), - PointMeta::new(self.normal_z), +impl From for Point { + fn from(point: PointXYZINormal) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [ + point.intensity.into(), + point.normal_x.into(), + point.normal_y.into(), + point.normal_z.into(), ], - )) + } } } -impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - intensity: data.1[0].get()?, - normal_x: data.1[1].get()?, - normal_y: data.1[2].get()?, - normal_z: data.1[3].get()?, - }) +impl From> for PointXYZINormal { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + intensity: point.meta[0].get(), + normal_x: point.meta[1].get(), + normal_y: point.meta[2].get(), + normal_z: point.meta[3].get(), + } } } impl MetaNames<4> for PointXYZINormal { - fn meta_names() -> [String; 4] { - ["intensity", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 4] { + ["intensity", "normal_x", "normal_y", "normal_z"] } } -impl PointConvertible for PointXYZINormal {} +impl PointConvertible() }, 3, 4> for PointXYZINormal {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a label. @@ -312,34 +298,33 @@ pub struct PointXYZL { pub label: u32, } -impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZL { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { - Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)])) +impl From> for PointXYZL { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + label: point.meta[0].get(), + } } } -impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - label: data.1[0].get()?, - }) +impl From for Point { + fn from(point: PointXYZL) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [point.label.into()], + } } } impl MetaNames<1> for PointXYZL { - fn meta_names() -> [String; 1] { - ["label".to_string()] + fn meta_names() -> [&'static str; 1] { + ["label"] } } -impl PointConvertible for PointXYZL {} +impl PointConvertible() }, 3, 1> for PointXYZL {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a label. @@ -354,43 +339,41 @@ pub struct PointXYZRGBL { pub label: u32, } -impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(( - [self.x, self.y, self.z], - [PointMeta::new(rgb), PointMeta::new(self.label)], - )) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBL { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGBL { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], r: rgb_unpacked[0], g: rgb_unpacked[1], b: rgb_unpacked[2], - label: data.1[1].get()?, - }) + label: point.meta[1].get(), + } + } +} + +impl From for Point { + fn from(point: PointXYZRGBL) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [ + pack_rgb(point.r, point.g, point.b).into(), + point.label.into(), + ], + } } } impl MetaNames<2> for PointXYZRGBL { - fn meta_names() -> [String; 2] { - ["rgb", "label"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 2] { + ["rgb", "label"] } } -impl PointConvertible for PointXYZRGBL {} +impl PointConvertible() }, 3, 2> for PointXYZRGBL {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a normal vector. @@ -404,40 +387,36 @@ pub struct PointXYZNormal { pub normal_z: f32, } -impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZNormal { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> { - Ok(( - [self.x, self.y, self.z], - [ - PointMeta::new(self.normal_x), - PointMeta::new(self.normal_y), - PointMeta::new(self.normal_z), - ], - )) +impl From> for PointXYZNormal { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + normal_x: point.meta[0].get(), + normal_y: point.meta[1].get(), + normal_z: point.meta[2].get(), + } } } -impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZNormal { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 3])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - normal_x: data.1[0].get()?, - normal_y: data.1[1].get()?, - normal_z: data.1[2].get()?, - }) +impl From for Point { + fn from(point: PointXYZNormal) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [ + point.normal_x.into(), + point.normal_y.into(), + point.normal_z.into(), + ], + } } } impl MetaNames<3> for PointXYZNormal { - fn meta_names() -> [String; 3] { - ["normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 3] { + ["normal_x", "normal_y", "normal_z"] } } -impl PointConvertible for PointXYZNormal {} +impl PointConvertible() }, 3, 3> for PointXYZNormal {} diff --git a/src/reader.rs b/src/reader.rs new file mode 100644 index 0000000..61aa956 --- /dev/null +++ b/src/reader.rs @@ -0,0 +1,422 @@ +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 = + Reader() }, DIM, METADIM, C>; + +/// Convenience type for a Reader that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type ReaderF64 = + Reader() }, DIM, METADIM, C>; + +/// Convenience type for a Reader that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type ReaderI8 = + Reader() }, DIM, METADIM, C>; + +/// Convenience type for a Reader that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type ReaderI16 = + Reader() }, DIM, METADIM, C>; + +/// Convenience type for a Reader that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type ReaderI32 = + Reader() }, DIM, METADIM, C>; + +/// Convenience type for a Reader that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type ReaderU8 = + Reader() }, DIM, METADIM, C>; + +/// Convenience type for a Reader that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type ReaderU16 = + Reader() }, DIM, METADIM, C>; + +/// Convenience type for a Reader that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type ReaderU32 = + Reader() }, DIM, METADIM, C>; + +/// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`). +/// Every additional meta data is ignored. +pub type ReaderXYZ = ReaderF32<3, 0, PointXYZ>; + +/// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`). +/// Every additional meta data is ignored. +pub type ReaderXYZI = ReaderF32<3, 1, PointXYZI>; + +/// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`). +/// Every additional meta data is ignored. +pub type ReaderXYZNormal = ReaderF32<3, 3, PointXYZNormal>; + +/// Provides the message as an Iterator over xyz coordinates and unpacked rgb (see `PointXYZRGB`). +/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. +/// Every additional meta data is ignored. +pub type ReaderXYZRGB = ReaderF32<3, 1, PointXYZRGB>; + +/// Provides the message as an Iterator over xyz coordinates and unpacked rgb and intensity (see `PointXYZRGBL`). +/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. +/// Every additional meta data is ignored. +pub type ReaderXYZRGBL = ReaderF32<3, 2, PointXYZRGBL>; + +/// Provides the message as an Iterator over xyz coordinates and unpacked rgb with additional alpha channel (see `PointXYZRGBA`). +/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. +/// Every additional meta data is ignored. +pub type ReaderXYZRGBA = ReaderF32<3, 2, PointXYZRGBA>; + +/// Provides the message as an Iterator over xyz coordinates and normal and unpacked rgb (see `PointXYZRGBNormal`). +/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. +/// Every additional meta data is ignored. +pub type ReaderXYZRGBNormal = ReaderF32<3, 4, PointXYZRGBNormal>; + +/// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`). +/// Every additional meta data is ignored. +pub type ReaderXYZINormal = ReaderF32<3, 4, PointXYZINormal>; + +/// Provides the message as an Iterator over xyz coordinates and intensity with additional label (see `PointXYZL`). +/// Every additional meta data is ignored. +/// The label is typically used for semantic segmentation. +pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>; + +/// The Reader provides a an iterator abstraction of the PointCloud2Msg. +/// +/// The iterator is defined at compile time, so the point has to be described via template arguments. +/// +/// # Predefined Readers +/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity. +/// +/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first. +/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data. +/// +/// ROS1 with rosrust: +/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback +/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); +/// +/// ROS2 with r2r: +/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); +/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); +/// +/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags. +/// +/// ## Example +/// ``` +/// use ros_pointcloud2::{ +/// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, +/// }; +/// +/// +/// let cloud_points: Vec = vec![ +/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, +/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, +/// ]; +/// +/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap(); +/// let convert = ReaderXYZ::try_from(msg).unwrap(); +/// // ^^^^^^^^^ conversion from PointCloud2Msg to Reader that implements Iterator +/// +/// convert.for_each(|point: PointXYZ| { +/// // do something with the point +/// }); +/// ``` +/// # Fully Custom Reader +/// When the predefined types are not enough (like sensor specific metadata), you can describe your Reader with the following template arguments: +/// - T: The coordinate type, e.g. f32 +/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this. +/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates. +/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field. +/// Afterwards, implement the PointConvertible trait for your custom point type. +/// +/// ## Example +/// ``` +/// use ros_pointcloud2::{ +/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta, +/// }; +/// +/// type Xyz = f32; // coordinate type +/// const XYZ_S: usize = size_of!(Xyz); +/// const DIM: usize = 3; // helper constant for dimensionality +/// const METADIM: usize = 1; // helper constant for meta data fields +/// +/// #[derive(Debug, PartialEq, Clone)] +/// struct CustomPoint { +/// x: f32, +/// y: f32, +/// z: f32, +/// i: u8, +/// } +/// +/// impl From> for CustomPoint { +/// fn from(point: Point) -> Self { +/// Self { +/// x: point.coords[0], +/// y: point.coords[1], +/// z: point.coords[2], +/// i: point.meta[0].get(), +/// } +/// } +/// } +/// +///impl From for Point { +/// fn from(point: CustomPoint) -> Self { +/// Point { +/// coords: [point.x, point.y, point.z], +/// meta: [ +/// point.i.into(), +/// ], +/// } +/// } +///} +/// +/// impl MetaNames for CustomPoint { +/// fn meta_names() -> [&'static str; METADIM] { +/// ["intensity"] +/// } +/// } +/// +/// impl PointConvertible for CustomPoint {} +/// +/// type MyReader = Reader; +/// // ^^^^^^^^ your custom Reader +/// type MyWriter = Writer; +/// ``` +pub struct Reader +where + T: FromBytes, + C: MetaNames, +{ + iteration: usize, + data: Vec, + point_step_size: usize, + cloud_length: usize, + offsets: Vec, + meta: Vec<(String, FieldDatatype)>, + endianness: Endianness, + phantom_t: std::marker::PhantomData, // internally used for byte and datatype conversions + phantom_c: std::marker::PhantomData, // internally used for meta names array +} + +/// The iterator implementation for the Reader struct. +/// The iterator is fallible because the data is read from a byte buffer inside the PointCloud2 message, which is inherently unsafe. +/// +/// See ConversionError for possible errors that can occur during iteration. +impl Iterator + for Reader +where + T: FromBytes, + C: PointConvertible, +{ + type Item = C; + + /// The size_hint is the length of the remaining elements and the maximum length of the iterator. + /// + /// PointCloud2 messages contain the length of the cloud, so we can prepare coming operations. + /// This hint is used inside common iterator functions like `collect>`, for example. + fn size_hint(&self) -> (usize, Option) { + (self.cloud_length - self.iteration, Some(self.cloud_length)) + } + + /// Get the data from the byte buffer and convert it to the predefined point. + /// It must keep track of the current iteration and the length of the cloud so it has to mutate self. + /// + /// The current point is then converted into the custom type. If the conversion fails, an error is returned. + fn next(&mut self) -> Option { + if self.iteration >= self.cloud_length { + return None; // iteration finished + } + + let mut xyz = [T::default(); DIM]; + xyz.iter_mut() + .zip(self.offsets.iter()) + .for_each(|(p_xyz, in_point_offset)| { + *p_xyz = load_loadable::( + (self.iteration * self.point_step_size) + in_point_offset, + &self.data, + &self.endianness, + ); + }); + + debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch"); + debug_assert!( + self.offsets.len() == DIM + METADIM, + "Offset length mismatch" + ); + + let mut meta = [PointMeta::default(); METADIM]; + meta.iter_mut() + .zip(self.offsets.iter().skip(DIM)) + .zip(self.meta.iter()) + .for_each(|((p_meta, in_point_offset), (_, meta_type))| { + let start = (self.iteration * self.point_step_size) + in_point_offset; + *p_meta = PointMeta::from_buffer(&self.data, start, meta_type); + }); + + self.iteration += 1; + + Some(C::from(Point { coords: xyz, meta })) + } +} + +impl TryFrom + for Reader +where + T: FromBytes, + C: MetaNames, +{ + type Error = ConversionError; + + /// Convert a PointCloud2Msg into a Reader. + /// Converting a PointCloud2Msg into a Reader is a fallible operation since the message can contain only 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 effectively a constant time complexity O(1) for practical purposes. + /// + /// # Example + /// ``` + /// use ros_pointcloud2::{ + /// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, + /// }; + /// + /// let cloud_points: Vec = vec![ + /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, + /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, + /// ]; + /// + /// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap(); + /// + /// let convert = ReaderXYZ::try_from(msg).unwrap(); + /// // ^^^^^^^^ conversion from PointCloud2Msg to Reader + /// ``` + fn try_from(cloud: PointCloud2Msg) -> Result { + if cloud.fields.len() < DIM { + return Err(ConversionError::NotEnoughFields); + } + + let xyz_field_type = T::field_datatype(); + + let mut has_x: Option = None; + let mut has_y: Option = None; + let mut has_z: Option = None; + + let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); METADIM]; + + let lower_meta_names = C::meta_names() + .iter() + .map(|x| x.to_lowercase()) + .collect::>(); + + for (idx, field) in cloud.fields.iter().enumerate() { + let lower_field_name = field.name.to_lowercase(); + match lower_field_name.as_str() { + "x" => has_x = Some(idx), + "y" => has_y = Some(idx), + "z" => has_z = Some(idx), + _ => { + if lower_meta_names.contains(&lower_field_name) { + let meta_idx = idx - DIM; + debug_assert!( + meta_idx < meta_with_offsets.len(), + "Meta data length mismatch" + ); + meta_with_offsets[meta_idx].0 = field.name.clone(); + meta_with_offsets[meta_idx].1 = field.datatype.try_into()?; + meta_with_offsets[meta_idx].2 = field.offset as usize; + } + } + } + } + + meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2)); + + debug_assert!( + meta_with_offsets.len() == METADIM, + "Meta data length mismatch" + ); + + let mut meta_offsets = [usize::default(); METADIM]; + let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM]; + + meta_with_offsets + .into_iter() + .zip(meta.iter_mut()) + .zip(meta_offsets.iter_mut()) + .for_each(|(((name, datatype, offset), meta), meta_offset)| { + *meta = (name, datatype); + *meta_offset = offset; + }); + + let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?; + let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?; + + let mut offsets = vec![x_field.offset as usize, y_field.offset as usize]; + + let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type); + match z_field { + Ok(z_field) => { + offsets.push(z_field.offset as usize); + } + Err(err) => match err { + ConversionError::NotEnoughFields => { + if DIM == 3 { + return Err(ConversionError::NotEnoughFields); + } + } + _ => return Err(err), + }, + } + + let endian = if cloud.is_bigendian { + Endianness::Big + } else { + Endianness::Little + }; + + if offsets.len() != DIM { + return Err(ConversionError::NotEnoughFields); + } + + offsets.extend(meta_offsets); + + if offsets.len() != DIM + METADIM { + return Err(ConversionError::NotEnoughFields); + } + + let point_step_size = cloud.point_step as usize; + let cloud_length = cloud.width as usize * cloud.height as usize; + if point_step_size * cloud_length != cloud.data.len() { + return Err(ConversionError::DataLengthMismatch); + } + + let last_offset = offsets.last().expect("Dimensionality is 0."); + + if let Some(last_meta) = meta.last() { + let size_with_last_meta = last_offset + last_meta.1.size(); + if size_with_last_meta > point_step_size { + return Err(ConversionError::DataLengthMismatch); + } + } else if last_offset + xyz_field_type.size() > point_step_size { + return Err(ConversionError::DataLengthMismatch); + } + + Ok(Self { + iteration: 0, + data: cloud.data, + point_step_size, + cloud_length: cloud.width as usize * cloud.height as usize, + offsets, + meta, + endianness: endian, + phantom_t: std::marker::PhantomData, + phantom_c: std::marker::PhantomData, + }) + } +} diff --git a/src/ros_types.rs b/src/ros_types.rs index 9d729b6..0e2bbfc 100644 --- a/src/ros_types.rs +++ b/src/ros_types.rs @@ -70,19 +70,19 @@ impl From for PointCloud2Msg { } #[cfg(feature = "r2r_msg")] -impl Into for PointCloud2Msg { - fn into(self) -> r2r::sensor_msgs::msg::PointCloud2 { +impl From for r2r::sensor_msgs::msg::PointCloud2 { + fn from(msg: PointCloud2Msg) -> Self { r2r::sensor_msgs::msg::PointCloud2 { header: r2r::std_msgs::msg::Header { stamp: r2r::builtin_interfaces::msg::Time { - sec: self.header.stamp.sec as i32, - nanosec: self.header.stamp.nsec, + sec: msg.header.stamp.sec as i32, + nanosec: msg.header.stamp.nsec, }, - frame_id: self.header.frame_id, + frame_id: msg.header.frame_id, }, - height: self.height, - width: self.width, - fields: self + height: msg.height, + width: msg.width, + fields: msg .fields .into_iter() .map(|field| r2r::sensor_msgs::msg::PointField { @@ -92,11 +92,11 @@ impl Into for PointCloud2Msg { count: field.count, }) .collect(), - is_bigendian: self.is_bigendian, - point_step: self.point_step, - row_step: self.row_step, - data: self.data, - is_dense: self.is_dense, + is_bigendian: msg.is_bigendian, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + is_dense: msg.is_dense, } } } @@ -135,20 +135,20 @@ impl From for PointCloud2Msg { } #[cfg(feature = "rosrust_msg")] -impl Into for PointCloud2Msg { - fn into(self) -> rosrust_msg::sensor_msgs::PointCloud2 { +impl From for rosrust_msg::sensor_msgs::PointCloud2 { + fn from(msg: PointCloud2Msg) -> Self { rosrust_msg::sensor_msgs::PointCloud2 { header: rosrust_msg::std_msgs::Header { - seq: self.header.seq, - stamp: rosrust::Time { - sec: self.header.stamp.sec, - nsec: self.header.stamp.nsec, + seq: msg.header.seq, + stamp: TimeMsg { + sec: msg.header.stamp.sec, + nsec: msg.header.stamp.nsec, }, - frame_id: self.header.frame_id, + frame_id: msg.header.frame_id, }, - height: self.height, - width: self.width, - fields: self + height: msg.height, + width: msg.width, + fields: msg .fields .into_iter() .map(|field| rosrust_msg::sensor_msgs::PointField { @@ -158,11 +158,11 @@ impl Into for PointCloud2Msg { count: field.count, }) .collect(), - is_bigendian: self.is_bigendian, - point_step: self.point_step, - row_step: self.row_step, - data: self.data, - is_dense: self.is_dense, + is_bigendian: msg.is_bigendian, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + is_dense: msg.is_dense, } } } diff --git a/src/writer.rs b/src/writer.rs new file mode 100644 index 0000000..3e2ddbe --- /dev/null +++ b/src/writer.rs @@ -0,0 +1,346 @@ +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 = + Writer() }, DIM, METADIM, C>; + +/// Convenience type for a Writer that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type WriterF64 = + Writer() }, DIM, METADIM, C>; + +/// Convenience type for a Writer that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type WriterI8 = + Writer() }, DIM, METADIM, C>; + +/// Convenience type for a Writer that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type WriterI16 = + Writer() }, DIM, METADIM, C>; + +/// Convenience type for a Writer that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type WriterI32 = + Writer() }, DIM, METADIM, C>; + +/// Convenience type for a Writer that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type WriterU8 = + Writer() }, DIM, METADIM, C>; + +/// Convenience type for a Writer that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type WriterU16 = + Writer() }, DIM, METADIM, C>; + +/// Convenience type for a Writer that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type. +pub type WriterU32 = + Writer() }, DIM, METADIM, C>; + +/// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`). +pub type WriterXYZ = WriterF32<3, 0, PointXYZ>; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`). +pub type WriterXYZI = WriterF32<3, 1, PointXYZI>; + +/// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`). +pub type WriterXYZNormal = WriterF32<3, 3, PointXYZNormal>; + +/// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`). +pub type WriterXYZRGB = WriterF32<3, 1, PointXYZRGB>; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`). +pub type WriterXYZRGBL = WriterF32<3, 2, PointXYZRGBL>; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels and alpha channel (see `PointXYZRGBA`). +pub type WriterXYZRGBA = WriterF32<3, 2, PointXYZRGBA>; + +/// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`). +pub type WriterXYZRGBNormal = WriterF32<3, 4, PointXYZRGBNormal>; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`). +pub type WriterXYZINormal = WriterF32<3, 4, PointXYZINormal>; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`). +pub type WriterXYZL = WriterF32<3, 1, PointXYZL>; + +/// The Write creates a PointCloud2Msg out of your point iterator. +/// +/// The iterator is defined at compile time, so the Point has to be described via template arguments. +/// +/// # Predefined Readers +/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity. +/// +/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first. +/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data. +/// +/// ROS1 with rosrust: +/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback +/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); +/// +/// ROS2 with r2r: +/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); +/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); +/// +/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags. +/// +/// ## Example +/// ``` +/// use ros_pointcloud2::{ +/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg, +/// }; +/// +/// +/// let cloud_points: Vec = vec![ +/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, +/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, +/// ]; +/// +/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap(); +/// // ^^^^^^^^^ creating PointCloud2Msg from an iterator +/// ``` +/// # Fully Custom Writer +/// When the predefined types are not enough (like sensor specific metadata), you can describe your Writer with the following template arguments: +/// - T: The coordinate type, e.g. f32 +/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this. +/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates. +/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field. +/// Afterwards, implement the PointConvertible trait for your custom point type. +/// +/// ## Example +/// ``` +/// use ros_pointcloud2::{ +/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta, +/// }; +/// +/// type Xyz = f32; // coordinate type +/// const XYZ_S: usize = size_of!(Xyz); +/// const DIM: usize = 3; // helper constant for dimensionality +/// const METADIM: usize = 1; // helper constant for meta data fields +/// +/// #[derive(Debug, PartialEq, Clone)] +/// struct CustomPoint { +/// x: f32, +/// y: f32, +/// z: f32, +/// i: u8, +/// } +/// +/// impl From> for CustomPoint { +/// fn from(point: Point) -> Self { +/// Self { +/// x: point.coords[0], +/// y: point.coords[1], +/// z: point.coords[2], +/// i: point.meta[0].get(), +/// } +/// } +/// } +/// +///impl From for Point { +/// fn from(point: CustomPoint) -> Self { +/// Point { +/// coords: [point.x, point.y, point.z], +/// meta: [ +/// point.i.into(), +/// ], +/// } +/// } +///} +/// +/// impl MetaNames for CustomPoint { +/// fn meta_names() -> [&'static str; METADIM] { +/// ["intensity"] +/// } +/// } +/// impl PointConvertible for CustomPoint {} +/// +/// type MyReader = Reader; +/// type MyWriter = Writer; +/// // ^^^^^^^^ your custom Writer +/// ``` +pub struct Writer +where + T: FromBytes, + C: PointConvertible, +{ + coordinates: Box>, + phantom_t: std::marker::PhantomData, +} + +impl TryInto + for Writer +where + T: FromBytes, + C: PointConvertible, +{ + type Error = ConversionError; + + /// Writes the points to a PointCloud2Msg. + /// + /// First use the `from` method for initializing the Writer. + /// Then use the `try_into` method to do the actual conversion. + /// + /// The operation is O(n) in time complexity where n is the number of points in the point cloud. + /// + /// # Example + /// ``` + /// use ros_pointcloud2::{ + /// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg, + /// }; + /// + /// let cloud_points: Vec = vec![ + /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, + /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, + /// ]; + /// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap(); + /// // ^^^^^^^^ ROS message conversion + /// ``` + fn try_into(mut self) -> Result { + if DIM > 3 { + return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time? + } + + let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack + + if DIM > 1 { + fields.push(PointFieldMsg { + name: "x".into(), + offset: 0, + datatype: T::field_datatype().into(), + count: 1, + }); + + fields.push(PointFieldMsg { + name: "y".into(), + offset: SIZE as u32, + datatype: T::field_datatype().into(), + count: 1, + }); + } + + if DIM == 3 { + fields.push(PointFieldMsg { + name: "z".into(), + offset: 2 * SIZE as u32, + datatype: T::field_datatype().into(), + count: 1, + }); + } + + let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?; + let point: Point = first_point.clone().into(); + let meta_names = C::meta_names(); + + let mut meta_offsets_acc = DIM as u32 * SIZE as u32; + for (meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) { + let datatype_code = meta_value.datatype.into(); + if FieldDatatype::try_from(datatype_code).is_err() { + return Err(ConversionError::UnsupportedFieldType); + } + + fields.push(PointFieldMsg { + name: meta_name.into(), + offset: meta_offsets_acc, + datatype: datatype_code, + count: 1, + }); + meta_offsets_acc += meta_value.datatype.size() as u32 + } + + let mut cloud = PointCloud2Msg { + point_step: fields.iter().fold(Default::default(), |acc, field| { + let field_type: FieldDatatype = field + .datatype + .try_into() + .expect("Unsupported field but checked before."); + let field_size = field_type.size(); + acc + field.count * field_size as u32 + }), + ..Default::default() + }; + + // actual point -> byte conversion -- O(n) + add_point_to_byte_buffer(first_point, &mut cloud)?; + for coords in self.coordinates { + add_point_to_byte_buffer(coords, &mut cloud)?; + } + + cloud.fields = fields; + cloud.height = 1; // everything is in one row (unstructured) + cloud.is_bigendian = false; // ROS default + cloud.is_dense = true; // ROS default + cloud.row_step = cloud.width * cloud.point_step; // Note: redundant but defined in PointCloud2 message + + Ok(cloud) + } +} + +impl + Writer +where + T: FromBytes, + C: PointConvertible, +{ + /// Create a Writer from any iterator that iterates over a template-defined point to a ROS message type. + /// First use the `from` method for initializing the Writer. + /// Then use the `try_into` method to do the actual conversion. + /// + /// The operation is O(n) in time complexity where n is the number of points in the point cloud. + /// + /// # Example + /// ``` + /// use ros_pointcloud2::{ + /// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg, + /// }; + /// + /// let cloud_points: Vec = vec![ + /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, + /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, + /// ]; + // let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap(); + /// // ^^^^ Writer creation + /// ``` + pub fn from(coordinates: impl IntoIterator + 'static) -> Self { + Self { + coordinates: Box::new(coordinates.into_iter()), + phantom_t: std::marker::PhantomData, + } + } +} + +#[inline(always)] +fn add_point_to_byte_buffer< + T: FromBytes, + const SIZE: usize, + const DIM: usize, + const METADIM: usize, + C: PointConvertible, +>( + coords: C, + cloud: &mut PointCloud2Msg, +) -> Result { + let point: Point = 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) +} \ No newline at end of file diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index c8c2a90..9d77b0a 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -1,31 +1,34 @@ -use fallible_iterator::FallibleIterator; -use ros_pointcloud2::mem_macros::size_of; use ros_pointcloud2::pcl_utils::*; +use ros_pointcloud2::reader::*; use ros_pointcloud2::ros_types::PointCloud2Msg; +use ros_pointcloud2::writer::*; use ros_pointcloud2::*; use std::fmt::Debug; -use std::{cmp, fmt}; -fn convert_from_into(cloud: Vec

) -where - C: FallibleIterator - + TryFrom - + TryFrom> - + TryInto, - ::Error: Debug, - >::Error: Debug, - >::Error: Debug, - >>::Error: Debug, - P: Clone + fmt::Debug + cmp::PartialEq, -{ - let copy = cloud.clone(); - let msg: Result = C::try_from(cloud).unwrap().try_into(); - assert!(msg.is_ok()); - let to_p_type = C::try_from(msg.unwrap()); - assert!(to_p_type.is_ok()); - let to_p_type = to_p_type.unwrap(); - let back_to_type = to_p_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); +macro_rules! convert_from_into { + ($reader:ty, $writer:ty, $point:ty, $cloud:expr) => { + convert_from_into_in_out_cloud!( + $reader, + $writer, + $point, + $cloud.clone(), + $point, + $cloud, + $point + ); + }; +} + +macro_rules! convert_from_into_in_out_cloud { + ($reader:ty, $writer:ty, $point:ty, $in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => { + let msg: Result = <$writer>::from($in_cloud).try_into(); + assert!(msg.is_ok()); + let to_p_type = <$reader>::try_from(msg.unwrap()); + assert!(to_p_type.is_ok()); + let to_p_type = to_p_type.unwrap(); + let back_to_type = to_p_type.collect::>(); + assert_eq!($out_cloud, back_to_type); + }; } #[test] @@ -39,50 +42,64 @@ fn custom_xyz_f32() { y: f32, z: f32, } - type MyConverter = Convert; - impl From for ([f32; DIM], [PointMeta; METADIM]) { + + type MyReader = ReaderF32; + type MyWriter = WriterF32; + + impl From> for CustomPoint { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + } + } + } + + impl From for Point { fn from(point: CustomPoint) -> Self { - ([point.x, point.y, point.z], []) - } - } - impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - }) + Point { + coords: [point.x, point.y, point.z], + meta: [], + } } } + impl MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { + fn meta_names() -> [&'static str; METADIM] { [] } } - impl PointConvertible for CustomPoint {} + impl PointConvertible() }, 3, 0> for CustomPoint {} - convert_from_into::(vec![ - CustomPoint { - x: 1.0, - y: 2.0, - z: 3.0, - }, - CustomPoint { - x: 4.0, - y: 5.0, - z: 6.0, - }, - CustomPoint { - x: 7.0, - y: 8.0, - z: 9.0, - }, - ]); + convert_from_into!( + MyReader, + MyWriter, + CustomPoint, + vec![ + CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + }, + CustomPoint { + x: 4.0, + y: 5.0, + z: 6.0, + }, + CustomPoint { + x: 7.0, + y: 8.0, + z: 9.0, + }, + ] + ); } #[test] fn custom_xyzi_f32() { + type Xyz = f32; + const XYZ_S: usize = std::mem::size_of::(); const DIM: usize = 3; const METADIM: usize = 1; #[derive(Debug, PartialEq, Clone)] @@ -92,30 +109,39 @@ fn custom_xyzi_f32() { z: f32, i: u8, } - type MyConverter = Convert; - impl From for ([f32; DIM], [PointMeta; METADIM]) { + + impl From> for CustomPoint { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + i: point.meta[0].get(), + } + } + } + + impl From for Point { fn from(point: CustomPoint) -> Self { - ([point.x, point.y, point.z], [PointMeta::new(point.i)]) - } - } - impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - i: data.1.first().unwrap().get().unwrap(), - }) + Point { + coords: [point.x, point.y, point.z], + meta: [point.i.into()], + } } } + impl MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { - ["intensity"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; METADIM] { + ["intensity"] } } - impl PointConvertible for CustomPoint {} - convert_from_into::(vec![ + + type MyReader = reader::Reader; + type MyWriter = writer::Writer; + + impl PointConvertible for CustomPoint {} + + let cloud = vec![ CustomPoint { x: 0.0, y: 1.0, @@ -140,7 +166,8 @@ fn custom_xyzi_f32() { z: f32::MAX, i: u8::MAX, }, - ]); + ]; + convert_from_into!(MyReader, MyWriter, CustomPoint, cloud); } #[test] @@ -157,41 +184,44 @@ fn custom_rgba_f32() { b: u8, a: u8, } - type MyConverter = Convert; - impl From for ([f32; DIM], [PointMeta; METADIM]) { + type MyReader = reader::Reader() }, DIM, METADIM, CustomPoint>; + type MyWriter = writer::Writer() }, DIM, METADIM, CustomPoint>; + + impl From> for CustomPoint { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + r: point.meta[0].get(), + g: point.meta[1].get(), + b: point.meta[2].get(), + a: point.meta[3].get(), + } + } + } + + impl From for Point { fn from(point: CustomPoint) -> Self { - ( - [point.x, point.y, point.z], - [ - PointMeta::new(point.r), - PointMeta::new(point.g), - PointMeta::new(point.b), - PointMeta::new(point.a), + Point { + coords: [point.x, point.y, point.z], + meta: [ + point.r.into(), + point.g.into(), + point.b.into(), + point.a.into(), ], - ) - } - } - impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - r: data.1[0].get()?, - g: data.1[1].get()?, - b: data.1[2].get()?, - a: data.1[3].get()?, - }) + } } } + impl MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { - ["r", "g", "b", "a"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; METADIM] { + ["r", "g", "b", "a"] } } - impl PointConvertible for CustomPoint {} - convert_from_into::(vec![ + impl PointConvertible() }, DIM, METADIM> for CustomPoint {} + let cloud = vec![ CustomPoint { x: 0.0, y: 1.0, @@ -228,7 +258,8 @@ fn custom_rgba_f32() { b: u8::MAX, a: u8::MAX, }, - ]); + ]; + convert_from_into!(MyReader, MyWriter, CustomPoint, cloud); } #[test] @@ -256,294 +287,364 @@ fn converterxyz() { }, ]; - convert_from_into::(cloud); + convert_from_into!(ReaderXYZ, WriterXYZ, PointXYZ, cloud); } #[test] fn converterxyzrgba() { - convert_from_into::(vec![ - PointXYZRGBA { - x: 0.0, - y: 1.0, - z: 5.0, - r: 0, - g: 0, - b: 0, - a: 0, - }, - PointXYZRGBA { - x: 1.0, - y: 1.5, - z: 5.0, - r: 1, - g: 1, - b: 1, - a: 1, - }, - PointXYZRGBA { - x: 1.3, - y: 1.6, - z: 5.7, - r: 2, - g: 2, - b: 2, - a: 2, - }, - PointXYZRGBA { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - r: u8::MAX, - g: u8::MAX, - b: u8::MAX, - a: u8::MAX, - }, - ]); + convert_from_into!( + ReaderXYZRGBA, + WriterXYZRGBA, + PointXYZRGBA, + vec![ + PointXYZRGBA { + x: 0.0, + y: 1.0, + z: 5.0, + r: 0, + g: 0, + b: 0, + a: 0, + }, + PointXYZRGBA { + x: 1.0, + y: 1.5, + z: 5.0, + r: 1, + g: 1, + b: 1, + a: 1, + }, + PointXYZRGBA { + x: 1.3, + y: 1.6, + z: 5.7, + r: 2, + g: 2, + b: 2, + a: 2, + }, + PointXYZRGBA { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + r: u8::MAX, + g: u8::MAX, + b: u8::MAX, + a: u8::MAX, + }, + ] + ); } #[test] fn converterxyzinormal() { - convert_from_into::(vec![ - PointXYZINormal { - x: 0.0, - y: 1.0, - z: 5.0, - intensity: 0.0, - normal_x: 0.0, - normal_y: 0.0, - normal_z: 0.0, - }, - PointXYZINormal { - x: 1.0, - y: 1.5, - z: 5.0, - intensity: 1.0, - normal_x: 1.0, - normal_y: 1.0, - normal_z: 1.0, - }, - PointXYZINormal { - x: 1.3, - y: 1.6, - z: 5.7, - intensity: 2.0, - normal_x: 2.0, - normal_y: 2.0, - normal_z: 2.0, - }, - PointXYZINormal { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - intensity: f32::MAX, - normal_x: f32::MAX, - normal_y: f32::MAX, - normal_z: f32::MAX, - }, - ]); + convert_from_into!( + ReaderXYZINormal, + WriterXYZINormal, + PointXYZINormal, + vec![ + PointXYZINormal { + x: 0.0, + y: 1.0, + z: 5.0, + intensity: 0.0, + normal_x: 0.0, + normal_y: 0.0, + normal_z: 0.0, + }, + PointXYZINormal { + x: 1.0, + y: 1.5, + z: 5.0, + intensity: 1.0, + normal_x: 1.0, + normal_y: 1.0, + normal_z: 1.0, + }, + PointXYZINormal { + x: 1.3, + y: 1.6, + z: 5.7, + intensity: 2.0, + normal_x: 2.0, + normal_y: 2.0, + normal_z: 2.0, + }, + PointXYZINormal { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + intensity: f32::MAX, + normal_x: f32::MAX, + normal_y: f32::MAX, + normal_z: f32::MAX, + }, + ] + ); } #[test] fn converterxyzrgbnormal() { - convert_from_into::(vec![ - PointXYZRGBNormal { - x: 0.0, - y: 1.0, - z: 5.0, - r: 0, - g: 0, - b: 0, - normal_x: 0.0, - normal_y: 0.0, - normal_z: 0.0, - }, - PointXYZRGBNormal { - x: 1.0, - y: 1.5, - z: 5.0, - r: 1, - g: 1, - b: 1, - normal_x: 1.0, - normal_y: 1.0, - normal_z: 1.0, - }, - PointXYZRGBNormal { - x: 1.3, - y: 1.6, - z: 5.7, - r: 2, - g: 2, - b: 2, - normal_x: 2.0, - normal_y: 2.0, - normal_z: 2.0, - }, - PointXYZRGBNormal { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - r: u8::MAX, - g: u8::MAX, - b: u8::MAX, - normal_x: f32::MAX, - normal_y: f32::MAX, - normal_z: f32::MAX, - }, - ]); + convert_from_into!( + ReaderXYZRGBNormal, + WriterXYZRGBNormal, + PointXYZRGBNormal, + vec![ + PointXYZRGBNormal { + x: 0.0, + y: 1.0, + z: 5.0, + r: 0, + g: 0, + b: 0, + normal_x: 0.0, + normal_y: 0.0, + normal_z: 0.0, + }, + PointXYZRGBNormal { + x: 1.0, + y: 1.5, + z: 5.0, + r: 1, + g: 1, + b: 1, + normal_x: 1.0, + normal_y: 1.0, + normal_z: 1.0, + }, + PointXYZRGBNormal { + x: 1.3, + y: 1.6, + z: 5.7, + r: 2, + g: 2, + b: 2, + normal_x: 2.0, + normal_y: 2.0, + normal_z: 2.0, + }, + PointXYZRGBNormal { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + r: u8::MAX, + g: u8::MAX, + b: u8::MAX, + normal_x: f32::MAX, + normal_y: f32::MAX, + normal_z: f32::MAX, + }, + ] + ); } #[test] fn converterxyznormal() { - convert_from_into::(vec![ - PointXYZNormal { - x: 0.0, - y: 1.0, - z: 5.0, - normal_x: 0.0, - normal_y: 0.0, - normal_z: 0.0, - }, - PointXYZNormal { - x: 1.0, - y: 1.5, - z: 5.0, - normal_x: 1.0, - normal_y: 1.0, - normal_z: 1.0, - }, - PointXYZNormal { - x: 1.3, - y: 1.6, - z: 5.7, - normal_x: 2.0, - normal_y: 2.0, - normal_z: 2.0, - }, - PointXYZNormal { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - normal_x: f32::MAX, - normal_y: f32::MAX, - normal_z: f32::MAX, - }, - ]); + convert_from_into!( + ReaderXYZNormal, + WriterXYZNormal, + PointXYZNormal, + vec![ + PointXYZNormal { + x: 0.0, + y: 1.0, + z: 5.0, + normal_x: 0.0, + normal_y: 0.0, + normal_z: 0.0, + }, + PointXYZNormal { + x: 1.0, + y: 1.5, + z: 5.0, + normal_x: 1.0, + normal_y: 1.0, + normal_z: 1.0, + }, + PointXYZNormal { + x: 1.3, + y: 1.6, + z: 5.7, + normal_x: 2.0, + normal_y: 2.0, + normal_z: 2.0, + }, + PointXYZNormal { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + normal_x: f32::MAX, + normal_y: f32::MAX, + normal_z: f32::MAX, + }, + ] + ); } #[test] fn converterxyzrgbl() { - convert_from_into::(vec![ - PointXYZRGBL { - x: 0.0, - y: 1.0, - z: 5.0, - r: 0, - g: 0, - b: 0, - label: 0, - }, - PointXYZRGBL { - x: 1.0, - y: 1.5, - z: 5.0, - r: 1, - g: 1, - b: 1, - label: 1, - }, - PointXYZRGBL { - x: 1.3, - y: 1.6, - z: 5.7, - r: 2, - g: 2, - b: 2, - label: 2, - }, - PointXYZRGBL { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - r: u8::MAX, - g: u8::MAX, - b: u8::MAX, - label: u32::MAX, - }, - ]); + convert_from_into!( + ReaderXYZRGBL, + WriterXYZRGBL, + PointXYZRGBL, + vec![ + PointXYZRGBL { + x: 0.0, + y: 1.0, + z: 5.0, + r: 0, + g: 0, + b: 0, + label: 0, + }, + PointXYZRGBL { + x: 1.0, + y: 1.5, + z: 5.0, + r: 1, + g: 1, + b: 1, + label: 1, + }, + PointXYZRGBL { + x: 1.3, + y: 1.6, + z: 5.7, + r: 2, + g: 2, + b: 2, + label: 2, + }, + PointXYZRGBL { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + r: u8::MAX, + g: u8::MAX, + b: u8::MAX, + label: u32::MAX, + }, + ] + ); } #[test] fn converterxyzrgb() { - convert_from_into::(vec![ - PointXYZRGB { - x: 0.0, - y: 1.0, - z: 5.0, - r: 0, - g: 0, - b: 0, - }, - PointXYZRGB { - x: 1.0, - y: 1.5, - z: 5.0, - r: 1, - g: 1, - b: 1, - }, - PointXYZRGB { - x: 1.3, - y: 1.6, - z: 5.7, - r: 2, - g: 2, - b: 2, - }, - PointXYZRGB { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - r: u8::MAX, - g: u8::MAX, - b: u8::MAX, - }, - ]); + convert_from_into!( + ReaderXYZRGB, + WriterXYZRGB, + PointXYZRGB, + vec![ + PointXYZRGB { + x: 0.0, + y: 1.0, + z: 5.0, + r: 0, + g: 0, + b: 0, + }, + PointXYZRGB { + x: 1.0, + y: 1.5, + z: 5.0, + r: 1, + g: 1, + b: 1, + }, + PointXYZRGB { + x: 1.3, + y: 1.6, + z: 5.7, + r: 2, + g: 2, + b: 2, + }, + PointXYZRGB { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + r: u8::MAX, + g: u8::MAX, + b: u8::MAX, + }, + ] + ); } #[test] fn converterxyzl() { - convert_from_into::(vec![ - PointXYZL { - x: 0.0, - y: 1.0, - z: 5.0, - label: 0, - }, - PointXYZL { - x: 1.0, - y: 1.5, - z: 5.0, - label: 1, - }, - PointXYZL { - x: 1.3, - y: 1.6, - z: 5.7, - label: 2, - }, - PointXYZL { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - label: u32::MAX, - }, - ]); + convert_from_into!( + ReaderXYZL, + WriterXYZL, + PointXYZL, + vec![ + PointXYZL { + x: 0.0, + y: 1.0, + z: 5.0, + label: 0, + }, + PointXYZL { + x: 1.0, + y: 1.5, + z: 5.0, + label: 1, + }, + PointXYZL { + x: 1.3, + y: 1.6, + z: 5.7, + label: 2, + }, + PointXYZL { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + label: u32::MAX, + }, + ] + ); } #[test] fn converterxyzi() { - convert_from_into::(vec![ + convert_from_into!( + ReaderXYZI, + WriterXYZI, + PointXYZI, + vec![ + PointXYZI { + x: 0.0, + y: 1.0, + z: 5.0, + intensity: 0.0, + }, + PointXYZI { + x: 1.0, + y: 1.5, + z: 5.0, + intensity: 1.0, + }, + PointXYZI { + x: 1.3, + y: 1.6, + z: 5.7, + intensity: 2.0, + }, + PointXYZI { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + intensity: f32::MAX, + }, + ] + ); +} + +#[test] +fn write_xyzi_read_xyz() { + let write_cloud = vec![ PointXYZI { x: 0.0, y: 1.0, @@ -568,5 +669,183 @@ fn converterxyzi() { z: f32::MAX, intensity: f32::MAX, }, - ]); + ]; + + let read_cloud = vec![ + PointXYZ { + x: 0.0, + y: 1.0, + z: 5.0, + }, + PointXYZ { + x: 1.0, + y: 1.5, + z: 5.0, + }, + PointXYZ { + x: 1.3, + y: 1.6, + z: 5.7, + }, + PointXYZ { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + }, + ]; + + convert_from_into_in_out_cloud!( + ReaderXYZ, + WriterXYZI, + PointXYZI, + write_cloud, + PointXYZI, + read_cloud, + PointXYZ + ); +} + +#[test] +fn write_less_than_available() { + const DIM: usize = 3; + const METADIM: usize = 0; + + #[derive(Debug, PartialEq, Clone)] + struct CustomPoint { + x: f32, + y: f32, + z: f32, + dummy: f32, + } + + type MyReader = ReaderF32; + type MyWriter = WriterF32; + + impl From> for CustomPoint { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + dummy: 0.0, + } + } + } + + impl From for Point { + fn from(point: CustomPoint) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [], + } + } + } + + impl MetaNames for CustomPoint { + fn meta_names() -> [&'static str; METADIM] { + [] + } + } + impl PointConvertible() }, 3, 0> for CustomPoint {} + + let write_cloud = vec![ + CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + dummy: -10.0, + }, + CustomPoint { + x: 4.0, + y: 5.0, + z: 6.0, + dummy: -10.0, + }, + CustomPoint { + x: 7.0, + y: 8.0, + z: 9.0, + dummy: -10.0, + }, + ]; + + let read_cloud = vec![ + CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + dummy: 0.0, + }, + CustomPoint { + x: 4.0, + y: 5.0, + z: 6.0, + dummy: 0.0, + }, + CustomPoint { + x: 7.0, + y: 8.0, + z: 9.0, + dummy: 0.0, + }, + ]; + + convert_from_into_in_out_cloud!( + MyReader, + MyWriter, + CustomPoint, + write_cloud, + CustomPoint, + read_cloud, + CustomPoint + ); +} + +#[test] +fn readme() { + use ros_pointcloud2::{ + pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, + }; + + // Your points (here using the predefined type PointXYZ). + let cloud_points = vec![ + PointXYZ { + x: 1337.0, + y: 42.0, + z: 69.0, + }, + PointXYZ { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + }, + ]; + + // For equality test later + let cloud_copy = cloud_points.clone(); + + // Vector -> Writer -> Message + let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points) + .try_into() // iterating points here O(n) + .unwrap(); + + // Convert to your ROS crate message type, we will use r2r here. + // let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); + + // Publish ... + + // ... now incoming from a topic. + // let internal_msg: PointCloud2Msg = msg.into(); + + // Message -> Reader. The Reader implements the Iterator trait. + let reader = ReaderXYZ::try_from(internal_msg).unwrap(); + let new_cloud_points = reader + .map(|point: PointXYZ| { + // Some logic here + + point + }) + .collect::>(); + + assert_eq!(new_cloud_points, cloud_copy); } diff --git a/tests/r2r_msg_test.rs b/tests/r2r_msg_test.rs index fb430ed..1c4540b 100644 --- a/tests/r2r_msg_test.rs +++ b/tests/r2r_msg_test.rs @@ -1,10 +1,10 @@ #[cfg(feature = "r2r_msg")] #[test] fn convertxyz_r2r_msg() { - use ros_pointcloud2::fallible_iterator::FallibleIterator; - use ros_pointcloud2::pcl_utils::PointXYZ; - use ros_pointcloud2::ros_types::PointCloud2Msg; - use ros_pointcloud2::ConvertXYZ; + use ros_pointcloud2::{ + pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, + }; + use r2r::sensor_msgs::msg::PointCloud2; let cloud = vec![ @@ -25,10 +25,10 @@ fn convertxyz_r2r_msg() { }, ]; let copy = cloud.clone(); - let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); + let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap(); let r2r_msg_cloud: PointCloud2 = internal_cloud.into(); let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into(); - let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap(); - let back_to_type = to_convert.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap(); + let back_to_type = to_convert.collect::>(); + assert_eq!(copy, back_to_type); } diff --git a/tests/rosrust_msg_test.rs b/tests/rosrust_msg_test.rs index d519947..03d7fef 100644 --- a/tests/rosrust_msg_test.rs +++ b/tests/rosrust_msg_test.rs @@ -1,10 +1,9 @@ #[cfg(feature = "rosrust_msg")] #[test] fn convertxyz_rosrust_msg() { - use ros_pointcloud2::fallible_iterator::FallibleIterator; - use ros_pointcloud2::pcl_utils::PointXYZ; - use ros_pointcloud2::ros_types::PointCloud2Msg; - use ros_pointcloud2::ConvertXYZ; + use ros_pointcloud2::{ + pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, + }; let cloud = vec![ PointXYZ { @@ -24,10 +23,10 @@ fn convertxyz_rosrust_msg() { }, ]; let copy = cloud.clone(); - let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); + let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap(); let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into(); let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into(); - let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap(); - let back_to_type = to_convert.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap(); + let back_to_type = to_convert.collect::>(); + assert_eq!(copy, back_to_type); }