diff --git a/Cargo.toml b/Cargo.toml
index ac5599c..8b57e3f 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -52,3 +52,6 @@ derive = ["dep:rpcl2_derive", "dep:type-layout"]
nalgebra = ["dep:nalgebra"]
default = ["derive"]
+
+[package.metadata.docs.rs]
+rustdoc-args = ["--generate-link-to-definition"]
diff --git a/README.md b/README.md
index 82813b0..8c4a8d1 100644
--- a/README.md
+++ b/README.md
@@ -1,9 +1,10 @@
## !! Note !!
+
This library is currently in development for v1.0.0, for the documentation of v0.4.0 on crates.io, visit the [docs](https://docs.rs/ros_pointcloud2/0.4.0/ros_pointcloud2/).
ROS PointCloud2
- A complete and versatile implementation of PointCloud2.
+ A PointCloud2 message conversion library.
@@ -83,6 +84,14 @@ Also, indicate the following dependencies to your linker inside the `package.xml
Please open an issue or PR if you need other integrations.
+## Performance
+
+The library offers a speed up when compared to PointCloudLibrary (PCL) conversions but the specific factor depends heavily on the use case and system.
+`vec` conversions are on average ~7.5x faster than PCL while the single core iteration `_iter` APIs are around 2x faster.
+Parallelization with `_par_iter` showcases a 10.5x speed up compared to an OpenMP accelerated PCL pipeline.
+
+The full benchmarks are publicly available in this [repository](https://github.com/stelzo/ros_pcl_conv_bench).
+
## License
[MIT](https://choosealicense.com/licenses/mit/)
diff --git a/rpcl2_derive/Cargo.toml b/rpcl2_derive/Cargo.toml
index 1d39726..07f2b25 100644
--- a/rpcl2_derive/Cargo.toml
+++ b/rpcl2_derive/Cargo.toml
@@ -9,4 +9,4 @@ proc-macro = true
[dependencies]
syn = "1"
quote = "1"
-proc-macro2 = "1"
\ No newline at end of file
+proc-macro2 = "1"
diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs
index ba8e999..894bfd3 100644
--- a/rpcl2_derive/src/lib.rs
+++ b/rpcl2_derive/src/lib.rs
@@ -21,7 +21,7 @@ fn get_allowed_types() -> HashMap<&'static str, usize> {
// Given a field, get the value of the `rpcl2` renaming attribute like
// #[rpcl2(name = "new_name")]
-fn get_ros_fields_attribute(attrs: &Vec) -> Option {
+fn get_ros_fields_attribute(attrs: &[syn::Attribute]) -> Option {
for attr in attrs {
if attr.path.is_ident("rpcl2") {
let meta = attr.parse_meta().unwrap();
@@ -50,12 +50,13 @@ fn struct_field_rename_array(input: &DeriveInput) -> Vec {
_ => panic!("StructNames can only be derived for structs"),
};
- fields.iter().map(|field| {
- let field_name = field.ident.as_ref().unwrap();
- let ros_fields_attr = get_ros_fields_attribute(&field.attrs);
- match ros_fields_attr {
- Some(ros_fields) => {
- match ros_fields {
+ fields
+ .iter()
+ .map(|field| {
+ let field_name = field.ident.as_ref().unwrap();
+ let ros_fields_attr = get_ros_fields_attribute(&field.attrs);
+ match ros_fields_attr {
+ Some(ros_fields) => match ros_fields {
syn::Lit::Str(lit_str) => {
let val = lit_str.value();
if val.is_empty() {
@@ -66,26 +67,26 @@ fn struct_field_rename_array(input: &DeriveInput) -> Vec {
_ => {
panic!("Only string literals are allowed for the rpcl2 attribute")
}
- }
+ },
+ None => String::from(field_name.to_token_stream().to_string()),
}
- None => {
- String::from(field_name.to_token_stream().to_string())
- }
- }
- }).collect()
+ })
+ .collect()
}
-#[proc_macro_derive(RosFields, attributes(rpcl2))]
+/// This macro will implement the `Fields` trait for your struct so you can use your point for the PointCloud2 conversion.
+///
+/// Use the rename attribute if your struct field name should be different to the ROS field name.
+#[proc_macro_derive(Fields, attributes(rpcl2))]
pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
- // Parse the input tokens into a syntax tree
let input = parse_macro_input!(input as DeriveInput);
-
- // Get the name of the struct
let struct_name = &input.ident;
- let field_names = struct_field_rename_array(&input).into_iter().map(|field_name| {
- quote! { #field_name }
- });
+ let field_names = struct_field_rename_array(&input)
+ .into_iter()
+ .map(|field_name| {
+ quote! { #field_name }
+ });
let field_names_len = field_names.len();
@@ -103,70 +104,12 @@ pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
expanded.into()
}
-/// Derive macro for the `Fields` trait.
-///
-/// Given the ordering from the source code of your struct, this macro will generate an array of field names.
-/// If the fields contain attributes for renaming or skipping, the generated array will reflect the final ordering.
-/*#[proc_macro_derive(RosFields)]
-pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
- let input = parse_macro_input!(input as DeriveInput);
- let name = input.clone().ident;
-
- let fields = match input.data {
- syn::Data::Struct(ref data) => data.fields.clone(),
- _ => {
- return syn::Error::new_spanned(input, "Only structs are supported")
- .to_compile_error()
- .into()
- }
- };
-
- let allowed_datatypes = get_allowed_types();
-
- if fields.is_empty() {
- return syn::Error::new_spanned(input, "No fields found")
- .to_compile_error()
- .into();
- }
-
- for field in fields.iter() {
- let ty = field.ty.to_token_stream().to_string();
- if !allowed_datatypes.contains_key(&ty.as_str()) {
- return syn::Error::new_spanned(field, "Field type not allowed")
- .to_compile_error()
- .into();
- }
- }
-
- let field_len_token: usize = fields.len();
-
- let field_names = fields
- .iter()
- .map(|field| {
- let field_name = field.ident.as_ref().unwrap();
- quote! { stringify!(#field_name) }
- })
- .collect::>();
-
- let field_impl = quote! {
- impl Fields<#field_len_token> for #name {
- fn field_names_ordered() -> [&'static str; #field_len_token] {
- [
- #(#field_names,)*
- ]
- }
- }
- };
-
- TokenStream::from(field_impl)
-}*/
-
/// This macro will fully implement the `PointConvertible` trait for your struct so you can use your point for the PointCloud2 conversion.
///
/// Note that the repr(C) attribute is required for the struct to work efficiently with C++ PCL.
/// With Rust layout optimizations, the struct might not work with the PCL library but the message still conforms to the specification of PointCloud2.
/// Furthermore, Rust layout can lead to smaller messages to be send over the network.
-#[proc_macro_derive(RosFull)]
+#[proc_macro_derive(PointConvertible)]
pub fn ros_point_derive(input: TokenStream) -> TokenStream {
let input = parse_macro_input!(input as DeriveInput);
let name = input.clone().ident;
@@ -199,13 +142,11 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
let field_len_token: usize = fields.len();
- let field_names = fields
- .iter()
- .map(|field| {
- let field_name = field.ident.as_ref().unwrap();
- quote! { stringify!(#field_name) }
- })
- .collect::>();
+ let field_names = struct_field_rename_array(&input)
+ .into_iter()
+ .map(|field_name| {
+ quote! { #field_name }
+ });
let field_impl = quote! {
impl ros_pointcloud2::Fields<#field_len_token> for #name {
diff --git a/src/convert.rs b/src/convert.rs
index fce16d7..88c8a10 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -1,3 +1,5 @@
+use std::str::FromStr;
+
use crate::*;
/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html).
@@ -13,9 +15,8 @@ pub enum FieldDatatype {
I8,
I16,
- /// While RGB is not officially supported by ROS, it is used in practice as a packed f32.
- /// To make it easier to work with and avoid packing code, the
- /// [`ros_pointcloud2::points::RGB`] union is supported here and handled like a f32.
+ /// While RGB is not officially supported by ROS, it is used in the tooling as a packed f32.
+ /// To make it easy to work with and avoid packing code, the [`ros_pointcloud2::points::RGB`] union is supported here and handled like a f32.
RGB,
}
@@ -35,11 +36,11 @@ impl FieldDatatype {
}
}
-impl TryFrom for FieldDatatype {
- type Error = MsgConversionError;
+impl FromStr for FieldDatatype {
+ type Err = MsgConversionError;
- fn try_from(value: String) -> Result {
- match value.to_lowercase().as_str() {
+ fn from_str(s: &str) -> Result {
+ match s {
"f32" => Ok(FieldDatatype::F32),
"f64" => Ok(FieldDatatype::F64),
"i32" => Ok(FieldDatatype::I32),
@@ -49,7 +50,7 @@ impl TryFrom for FieldDatatype {
"i8" => Ok(FieldDatatype::I8),
"i16" => Ok(FieldDatatype::I16),
"rgb" => Ok(FieldDatatype::RGB),
- _ => Err(MsgConversionError::UnsupportedFieldType(value)),
+ _ => Err(MsgConversionError::UnsupportedFieldType(s.into())),
}
}
}
@@ -148,6 +149,14 @@ impl From for u8 {
}
}
+impl TryFrom<&ros_types::PointFieldMsg> for FieldDatatype {
+ type Error = MsgConversionError;
+
+ fn try_from(value: &ros_types::PointFieldMsg) -> Result {
+ Self::try_from(value.datatype)
+ }
+}
+
/// Matching field names from each data point.
/// Always make sure to use the same order as in your conversion implementation to have a correct mapping.
///
@@ -372,10 +381,22 @@ impl FromBytes for u8 {
}
}
+pub enum ByteSimilarity {
+ Equal,
+ Overlapping,
+ Different,
+}
+
#[derive(Default, Clone, Debug, PartialEq, Copy)]
-pub enum Endianness {
+pub enum Endian {
Big,
-
#[default]
Little,
}
+
+#[derive(Default, Clone, Debug, PartialEq, Copy)]
+pub enum Denseness {
+ #[default]
+ Dense,
+ Sparse,
+}
diff --git a/src/iterator.rs b/src/iterator.rs
index d5ec970..e8f0bdc 100644
--- a/src/iterator.rs
+++ b/src/iterator.rs
@@ -1,5 +1,5 @@
use crate::{
- convert::{Endianness, FieldDatatype},
+ convert::{Endian, FieldDatatype},
Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, RPCL2Point,
};
@@ -162,7 +162,7 @@ struct ByteBufferView {
point_step_size: usize,
offsets: [usize; N],
meta: Vec<(String, FieldDatatype)>,
- endianness: Endianness,
+ endian: Endian,
}
impl ByteBufferView {
@@ -173,7 +173,7 @@ impl ByteBufferView {
end_point_idx: usize,
offsets: [usize; N],
meta: Vec<(String, FieldDatatype)>,
- endianness: Endianness,
+ endian: Endian,
) -> Self {
Self {
data: std::sync::Arc::<[u8]>::from(data),
@@ -182,7 +182,7 @@ impl ByteBufferView {
point_step_size,
offsets,
meta,
- endianness,
+ endian,
}
}
@@ -205,7 +205,7 @@ impl ByteBufferView {
&self.data,
offset + in_point_offset,
*meta_type,
- self.endianness,
+ self.endian,
);
});
@@ -221,7 +221,7 @@ impl ByteBufferView {
point_step_size: self.point_step_size,
offsets: self.offsets,
meta: self.meta.clone(),
- endianness: self.endianness,
+ endian: self.endian,
}
}
@@ -303,14 +303,8 @@ where
*meta_offset = offset;
});
- let endian = if cloud.is_bigendian {
- Endianness::Big
- } else {
- Endianness::Little
- };
-
let point_step_size = cloud.point_step as usize;
- let cloud_length = cloud.width as usize * cloud.height as usize;
+ let cloud_length = cloud.dimensions.width as usize * cloud.dimensions.height as usize;
if point_step_size * cloud_length != cloud.data.len() {
return Err(MsgConversionError::DataLengthMismatch);
}
@@ -323,7 +317,7 @@ where
return Err(MsgConversionError::DataLengthMismatch);
}
- let cloud_length = cloud.width as usize * cloud.height as usize;
+ let cloud_length = cloud.dimensions.width as usize * cloud.dimensions.height as usize;
let data = ByteBufferView::new(
cloud.data,
@@ -332,7 +326,7 @@ where
cloud_length - 1,
offsets,
meta,
- endian,
+ cloud.endian,
);
Ok(Self {
diff --git a/src/lib.rs b/src/lib.rs
index 588c915..83edad9 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,52 +1,38 @@
-//! A complete implementation of PointCloud2 message type conversions.
+//! A PointCloud2 message conversion library.
//!
-//! The library provides a [`ros_pointcloud2::PointCloud2Msg`] type that implements conversions to and from Vec and iterators per default.
+//! The library provides the [`ros_pointcloud2::PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators.
//!
+//! - [`ros_pointcloud2::PointCloud2Msg::try_from_vec`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_into_vec`]
//! - [`ros_pointcloud2::PointCloud2Msg::try_from_iter`]
//! - [`ros_pointcloud2::PointCloud2Msg::try_into_iter`]
-//! - [`ros_pointcloud2::PointCloud2Msg::try_from_vec`] (needs `derive` feature - default)
-//! - [`ros_pointcloud2::PointCloud2Msg::try_into_vec`] (needs `derive` feature - default)
+//! - [`ros_pointcloud2::PointCloud2Msg::try_into_par_iter`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_from_par_iter`]
//!
-//! The `vec` APIs use the `derive` feature to analyze the point types and shorten the conversion time when possible.
-//! The `iter` APIs are more flexible. You can disable the default features when only using iterators. This also removes the procmacro dependencies.
+//! The best choice depends on your use case and the point cloud size.
+//! A good rule of thumb for minimum latency is to use `_vec` per default.
//!
-//! Parallel iterators are also available with the `rayon` feature.
+//! The `_iter` APIs bring more predictable performance and avoid memory allocation but are slower in general (see Performance section in the repository).
+//! If you do any processing on larger point clouds or heavy processing on any sized cloud, switch to `_par_iter`.
//!
-//! - [`ros_pointcloud2::PointCloud2Msg::try_into_par_iter`] (requires `rayon` feature)
-//! - [`ros_pointcloud2::PointCloud2Msg::try_from_par_iter`] (requires `rayon` + `derive` feature)
-//!
-//! A good rule of thumb for minimum latency is to use `vec` per default.
-//! If you do any processing on larger point clouds or heavy processing on any sized cloud, switch to `par_iter`.
-//!
-//! If you are not sure which feature makes sense for your application and device, you should try out the benchmark in the `benches` directory.
-//!
-//! For ROS interoperability, the message type generated by the respective crate must be converted to
-//! the [`ros_pointcloud2::PointCloud2Msg`] using the `From` trait.
-//! This mostly does ownership transfers and avoids copies of point data.
-//!
-//! There are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust` message types
+//! For ROS interoperability, there are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust` message types
//! available with 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 [`ros_pointcloud2::pcl_utils::PointXYZ`] or
-//! [`ros_pointcloud2::pcl_utils::PointXYZI`] are provided in the
-//! [`ros_pointcloud2::pcl_utils`] module. You can implement any additional custom point type.
+//! Common point types like [`ros_pointcloud2::points::PointXYZ`] or
+//! [`ros_pointcloud2::points::PointXYZI`] are provided. You can easily add any additional custom type.
//! See `examples/custom_enum_field_filter.rs` for an example.
//!
//! # Example PointXYZ
//! ```
//! use ros_pointcloud2::prelude::*;
//!
-//! // PointXYZ is predefined
//! let cloud_points = vec![
//! PointXYZ::new(9.0006, 42.0, -6.2),
//! PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
//! ];
+//! let cloud_copy = cloud_points.clone(); // For equality test later.
//!
-//! // For equality test later
-//! let cloud_copy = cloud_points.clone();
-//!
-//! // Give the Vec or anything that implements `IntoIterator`.
//! let in_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
//!
//! // Convert to your ROS crate message type, we will use r2r here.
@@ -56,15 +42,20 @@
//! // let in_msg: PointCloud2Msg = msg.into();
//!
//! let new_pcl = in_msg.try_into_iter().unwrap()
-//! .map(|point: PointXYZ| { // Define the data you want from the point
-//! // Some logic here
+//! .map(|point: PointXYZ| { // Define the data you want from the point.
+//! // Some logic here.
//!
//! point
//! })
-//! .collect::>(); // iterating points here O(n)
-//!
+//! .collect::>();
//! assert_eq!(new_pcl, cloud_copy);
//! ```
+//!
+//! # Features
+//! In addition to the ROS intregrations, the following features are available:
+//! - 'derive' (default): Helpful derive macros for custom point types. Also, derive enables direct copy with `_vec` endpoints.
+//! - 'rayon': Parallel iterator support for `_par_iter` functions. `try_from_par_iter` additionally needs the 'derive' feature to be enabled.
+//! - 'nalgebra': Predefined points offer `xyz()` getter functions for `nalgebra::Point3` types.
pub mod convert;
pub mod points;
@@ -73,20 +64,29 @@ pub mod ros_types;
pub mod iterator;
+use std::num::TryFromIntError;
+use std::str::FromStr;
+
use crate::convert::{FieldDatatype, FromBytes};
use crate::ros_types::{HeaderMsg, PointFieldMsg};
-use convert::Endianness;
pub use convert::Fields;
+use convert::{ByteSimilarity, Denseness, Endian};
-/// All errors that can occur while converting to or from the PointCloud2 message.
+/// All errors that can occur while converting to or from the message type.
+#[derive(Debug)]
pub enum MsgConversionError {
InvalidFieldFormat,
- NotEnoughFields,
- TooManyDimensions,
UnsupportedFieldType(String),
- NoPoints,
DataLengthMismatch,
FieldsNotFound(Vec),
+ UnsupportedFieldCount,
+ NumberConversion,
+}
+
+impl From for MsgConversionError {
+ fn from(_: TryFromIntError) -> Self {
+ MsgConversionError::NumberConversion
+ }
}
impl std::fmt::Display for MsgConversionError {
@@ -95,15 +95,6 @@ impl std::fmt::Display for MsgConversionError {
MsgConversionError::InvalidFieldFormat => {
write!(f, "The field does not match the expected datatype.")
}
- MsgConversionError::NotEnoughFields => {
- write!(f, "Not enough fields in the PointCloud2 message.")
- }
- MsgConversionError::TooManyDimensions => {
- write!(
- f,
- "The dimensionality of the point inside the message is too high."
- )
- }
MsgConversionError::UnsupportedFieldType(datatype) => {
write!(
f,
@@ -111,70 +102,193 @@ impl std::fmt::Display for MsgConversionError {
datatype
)
}
- MsgConversionError::NoPoints => {
- write!(f, "There are no points in the point cloud.")
- }
MsgConversionError::DataLengthMismatch => {
- write!(f, "The length of the byte buffer in the message does not match the expected length computed from the fields.")
+ write!(f, "The length of the byte buffer in the message does not match the expected length computed from the fields, indicating a corrupted or malformed message.")
}
MsgConversionError::FieldsNotFound(fields) => {
- write!(f, "There are fields missing in the message: {:?}", fields)
+ write!(f, "Some fields are not found in the message: {:?}", fields)
+ }
+ MsgConversionError::UnsupportedFieldCount => {
+ write!(
+ f,
+ "Only field_count 1 is supported for reading and writing."
+ )
+ }
+ MsgConversionError::NumberConversion => {
+ write!(f, "The number is too large to be converted into a PointCloud2 supported datatype.")
}
}
}
}
-impl std::fmt::Debug for MsgConversionError {
- fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
- ::fmt(self, f)
+impl std::error::Error for MsgConversionError {
+ fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
+ None
}
}
-impl std::error::Error for MsgConversionError {}
-
#[cfg(feature = "derive")]
-fn system_endian() -> Endianness {
+fn system_endian() -> Endian {
if cfg!(target_endian = "big") {
- Endianness::Big
+ Endian::Big
} else if cfg!(target_endian = "little") {
- Endianness::Little
+ Endian::Little
} else {
- panic!("Unsupported endianness");
+ panic!("Unsupported Endian");
}
}
+/// A PointCloud2 message type.
+///
+/// This representation is a small abstraction of the ROS message description, since every ROS library generates its own messages.
+/// To assert consistency, the type should be build with the [`ros_pointcloud2::PointCloud2MsgBuilder`].
#[derive(Clone, Debug)]
pub struct PointCloud2Msg {
pub header: HeaderMsg,
- pub height: u32,
- pub width: u32,
+ pub dimensions: CloudDimensions,
pub fields: Vec,
- pub is_bigendian: bool,
+ pub endian: Endian,
pub point_step: u32,
pub row_step: u32,
pub data: Vec,
- pub is_dense: bool,
+ pub dense: Denseness,
}
-impl Default for PointCloud2Msg {
- fn default() -> Self {
- Self {
- header: HeaderMsg::default(),
- height: 1, // everything is in one row (unstructured)
- width: 0,
- fields: Vec::new(),
- is_bigendian: false, // ROS default
- point_step: 0,
- row_step: 0,
- data: Vec::new(),
- is_dense: false, // ROS default
+#[derive(Clone, Debug)]
+pub struct CloudDimensionsBuilder(usize);
+
+impl CloudDimensionsBuilder {
+ pub fn new_with_width(width: usize) -> Self {
+ Self(width)
+ }
+
+ pub fn build(self) -> Result {
+ let width = match u32::try_from(self.0) {
+ Ok(w) => w,
+ Err(_) => return Err(MsgConversionError::NumberConversion),
+ };
+
+ Ok(CloudDimensions {
+ width,
+ height: if self.0 > 0 { 1 } else { 0 },
+ })
+ }
+}
+
+/// Creating a PointCloud2Msg with a builder pattern to avoid invalid states.
+#[derive(Clone, Debug, Default)]
+pub struct PointCloud2MsgBuilder {
+ header: HeaderMsg,
+ width: u32,
+ fields: Vec,
+ is_big_endian: bool,
+ point_step: u32,
+ row_step: u32,
+ data: Vec,
+ is_dense: bool,
+}
+
+impl PointCloud2MsgBuilder {
+ pub fn new() -> Self {
+ Self::default()
+ }
+
+ pub fn header(mut self, header: HeaderMsg) -> Self {
+ self.header = header;
+ self
+ }
+
+ pub fn width(mut self, width: u32) -> Self {
+ self.width = width;
+ self
+ }
+
+ pub fn fields(mut self, fields: Vec) -> Self {
+ self.fields = fields;
+ self
+ }
+
+ pub fn endian(mut self, is_big_endian: bool) -> Self {
+ self.is_big_endian = is_big_endian;
+ self
+ }
+
+ pub fn point_step(mut self, point_step: u32) -> Self {
+ self.point_step = point_step;
+ self
+ }
+
+ pub fn row_step(mut self, row_step: u32) -> Self {
+ self.row_step = row_step;
+ self
+ }
+
+ pub fn data(mut self, data: Vec) -> Self {
+ self.data = data;
+ self
+ }
+
+ pub fn dense(mut self, is_dense: bool) -> Self {
+ self.is_dense = is_dense;
+ self
+ }
+
+ pub fn build(self) -> Result {
+ if self.fields.is_empty() {
+ return Err(MsgConversionError::FieldsNotFound(vec![]));
+ }
+
+ if self.fields.iter().any(|f| f.count != 1) {
+ return Err(MsgConversionError::UnsupportedFieldCount);
+ }
+
+ let fields_size = self
+ .fields
+ .iter()
+ .map(FieldDatatype::try_from)
+ .collect::, _>>()?
+ .iter()
+ .map(|f| f.size() as u32)
+ .sum::<_>();
+
+ if self.point_step < fields_size {
+ return Err(MsgConversionError::InvalidFieldFormat);
}
+
+ if self.data.len() as u32 % self.point_step != 0 {
+ return Err(MsgConversionError::DataLengthMismatch);
+ }
+
+ Ok(PointCloud2Msg {
+ header: self.header,
+ dimensions: CloudDimensionsBuilder::new_with_width(self.width as usize).build()?,
+ fields: self.fields,
+ endian: if self.is_big_endian {
+ Endian::Big
+ } else {
+ Endian::Little
+ },
+ point_step: self.point_step,
+ row_step: self.row_step,
+ data: self.data,
+ dense: if self.is_dense {
+ Denseness::Dense
+ } else {
+ Denseness::Sparse
+ },
+ })
}
}
+#[derive(Clone, Debug, Default)]
+pub struct CloudDimensions {
+ pub width: u32,
+ pub height: u32,
+}
+
impl PointCloud2Msg {
#[cfg(feature = "derive")]
- fn prepare_direct_copy() -> Result
+ fn prepare_direct_copy() -> Result
where
C: PointConvertible,
{
@@ -187,35 +301,39 @@ impl PointCloud2Msg {
let layout = TypeLayoutInfo::try_from(C::type_layout())?;
debug_assert!(field_names.len() == layout.fields.len());
- let mut offset: u32 = 0;
+ let mut offset = 0;
let mut fields: Vec = Vec::with_capacity(layout.fields.len());
for f in layout.fields.into_iter() {
let f_translated = field_names[fields.len()].to_string();
match f {
- PointField::Field { datatype, size } => {
+ PointField::Field {
+ datatype,
+ size,
+ count,
+ } => {
fields.push(PointFieldMsg {
name: f_translated,
offset,
datatype,
..Default::default()
});
- offset += size; // assume field_count 1
+ offset += size * count;
}
PointField::Padding(size) => {
- offset += size; // assume field_count 1
+ offset += size;
}
}
}
- Ok(PointCloud2Msg {
- point_step: offset,
- fields,
- ..Default::default()
- })
+ Ok(PointCloud2MsgBuilder::new()
+ .fields(fields)
+ .point_step(offset))
}
#[cfg(feature = "derive")]
- fn assert_byte_similarity(&self) -> Result
+ fn assert_byte_similarity(
+ &self,
+ ) -> Result
where
C: PointConvertible,
{
@@ -232,39 +350,39 @@ impl PointCloud2Msg {
let mut field_counter = 0;
for (f, msg_f) in layout.fields.iter().zip(self.fields.iter()) {
match f {
- PointField::Field { datatype, size } => {
+ PointField::Field {
+ datatype,
+ size,
+ count,
+ } => {
let f_translated = field_names[field_counter].to_string();
field_counter += 1;
- if msg_f.name != f_translated {
- return Err(MsgConversionError::FieldsNotFound(vec![f_translated]));
- }
-
- if msg_f.datatype != *datatype {
- return Err(MsgConversionError::InvalidFieldFormat);
- }
-
- if msg_f.offset != offset {
- return Err(MsgConversionError::DataLengthMismatch);
- }
-
- if msg_f.count != 1 {
- return Err(MsgConversionError::DataLengthMismatch);
+ if msg_f.name != f_translated
+ || msg_f.offset != offset
+ || msg_f.datatype != *datatype
+ || msg_f.count != 1
+ {
+ return Ok(ByteSimilarity::Different);
}
- offset += size; // assume field_count 1
+ offset += size * count;
}
PointField::Padding(size) => {
- offset += size; // assume field_count 1
+ offset += size;
}
}
}
- Ok(offset == self.point_step)
+ Ok(if offset == self.point_step {
+ ByteSimilarity::Equal
+ } else {
+ ByteSimilarity::Overlapping
+ })
}
#[inline(always)]
- fn prepare() -> Result
+ fn prepare() -> Result
where
C: PointConvertible,
{
@@ -274,8 +392,9 @@ impl PointCloud2Msg {
let field_names = C::field_names_ordered();
debug_assert!(field_names.len() == N);
- let mut meta_offsets_acc = 0;
+ let mut meta_offsets_acc: u32 = 0;
let mut fields = vec![PointFieldMsg::default(); N];
+ let field_count: u32 = 1;
for ((meta_value, field_name), field_val) in point
.fields
.into_iter()
@@ -283,9 +402,7 @@ impl PointCloud2Msg {
.zip(fields.iter_mut())
{
let datatype_code = meta_value.datatype.into();
- FieldDatatype::try_from(datatype_code)?;
-
- let field_count = 1;
+ let _ = FieldDatatype::try_from(datatype_code)?;
*field_val = PointFieldMsg {
name: field_name.into(),
@@ -294,20 +411,16 @@ impl PointCloud2Msg {
count: 1,
};
- meta_offsets_acc += field_count * meta_value.datatype.size() as u32
+ meta_offsets_acc += field_count * meta_value.datatype.size() as u32;
}
- Ok(PointCloud2Msg {
- point_step: meta_offsets_acc,
- fields,
- ..Default::default()
- })
+ Ok(PointCloud2MsgBuilder::new()
+ .fields(fields)
+ .point_step(meta_offsets_acc))
}
/// Create a PointCloud2Msg from any iterable type.
///
- /// The operation is O(n) in time complexity where n is the number of points in the point cloud.
- ///
/// # Example
/// ```
/// use ros_pointcloud2::prelude::*;
@@ -326,6 +439,8 @@ impl PointCloud2Msg {
C: PointConvertible,
{
let mut cloud = Self::prepare::()?;
+ let mut cloud_width = 0;
+ let cloud_point_step = cloud.point_step;
iterable.into_iter().for_each(|pointdata| {
let point: RPCL2Point = pointdata.into();
@@ -337,42 +452,16 @@ impl PointCloud2Msg {
cloud.data.extend_from_slice(truncated_bytes);
});
- cloud.width += 1;
+ cloud_width += 1;
});
- cloud.row_step = cloud.width * cloud.point_step;
+ cloud = cloud.width(cloud_width);
+ cloud = cloud.row_step(cloud_width * cloud_point_step);
- Ok(cloud)
+ cloud.build()
}
- /*#[cfg(all(feature = "rayon", not(feature = "derive")))]
- pub fn try_from_par_iter(
- iterable: impl rayon::iter::ParallelIterator- ,
- ) -> Result
- where
- C: PointConvertible + Send + Sync,
- {
- let mut cloud = Self::prepare::()?;
-
- let iterable = iterable.collect::>();
-
- iterable.into_iter().for_each(|pointdata| {
- let point: RPCL2Point = pointdata.into();
-
- point.fields.iter().for_each(|meta| {
- let truncated_bytes = unsafe {
- std::slice::from_raw_parts(meta.bytes.as_ptr(), meta.datatype.size())
- };
- cloud.data.extend_from_slice(truncated_bytes);
- });
-
- cloud.width += 1;
- });
-
- cloud.row_step = cloud.width * cloud.point_step;
- Ok(cloud)
- }*/
-
+ /// Create a PointCloud2Msg from a parallel iterator. Requires the `rayon` and `derive` feature to be enabled.
#[cfg(all(feature = "rayon", feature = "derive"))]
pub fn try_from_par_iter(
iterable: impl rayon::iter::ParallelIterator
- ,
@@ -383,16 +472,31 @@ impl PointCloud2Msg {
Self::try_from_vec(iterable.collect::>())
}
+ /// Create a PointCloud2Msg from a Vec of points.
+ /// Since the point type is known at compile time, the conversion is done by direct copy.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZ::new(1.0, 2.0, 3.0),
+ /// PointXYZ::new(4.0, 5.0, 6.0),
+ /// ];
+ ///
+ /// let msg_out = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
+ /// ```
#[cfg(feature = "derive")]
pub fn try_from_vec(vec: Vec) -> Result
where
C: PointConvertible,
{
- match (system_endian(), Endianness::default()) {
- (Endianness::Big, Endianness::Big) | (Endianness::Little, Endianness::Little) => {
+ match (system_endian(), Endian::default()) {
+ (Endian::Big, Endian::Big) | (Endian::Little, Endian::Little) => {
let mut cloud = Self::prepare_direct_copy::()?;
+ let point_step = cloud.point_step;
- let bytes_total = vec.len() * cloud.point_step as usize;
+ let bytes_total = vec.len() * point_step as usize;
cloud.data.resize(bytes_total, u8::default());
let raw_data: *mut C = cloud.data.as_ptr() as *mut C;
unsafe {
@@ -403,50 +507,59 @@ impl PointCloud2Msg {
);
}
- cloud.width = vec.len() as u32;
- cloud.row_step = cloud.width * cloud.point_step;
-
- Ok(cloud)
+ Ok(cloud
+ .width(vec.len() as u32)
+ .row_step(vec.len() as u32 * point_step)
+ .build()?)
}
_ => Self::try_from_iter(vec),
}
}
+ /// Convert the PointCloud2Msg to a Vec of points.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZI::new(1.0, 2.0, 3.0, 0.5),
+ /// PointXYZI::new(4.0, 5.0, 6.0, 1.1),
+ /// ];
+ ///
+ /// let msg_out = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
+ /// let cloud_points_out: Vec = msg_out.try_into_vec().unwrap();
+ /// assert_eq!(1.0, cloud_points_out.get(0).unwrap().x);
+ /// ```
#[cfg(feature = "derive")]
pub fn try_into_vec(self) -> Result, MsgConversionError>
where
C: PointConvertible,
{
- let msg_endian = if self.is_bigendian {
- Endianness::Big
- } else {
- Endianness::Little
- };
-
- match (system_endian(), msg_endian) {
- (Endianness::Big, Endianness::Big) | (Endianness::Little, Endianness::Little) => {
- let bytematch = match self.assert_byte_similarity::() {
- Err(_) => return Ok(self.try_into_iter()?.collect()), // Fall back to iteration when type too different.
- Ok(bytematch) => bytematch,
+ match (system_endian(), self.endian) {
+ (Endian::Big, Endian::Big) | (Endian::Little, Endian::Little) => {
+ let bytematch = match self.assert_byte_similarity::()? {
+ ByteSimilarity::Equal => true,
+ ByteSimilarity::Overlapping => false,
+ ByteSimilarity::Different => return Ok(self.try_into_iter()?.collect()),
};
- // Similar enough to copy directly.
- let mut vec = Vec::with_capacity(self.width as usize);
- let raw_data: *const C = self.data.as_ptr() as *const C;
+ let cloud_width = self.dimensions.width as usize;
+ let point_step = self.point_step as usize;
+ let mut vec = Vec::with_capacity(cloud_width);
if bytematch {
- // Target and source are an exact match, so copy directly.
unsafe {
std::ptr::copy_nonoverlapping(
- raw_data as *const u8,
+ self.data.as_ptr(),
vec.as_mut_ptr() as *mut u8,
self.data.len(),
);
}
} else {
- // Target is a subtype of source, so copy point by point from buffer.
unsafe {
- for i in 0..self.width {
- let point = raw_data.add(i as usize).read();
+ for i in 0..cloud_width {
+ let point_ptr = self.data.as_ptr().add(i * point_step) as *const C;
+ let point = point_ptr.read();
vec.push(point);
}
}
@@ -454,10 +567,24 @@ impl PointCloud2Msg {
Ok(vec)
}
- _ => Ok(self.try_into_iter()?.collect()), // Endianess does not match, read point by point since endianness is read at conversion time.
+ _ => Ok(self.try_into_iter()?.collect()), // Endianess does not match, read point by point since Endian is read at conversion time.
}
}
+ /// Convert the PointCloud2Msg to an iterator.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZI::new(1.0, 2.0, 3.0, 0.5),
+ /// PointXYZI::new(4.0, 5.0, 6.0, 1.1),
+ /// ];
+ ///
+ /// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+ /// let cloud_points_out = msg_out.try_into_iter().unwrap().collect::>();
+ /// ```
pub fn try_into_iter(
self,
) -> Result, MsgConversionError>
@@ -467,6 +594,21 @@ impl PointCloud2Msg {
iterator::PointCloudIterator::try_from(self)
}
+ /// Convert the PointCloud2Msg to a parallel iterator. Requires the `rayon` feature to be enabled.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZ::new(1.0, 2.0, 3.0, 0.5),
+ /// PointXYZ::new(4.0, 5.0, 6.0, 1.1),
+ /// ];
+ ///
+ /// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+ /// let cloud_points_out = msg_out.try_into_par_iter::().unwrap().collect::>();
+ /// assert_eq!(2, cloud_points_out.len());
+ /// ```
#[cfg(feature = "rayon")]
pub fn try_into_par_iter(
self,
@@ -509,9 +651,7 @@ impl From<[PointData; N]> for RPCL2Point {
}
}
-/// Trait to enable point conversions on the fly while iterating.
-///
-/// Implement this trait for your custom point you want to read or write in the message.
+/// Trait to enable point conversions on the fly.
///
/// # Example
/// ```
@@ -556,6 +696,61 @@ pub trait PointConvertible:
{
}
+/// Trait to enable point conversions on the fly.
+///
+/// Implement this trait for your custom point you want to read or write in the message.
+/// For a more convenient way to implement this trait, enable the `derive` feature and use the `#[derive(PointConvertible, TypeLayout)]` macro.
+///
+/// # Derive Example
+/// ```
+/// use ros_pointcloud2::prelude::*;
+///
+/// #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)]
+/// pub struct MyPointXYZI {
+/// pub x: f32,
+/// pub y: f32,
+/// pub z: f32,
+/// pub intensity: f32,
+/// }
+/// ```
+///
+/// # Manual Example
+/// ```
+/// use ros_pointcloud2::prelude::*;
+///
+/// #[derive(Clone, Debug, PartialEq, Copy, Default, TypeLayout)]
+/// pub struct MyPointXYZI {
+/// pub x: f32,
+/// pub y: f32,
+/// pub z: f32,
+/// pub intensity: f32,
+/// }
+///
+/// impl From for RPCL2Point<4> {
+/// fn from(point: MyPointXYZI) -> Self {
+/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
+/// }
+/// }
+///
+/// impl From> for MyPointXYZI {
+/// fn from(point: RPCL2Point<4>) -> Self {
+/// Self {
+/// x: point[0].get(),
+/// y: point[1].get(),
+/// z: point[2].get(),
+/// intensity: point[3].get(),
+/// }
+/// }
+/// }
+///
+/// impl Fields<4> for MyPointXYZI {
+/// fn field_names_ordered() -> [&'static str; 4] {
+/// ["x", "y", "z", "intensity"]
+/// }
+/// }
+///
+/// impl PointConvertible<4> for MyPointXYZI {}
+/// ```
#[cfg(feature = "derive")]
pub trait PointConvertible:
type_layout::TypeLayout + From> + Into> + Fields + 'static + Default
@@ -565,7 +760,7 @@ pub trait PointConvertible:
#[cfg(feature = "derive")]
enum PointField {
Padding(u32),
- Field { size: u32, datatype: u8 },
+ Field { size: u32, datatype: u8, count: u32 },
}
#[cfg(feature = "derive")]
@@ -580,14 +775,15 @@ impl TryFrom for PointField {
fn try_from(f: type_layout::Field) -> Result {
match f {
type_layout::Field::Field { name: _, ty, size } => {
- let typename: String = ty.into_owned();
- let datatype = FieldDatatype::try_from(typename)?;
+ let typename: String = ty.into_owned().to_lowercase();
+ let datatype = FieldDatatype::from_str(typename.as_str())?;
Ok(Self::Field {
- size: size as u32,
+ size: size.try_into()?,
datatype: datatype.into(),
+ count: 1,
})
}
- type_layout::Field::Padding { size } => Ok(Self::Padding(size as u32)),
+ type_layout::Field::Padding { size } => Ok(Self::Padding(size.try_into()?)),
}
}
}
@@ -606,9 +802,9 @@ impl TryFrom for TypeLayoutInfo {
}
}
-/// Metadata representation for a point.
+/// Single data representation for a point.
///
-/// This struct is used to store meta data in a fixed size byte buffer along the with the
+/// This struct is used to store data fields in a fixed size byte buffer along the with the
/// datatype that is encoded so that it can be decoded later.
///
/// # Example
@@ -622,7 +818,7 @@ impl TryFrom for TypeLayoutInfo {
#[derive(Debug, Clone, Copy)]
pub struct PointData {
bytes: [u8; std::mem::size_of::()],
- endianness: Endianness,
+ endian: Endian,
datatype: FieldDatatype,
}
@@ -631,7 +827,7 @@ impl Default for PointData {
Self {
bytes: [u8::default(); std::mem::size_of::()],
datatype: FieldDatatype::F32,
- endianness: Endianness::default(),
+ endian: Endian::default(),
}
}
}
@@ -653,12 +849,7 @@ impl PointData {
}
#[inline(always)]
- fn from_buffer(
- data: &[u8],
- offset: usize,
- datatype: FieldDatatype,
- endianness: Endianness,
- ) -> Self {
+ fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self {
debug_assert!(data.len() >= offset + datatype.size());
let bytes = [u8::default(); std::mem::size_of::()];
unsafe {
@@ -670,7 +861,7 @@ impl PointData {
Self {
bytes,
datatype,
- endianness,
+ endian,
}
}
@@ -683,9 +874,9 @@ impl PointData {
/// let my_data: f64 = meta.get();
/// ```
pub fn get(&self) -> T {
- match self.endianness {
- Endianness::Big => T::from_be_bytes(convert::PointDataBuffer::new(self.bytes)),
- Endianness::Little => T::from_le_bytes(convert::PointDataBuffer::new(self.bytes)),
+ match self.endian {
+ Endian::Big => T::from_be_bytes(convert::PointDataBuffer::new(self.bytes)),
+ Endian::Little => T::from_le_bytes(convert::PointDataBuffer::new(self.bytes)),
}
}
}
@@ -741,10 +932,10 @@ impl From for PointData {
#[cfg(test)]
mod tests {
use super::*;
- use rpcl2_derive::RosFields;
+ use rpcl2_derive::Fields;
#[allow(dead_code)]
- #[derive(RosFields)]
+ #[derive(Fields)]
struct TestStruct {
field1: String,
#[rpcl2(name = "renamed_field")]
diff --git a/src/ros_types.rs b/src/ros_types.rs
index a17dd53..5bf7d9f 100644
--- a/src/ros_types.rs
+++ b/src/ros_types.rs
@@ -46,8 +46,10 @@ impl From for crate::PointCloud2Msg {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ dimensions: crate::CloudDimensions {
+ width: msg.width,
+ height: msg.height,
+ },
fields: msg
.fields
.into_iter()
@@ -58,11 +60,19 @@ impl From for crate::PointCloud2Msg {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ endian: if msg.is_bigendian {
+ crate::Endian::Big
+ } else {
+ crate::Endian::Little
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ dense: if msg.is_dense {
+ crate::convert::Denseness::Dense
+ } else {
+ crate::convert::Denseness::Sparse
+ },
}
}
}
@@ -78,8 +88,8 @@ impl From for r2r::sensor_msgs::msg::PointCloud2 {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ height: msg.dimensions.height,
+ width: msg.dimensions.width,
fields: msg
.fields
.into_iter()
@@ -90,11 +100,19 @@ impl From for r2r::sensor_msgs::msg::PointCloud2 {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ is_bigendian: if msg.endian == crate::Endian::Big {
+ true
+ } else {
+ false
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ is_dense: if msg.dense == crate::convert::Denseness::Dense {
+ true
+ } else {
+ false
+ },
}
}
}
@@ -111,8 +129,10 @@ impl From for crate::PointCloud2Msg {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ dimensions: crate::CloudDimensions {
+ width: msg.width,
+ height: msg.height,
+ },
fields: msg
.fields
.into_iter()
@@ -123,11 +143,19 @@ impl From for crate::PointCloud2Msg {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ is_bigendian: if msg.endian == crate::Endian::Big {
+ true
+ } else {
+ false
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ is_dense: if msg.dense == crate::convert::Denseness::Dense {
+ true
+ } else {
+ false
+ },
}
}
}
@@ -144,8 +172,8 @@ impl From for rosrust_msg::sensor_msgs::PointCloud2 {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ height: msg.dimensions.height,
+ width: msg.dimensions.width,
fields: msg
.fields
.into_iter()
@@ -156,11 +184,19 @@ impl From for rosrust_msg::sensor_msgs::PointCloud2 {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ is_bigendian: if msg.endian == crate::Endian::Big {
+ true
+ } else {
+ false
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ is_dense: if msg.dense == crate::convert::Denseness::Dense {
+ true
+ } else {
+ false
+ },
}
}
}
diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs
index 03f3299..4edd9ac 100644
--- a/tests/e2e_test.rs
+++ b/tests/e2e_test.rs
@@ -78,7 +78,7 @@ fn write_cloud_from_vec() {
#[test]
#[cfg(feature = "derive")]
fn custom_xyz_f32() {
- #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
+ #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
#[repr(C)]
struct CustomPoint {
x: f32,
@@ -138,7 +138,7 @@ fn custom_xyzi_f32() {
},
];
- #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
+ #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
#[repr(C)]
struct CustomPointXYZI {
x: f32,
@@ -153,7 +153,7 @@ fn custom_xyzi_f32() {
#[test]
#[cfg(feature = "derive")]
fn custom_rgba_f32() {
- #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
+ #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
#[repr(C)]
struct CustomPoint {
x: f32,