diff --git a/Cargo.toml b/Cargo.toml index a40cbe0..09b7f22 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "ros_pointcloud2" -version = "0.2.0" +version = "0.2.1" edition = "2021" authors = ["Christopher Sieh "] description = "Customizable conversions for working with sensor_msgs/PointCloud2." diff --git a/README.md b/README.md index 8c5ebf9..89514b7 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,13 @@ # ROS PointCloud2 -A lightweight Rust implementation for fast, safe and customizable conversions to and from the `sensor_msgs/PointCloud2` ROS message. +Customizable conversions to and from the `sensor_msgs/PointCloud2` ROS message. ```toml [dependencies] -ros_pointcloud2 = "0.2.0" +ros_pointcloud2 = "0.2.1" ``` -Providing a fast and memory efficient way for message conversion while allowing user defined types without the cost of iterations. +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 `Iterator` that converts each point from the message on the fly. An example for using this crate is [this filter node](https://github.com/stelzo/cloudfilter). It is also a good starting point for @@ -52,7 +52,7 @@ assert_eq!(new_cloud_points, cloud_copy); To use `ros_pointcloud2` in your favorite ROS crate, you can either use this crate's features (see Integration section below) or implement the `Into` and `From` traits for `PointCloud2Msg`. -Please avoid cloning the `data: Vec` of the message. +Try to avoid cloning the `data: Vec` field. ```rust use ros_pointcloud2::ros_types::PointCloud2Msg; @@ -73,13 +73,13 @@ impl From for PointCloud2Msg { ## Integrations -Currently, we only implement a conversion for the following ROS crate: +Currently, there is only 1 integration for the following ROS crate: - [rosrust](https://github.com/adnanademovic/rosrust) You can use one by enabling the corresponding feature. ```toml [dependencies] -ros_pointcloud2 = { version = "0.2.0", features = ["rosrust_msg"]} +ros_pointcloud2 = { version = "0.2.1", features = ["rosrust_msg"]} ``` Please open an issue or PR if you want to see support for other crates. diff --git a/src/lib.rs b/src/lib.rs index 9cbedf6..2f1ef27 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2,9 +2,9 @@ pub mod pcl_utils; pub mod ros_types; -use num_traits::Zero; use crate::pcl_utils::*; use crate::ros_types::{PointCloud2Msg, PointFieldMsg}; +use num_traits::Zero; #[macro_use] pub extern crate mem_macros; @@ -28,8 +28,15 @@ pub enum ConversionError { /// 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 {} - +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)] @@ -56,7 +63,9 @@ impl GetFieldDatatype for f32 { } impl GetFieldDatatype for f64 { - fn field_datatype() -> FieldDatatype { FieldDatatype::F64 } + fn field_datatype() -> FieldDatatype { + FieldDatatype::F64 + } } impl GetFieldDatatype for i32 { @@ -122,7 +131,11 @@ fn convert_msg_code_to_type(code: u8) -> Result } } -fn check_coord(coord: Option, fields: &Vec, xyz_field_type: &FieldDatatype) -> Result { +fn check_coord( + coord: Option, + fields: &Vec, + xyz_field_type: &FieldDatatype, +) -> Result { match coord { Some(y_idx) => { let field = &fields[y_idx]; @@ -130,7 +143,7 @@ fn check_coord(coord: Option, fields: &Vec, xyz_field_type return Err(ConversionError::InvalidFieldFormat); } Ok(field.clone()) - }, + } None => { return Err(ConversionError::NotEnoughFields); } @@ -157,7 +170,11 @@ pub trait MetaNames { /// const METADIM : usize = 4; // how many meta fields you have (e.g. r, g, b, a) /// type MyConverter = Convert; /// ``` -pub struct Convert> { +pub struct Convert +where + T: FromBytes, + C: PointConvertible, +{ iteration: usize, coordinates: Vec, phantom_t: std::marker::PhantomData, @@ -170,31 +187,21 @@ pub struct Convert, } -/// X, Y, Z in f32 (float) without meta data. pub type ConvertXYZ = Convert; - -/// X, Y, Z in f32 (float), meta: intensity in f32. pub type ConvertXYZI = Convert; - -/// X, Y, Z in f32 (float), meta: normal_x, normal_y, normal_z in f32. pub type ConvertXYZNormal = Convert; - -/// X, Y, Z in f32 (float), meta: r, g, b in u8. -pub type ConvertXYZRGB = Convert; - -/// X, Y, Z in f32 (float), meta: r, g, b, a in u8. -pub type ConvertXYZRGBA = Convert; - -/// X, Y, Z in f32 (float), meta: r, g, b in u8, normal_x, normal_y, normal_z in f32. -pub type ConvertXYZRGBNormal = Convert; - -/// X, Y, Z in f32 (float), meta: intensity in f32, normal_x, normal_y, normal_z in f32. +pub type ConvertXYZRGB = Convert; +pub type ConvertXYZRGBL = Convert; +pub type ConvertXYZRGBA = Convert; +pub type ConvertXYZRGBNormal = Convert; pub type ConvertXYZINormal = Convert; - -/// X, Y, Z in f32 (float), meta: label in u32. pub type ConvertXYZL = Convert; -impl> TryFrom> for Convert +impl TryFrom> + for Convert +where + T: FromBytes, + C: PointConvertible, { type Error = ConversionError; @@ -225,7 +232,13 @@ impl, offset: usize, datatype: &FieldDatatype) -> Result { + fn new_from_buffer( + data: &Vec, + offset: usize, + datatype: &FieldDatatype, + ) -> Result { let size = datatype_size(datatype); - let bytes = data.get(offset..offset + size).ok_or(ConversionError::DataLengthMismatch)?; + 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; @@ -325,7 +344,11 @@ impl PointMeta { } } -impl> TryFrom for Convert +impl TryFrom + for Convert +where + T: FromBytes, + C: PointConvertible, { type Error = ConversionError; @@ -359,9 +382,13 @@ impl = None; let mut has_z: Option = None; - let mut meta_with_offsets: Vec<(String, FieldDatatype, usize)> = Vec::with_capacity(METADIM); + 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::>(); + 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() { @@ -370,7 +397,11 @@ impl 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.push(( + field.name.clone(), + convert_msg_code_to_type(field.datatype)?, + field.offset as usize, + )); } } } @@ -378,31 +409,29 @@ impl = 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.clone())).collect(); + let meta: Vec<(String, FieldDatatype)> = meta_with_offsets + .iter() + .map(|x| (x.0.clone(), x.1.clone())) + .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 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), - } } + Err(err) => match err { + ConversionError::NotEnoughFields => { + if DIM == 3 { + return Err(ConversionError::NotEnoughFields); + } + } + _ => return Err(err), + }, } let endian = if cloud.is_bigendian { @@ -463,7 +492,12 @@ fn datatype_size(datatype: &FieldDatatype) -> usize { } } -impl> TryInto for Convert { +impl TryInto + for Convert +where + T: FromBytes, + C: PointConvertible, +{ type Error = ConversionError; /// Convert the point cloud to a ROS message. @@ -566,7 +600,10 @@ impl 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 + .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); @@ -577,7 +614,11 @@ impl> Convert +impl + Convert +where + T: FromBytes, + C: PointConvertible, { /// Convenience getter for the number of points in the cloud. pub fn len(&self) -> usize { @@ -585,7 +626,11 @@ impl> fallible_iterator::FallibleIterator for Convert +impl + fallible_iterator::FallibleIterator for Convert +where + T: FromBytes, + C: PointConvertible, { type Item = C; type Error = ConversionError; @@ -651,7 +696,6 @@ impl FromBytes for i8 { } } - impl FromBytes for i16 { fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0], bytes[1]]) @@ -788,7 +832,6 @@ fn load_bytes(start_idx: usize, data: &[u8]) -> Option<[u8; S]> Some(buff) } - #[cfg(test)] mod tests { use super::*; @@ -804,5 +847,4 @@ mod tests { f32::bytes(&1.0); f64::bytes(&1.0); } - } diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs index b1da6e0..6a76037 100644 --- a/src/pcl_utils.rs +++ b/src/pcl_utils.rs @@ -1,5 +1,18 @@ use crate::{ConversionError, MetaNames, PointConvertible, PointMeta}; +#[inline] +fn pack_rgb(r: u8, g: u8, b: u8) -> f32 { + ((r as u32) << 16) as f32 + ((g as u32) << 8) as f32 + (b as u32) as f32 +} + +#[inline] +fn unpack_rgb(rgb: f32) -> [u8; 3] { + let r: u8 = ((rgb as u32) >> 16) as u8; + let g: u8 = ((rgb as u32) >> 8) as u8; + let b: u8 = (rgb as u32) as u8; + [r, g, b] +} + /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates. #[derive(Clone, Debug, PartialEq, Copy)] @@ -35,8 +48,7 @@ impl MetaNames<0> for PointXYZ { } } -impl PointConvertible for PointXYZ {} - +impl PointConvertible 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. @@ -64,7 +76,7 @@ impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZI { x: data.0[0], y: data.0[1], z: data.0[2], - intensity: data.1[0].get().unwrap(), + intensity: data.1[0].get()?, }) } } @@ -75,7 +87,7 @@ impl MetaNames<1> for PointXYZI { } } -impl PointConvertible for PointXYZI {} +impl PointConvertible 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. @@ -89,40 +101,39 @@ pub struct PointXYZRGB { pub b: u8, } -impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZRGB { +impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB { type Error = ConversionError; - fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> { - Ok(([self.x, self.y, self.z], [ - PointMeta::new(self.r), - PointMeta::new(self.g), - PointMeta::new(self.b), - ])) + 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; 3])> for PointXYZRGB { +impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZRGB { type Error = ConversionError; - fn try_from(data: ([f32; 3], [PointMeta; 3])) -> Result { + fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result { + let rgb = data.1[0].get::()?; + let rgb_unpacked = unpack_rgb(rgb); Ok(Self { x: data.0[0], y: data.0[1], z: data.0[2], - r: data.1[0].get().unwrap(), - g: data.1[1].get().unwrap(), - b: data.1[2].get().unwrap(), + r: rgb_unpacked[0], + g: rgb_unpacked[1], + b: rgb_unpacked[2], }) } } -impl MetaNames<3> for PointXYZRGB { - fn meta_names() -> [String; 3] { - ["r", "g", "b"].map(|s| s.to_string()) +impl MetaNames<1> for PointXYZRGB { + fn meta_names() -> [String; 1] { + ["rgb"].map(|s| s.to_string()) } } -impl PointConvertible for PointXYZRGB {} +impl PointConvertible 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. @@ -138,47 +149,43 @@ pub struct PointXYZRGBA { pub a: u8, } -impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBA { +impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA { type Error = ConversionError; - fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { - Ok(([self.x, self.y, self.z], [ - PointMeta::new(self.r), - PointMeta::new(self.g), - PointMeta::new(self.b), - PointMeta::new(self.a), - ])) + 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; 4])> for PointXYZRGBA { +impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBA { type Error = ConversionError; - fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result { + fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result { + let rgb = data.1[0].get::()?; + let rgb_unpacked = unpack_rgb(rgb); Ok(Self { x: data.0[0], y: data.0[1], z: data.0[2], - r: data.1[0].get().unwrap(), - g: data.1[1].get().unwrap(), - b: data.1[2].get().unwrap(), - a: data.1[3].get().unwrap(), + r: rgb_unpacked[0], + g: rgb_unpacked[1], + b: rgb_unpacked[2], + a: data.1[1].get()?, }) } } -impl MetaNames<4> for PointXYZRGBA { - fn meta_names() -> [String; 4] { - [ - "r", - "g", - "b", - "a", - ].map(|s| s.to_string()) +impl MetaNames<2> for PointXYZRGBA { + fn meta_names() -> [String; 2] { + ["rgb", "a"].map(|s| s.to_string()) } } -impl PointConvertible for PointXYZRGBA {} +impl PointConvertible 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. @@ -195,53 +202,50 @@ pub struct PointXYZRGBNormal { pub normal_z: f32, } -impl TryInto<([f32; 3], [PointMeta; 6])> for PointXYZRGBNormal { +impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal { type Error = ConversionError; - fn try_into(self) -> Result<([f32; 3], [PointMeta; 6]), Self::Error> { - Ok(([self.x, self.y, self.z], [ - PointMeta::new(self.r), - PointMeta::new(self.g), - PointMeta::new(self.b), - PointMeta::new(self.normal_x), - PointMeta::new(self.normal_y), - PointMeta::new(self.normal_z), - ])) + 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; 6])> for PointXYZRGBNormal { +impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal { type Error = ConversionError; - fn try_from(data: ([f32; 3], [PointMeta; 6])) -> Result { + fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result { + let rgb = data.1[0].get::()?; + let rgb_unpacked = unpack_rgb(rgb); Ok(Self { x: data.0[0], y: data.0[1], z: data.0[2], - r: data.1[0].get().unwrap(), - g: data.1[1].get().unwrap(), - b: data.1[2].get().unwrap(), - normal_x: data.1[3].get().unwrap(), - normal_y: data.1[4].get().unwrap(), - normal_z: data.1[5].get().unwrap(), + 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()?, }) } } -impl MetaNames<6> for PointXYZRGBNormal { - fn meta_names() -> [String; 6] { - [ - "r", - "g", - "b", - "normal_x", - "normal_y", - "normal_z", - ].map(|s| s.to_string()) +impl MetaNames<4> for PointXYZRGBNormal { + fn meta_names() -> [String; 4] { + ["rgb", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) } } -impl PointConvertible for PointXYZRGBNormal {} +impl PointConvertible 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. @@ -260,12 +264,15 @@ 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), - ])) + 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), + ], + )) } } @@ -277,26 +284,21 @@ impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal { x: data.0[0], y: data.0[1], z: data.0[2], - intensity: data.1[0].get().unwrap(), - normal_x: data.1[1].get().unwrap(), - normal_y: data.1[2].get().unwrap(), - normal_z: data.1[3].get().unwrap(), + intensity: data.1[0].get()?, + normal_x: data.1[1].get()?, + normal_y: data.1[2].get()?, + normal_z: data.1[3].get()?, }) } } impl MetaNames<4> for PointXYZINormal { fn meta_names() -> [String; 4] { - [ - "intensity", - "normal_x", - "normal_y", - "normal_z", - ].map(|s| s.to_string()) + ["intensity", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) } } -impl PointConvertible for PointXYZINormal {} +impl PointConvertible 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,9 +314,7 @@ 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), - ])) + Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)])) } } @@ -326,7 +326,7 @@ impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL { x: data.0[0], y: data.0[1], z: data.0[2], - label: data.1[0].get().unwrap(), + label: data.1[0].get()?, }) } } @@ -337,7 +337,7 @@ impl MetaNames<1> for PointXYZL { } } -impl PointConvertible for PointXYZL {} +impl PointConvertible for PointXYZL {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a label. @@ -352,47 +352,43 @@ pub struct PointXYZRGBL { pub label: u32, } -impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBL { +impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL { type Error = ConversionError; - fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { - Ok(([self.x, self.y, self.z], [ - PointMeta::new(self.r), - PointMeta::new(self.g), - PointMeta::new(self.b), - PointMeta::new(self.label), - ])) + 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; 4])> for PointXYZRGBL { +impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBL { type Error = ConversionError; - fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result { + fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result { + let rgb = data.1[0].get::()?; + let rgb_unpacked = unpack_rgb(rgb); Ok(Self { x: data.0[0], y: data.0[1], z: data.0[2], - r: data.1[0].get().unwrap(), - g: data.1[1].get().unwrap(), - b: data.1[2].get().unwrap(), - label: data.1[3].get().unwrap(), + r: rgb_unpacked[0], + g: rgb_unpacked[1], + b: rgb_unpacked[2], + label: data.1[1].get()?, }) } } -impl MetaNames<4> for PointXYZRGBL { - fn meta_names() -> [String; 4] { - [ - "r", - "g", - "b", - "label", - ].map(|s| s.to_string()) +impl MetaNames<2> for PointXYZRGBL { + fn meta_names() -> [String; 2] { + ["rgb", "label"].map(|s| s.to_string()) } } -impl PointConvertible for PointXYZRGBL {} +impl PointConvertible 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. @@ -410,11 +406,14 @@ 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), - ])) + Ok(( + [self.x, self.y, self.z], + [ + PointMeta::new(self.normal_x), + PointMeta::new(self.normal_y), + PointMeta::new(self.normal_z), + ], + )) } } @@ -426,21 +425,17 @@ impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZNormal { x: data.0[0], y: data.0[1], z: data.0[2], - normal_x: data.1[0].get().unwrap(), - normal_y: data.1[1].get().unwrap(), - normal_z: data.1[2].get().unwrap(), + normal_x: data.1[0].get()?, + normal_y: data.1[1].get()?, + normal_z: data.1[2].get()?, }) } } impl MetaNames<3> for PointXYZNormal { fn meta_names() -> [String; 3] { - [ - "normal_x", - "normal_y", - "normal_z", - ].map(|s| s.to_string()) + ["normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) } } -impl PointConvertible for PointXYZNormal {} +impl PointConvertible for PointXYZNormal {} diff --git a/src/ros_types.rs b/src/ros_types.rs index 2fc2c1e..8f8ade1 100644 --- a/src/ros_types.rs +++ b/src/ros_types.rs @@ -6,10 +6,7 @@ pub struct TimeMsg { impl Default for TimeMsg { fn default() -> Self { - Self { - sec: 0, - nsec: 0, - } + Self { sec: 0, nsec: 0 } } } @@ -143,4 +140,3 @@ impl Into for PointCloud2Msg { } } } - diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index b444cff..97d4bfa 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -1,13 +1,37 @@ -use ros_pointcloud2::{ConversionError, Convert, ConvertXYZ, ConvertXYZINormal, ConvertXYZRGBA, ConvertXYZRGBNormal, MetaNames, PointConvertible, PointMeta}; -use ros_pointcloud2::mem_macros::size_of; -use ros_pointcloud2::pcl_utils::{PointXYZ, PointXYZINormal, PointXYZRGBA, PointXYZRGBNormal}; use fallible_iterator::FallibleIterator; +use ros_pointcloud2::mem_macros::size_of; +use ros_pointcloud2::pcl_utils::*; +use ros_pointcloud2::ros_types::PointCloud2Msg; +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()); +} #[test] fn custom_xyz_f32() { - const DIM : usize = 3; - const METADIM : usize = 0; + const DIM: usize = 3; + const METADIM: usize = 0; #[derive(Debug, PartialEq, Clone)] struct CustomPoint { @@ -38,25 +62,29 @@ fn custom_xyz_f32() { } impl PointConvertible for CustomPoint {} - let custom_cloud = 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 }, - ]; - let copy = custom_cloud.clone(); - let custom_msg: Result = MyConverter::try_from(custom_cloud).unwrap().try_into(); - assert!(custom_msg.is_ok()); - let to_custom_type = MyConverter::try_from(custom_msg.unwrap()); - assert!(to_custom_type.is_ok()); - let to_custom_type = to_custom_type.unwrap().map(|point| Ok(point)).collect::>(); - assert_eq!(copy, to_custom_type.unwrap()); + 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, + }, + ]); } - #[test] fn custom_xyzi_f32() { - const DIM : usize = 3; - const METADIM : usize = 1; + const DIM: usize = 3; + const METADIM: usize = 1; #[derive(Debug, PartialEq, Clone)] struct CustomPoint { x: f32, @@ -87,26 +115,38 @@ fn custom_xyzi_f32() { } } impl PointConvertible for CustomPoint {} - let custom_cloud = vec![ - CustomPoint { x: 0.0, y: 1.0, z: 5.0, i: 0 }, - CustomPoint { x: 1.0, y: 1.5, z: 5.0, i: 1 }, - CustomPoint { x: 1.3, y: 1.6, z: 5.7, i: 2 }, - CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, i: u8::MAX }, - ]; - let copy = custom_cloud.clone(); - let custom_msg: Result = MyConverter::try_from(custom_cloud).unwrap().try_into(); - assert!(custom_msg.is_ok()); - let to_custom_type = MyConverter::try_from(custom_msg.unwrap()); - assert!(to_custom_type.is_ok()); - let to_custom_type = to_custom_type.unwrap(); - let back_to_type = to_custom_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + convert_from_into::(vec![ + CustomPoint { + x: 0.0, + y: 1.0, + z: 5.0, + i: 0, + }, + CustomPoint { + x: 1.0, + y: 1.5, + z: 5.0, + i: 1, + }, + CustomPoint { + x: 1.3, + y: 1.6, + z: 5.7, + i: 2, + }, + CustomPoint { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + i: u8::MAX, + }, + ]); } #[test] fn custom_rgba_f32() { - const DIM : usize = 3; - const METADIM : usize = 4; + const DIM: usize = 3; + const METADIM: usize = 4; #[derive(Debug, PartialEq, Clone)] struct CustomPoint { x: f32, @@ -120,7 +160,15 @@ fn custom_rgba_f32() { type MyConverter = Convert; impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) { - ([self.x, self.y, self.z], [PointMeta::new(self.r), PointMeta::new(self.g), PointMeta::new(self.b), PointMeta::new(self.a)]) + ( + [self.x, self.y, self.z], + [ + PointMeta::new(self.r), + PointMeta::new(self.g), + PointMeta::new(self.b), + PointMeta::new(self.a), + ], + ) } } impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { @@ -143,95 +191,382 @@ fn custom_rgba_f32() { } } impl PointConvertible for CustomPoint {} - let custom_cloud = vec![ - CustomPoint { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 }, - CustomPoint { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, a: 1 }, - CustomPoint { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, a: 2 }, - CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX }, - ]; - let copy = custom_cloud.clone(); - let custom_msg: Result = MyConverter::try_from(custom_cloud).unwrap().try_into(); - assert!(custom_msg.is_ok()); - let to_custom_type = MyConverter::try_from(custom_msg.unwrap()); - assert!(to_custom_type.is_ok()); - let to_custom_type = to_custom_type.unwrap(); - let back_to_type = to_custom_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + convert_from_into::(vec![ + CustomPoint { + x: 0.0, + y: 1.0, + z: 5.0, + r: 0, + g: 0, + b: 0, + a: 0, + }, + CustomPoint { + x: 1.0, + y: 1.5, + z: 5.0, + r: 1, + g: 1, + b: 1, + a: 1, + }, + CustomPoint { + x: 1.3, + y: 1.6, + z: 5.7, + r: 2, + g: 2, + b: 2, + a: 2, + }, + CustomPoint { + x: f32::MAX, + y: f32::MIN, + z: f32::MAX, + r: u8::MAX, + g: u8::MAX, + b: u8::MAX, + a: u8::MAX, + }, + ]); } #[test] fn converterxyz() { let 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 }, + 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, + }, ]; - let copy = cloud.clone(); - let msg: Result = ConvertXYZ::try_from(cloud).unwrap().try_into(); - assert!(msg.is_ok()); - let to_xyz_type = ConvertXYZ::try_from(msg.unwrap()); - assert!(to_xyz_type.is_ok()); - let to_xyz_type: ConvertXYZ = to_xyz_type.unwrap(); - let back_to_type = to_xyz_type.map(|point: PointXYZ| Ok(point)).collect::>(); - assert!(back_to_type.is_ok()); - assert_eq!(copy, back_to_type.unwrap()); + convert_from_into::(cloud); } #[test] fn converterxyzrgba() { - let cloud = 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 }, - ]; - - let copy = cloud.clone(); - let msg: Result = ConvertXYZRGBA::try_from(cloud).unwrap().try_into(); - assert!(msg.is_ok()); - let to_xyzrgba_type = ConvertXYZRGBA::try_from(msg.unwrap()); - assert!(to_xyzrgba_type.is_ok()); - let to_xyzrgba_type = to_xyzrgba_type.unwrap(); - let back_to_type = to_xyzrgba_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + 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, + }, + ]); } #[test] fn converterxyzinormal() { - let cloud = 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 }, - ]; - - let copy = cloud.clone(); - let msg: Result = ConvertXYZINormal::try_from(cloud).unwrap().try_into(); - assert!(msg.is_ok()); - let to_xyzinormal_type = ConvertXYZINormal::try_from(msg.unwrap()); - assert!(to_xyzinormal_type.is_ok()); - let to_xyzinormal_type = to_xyzinormal_type.unwrap(); - let back_to_type = to_xyzinormal_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + 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, + }, + ]); } #[test] fn converterxyzrgbnormal() { - let cloud = 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 }, - ]; - - let copy = cloud.clone(); - let msg: Result = ConvertXYZRGBNormal::try_from(cloud).unwrap().try_into(); - assert!(msg.is_ok()); - let to_xyzrgbnormal_type = ConvertXYZRGBNormal::try_from(msg.unwrap()); - assert!(to_xyzrgbnormal_type.is_ok()); - let to_xyzrgbnormal_type = to_xyzrgbnormal_type.unwrap(); - let back_to_type = to_xyzrgbnormal_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + 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, + }, + ]); +} + +#[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, + }, + ]); +} + +#[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, + }, + ]); +} + +#[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, + }, + ]); +} + +#[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, + }, + ]); +} + +#[test] +fn converterxyzi() { + convert_from_into::(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, + }, + ]); } diff --git a/tests/rosrust_msg_test.rs b/tests/rosrust_msg_test.rs index 40e09c1..d519947 100644 --- a/tests/rosrust_msg_test.rs +++ b/tests/rosrust_msg_test.rs @@ -1,21 +1,33 @@ #[cfg(feature = "rosrust_msg")] #[test] fn convertxyz_rosrust_msg() { - use ros_pointcloud2::{ConvertXYZ}; + use ros_pointcloud2::fallible_iterator::FallibleIterator; use ros_pointcloud2::pcl_utils::PointXYZ; use ros_pointcloud2::ros_types::PointCloud2Msg; - use ros_pointcloud2::fallible_iterator::FallibleIterator; + use ros_pointcloud2::ConvertXYZ; let cloud = vec![ - PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, - PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, - PointXYZ { x: 7.0, y: 8.0, z: 9.0 }, + PointXYZ { + x: 1.0, + y: 2.0, + z: 3.0, + }, + PointXYZ { + x: 4.0, + y: 5.0, + z: 6.0, + }, + PointXYZ { + x: 7.0, + y: 8.0, + z: 9.0, + }, ]; let copy = cloud.clone(); let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().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 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()); -} \ No newline at end of file +}