diff --git a/Cargo.toml b/Cargo.toml index cfa998a..7818647 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "ros_pointcloud2" -version = "0.4.0" +version = "1.0.0-rc.1" edition = "2021" authors = ["Christopher Sieh "] description = "Customizable conversions for working with sensor_msgs/PointCloud2." @@ -54,4 +54,5 @@ nalgebra = ["dep:nalgebra"] default = ["derive"] [package.metadata.docs.rs] -rustdoc-args = ["--generate-link-to-definition"] +features = ["derive", "nalgebra", "rayon"] +default-target = "x86_64-unknown-linux-gnu" diff --git a/README.md b/README.md index 572877e..7c7c762 100644 --- a/README.md +++ b/README.md @@ -20,20 +20,21 @@ use ros_pointcloud2::prelude::*; // PointXYZ (and many others) are provided by the crate. let cloud_points = vec![ - PointXYZ::new(91.486, -4.1, 42.0001), - PointXYZ::new(f32::MAX, f32::MIN, f32::MAX), + PointXYZI::new(91.486, -4.1, 42.0001, 0.1), + PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MIN), ]; -// Give the Vec or anything that implements `IntoIterator`. -let in_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap(); +let out_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap(); // Convert the ROS crate message type, we will use r2r here. -// let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into(); +// let msg: r2r::sensor_msgs::msg::PointCloud2 = out_msg.into(); // Publish ... + // ... now incoming from a topic. // let in_msg: PointCloud2Msg = msg.into(); +let in_msg = out_msg; -let new_pcl = in_msg.try_into_iter().unwrap() +let processed_cloud = in_msg.try_into_iter().unwrap() .map(|point: PointXYZ| { // Define the info you want to have from the Msg. // Some logic here ... @@ -60,9 +61,9 @@ You can use `rosrust` and `r2r` by enabling the respective feature: ```toml [dependencies] -ros_pointcloud2 = { version = "*", features = ["r2r_msg"]} +ros_pointcloud2 = { version = "*", features = ["r2r_msg", "derive"]} # or -ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]} +ros_pointcloud2 = { version = "*", features = ["rosrust_msg", "derive"]} ``` ### rclrs (ros2_rust) diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs index 894bfd3..e548927 100644 --- a/rpcl2_derive/src/lib.rs +++ b/rpcl2_derive/src/lib.rs @@ -107,9 +107,9 @@ pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream { /// 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. +/// With Rust layout optimizations, the struct might not work with the PCL library but the message still conforms to the description of PointCloud2. /// Furthermore, Rust layout can lead to smaller messages to be send over the network. -#[proc_macro_derive(PointConvertible)] +#[proc_macro_derive(PointConvertible, attributes(rpcl2))] pub fn ros_point_derive(input: TokenStream) -> TokenStream { let input = parse_macro_input!(input as DeriveInput); let name = input.clone().ident; diff --git a/src/convert.rs b/src/convert.rs deleted file mode 100644 index 88c8a10..0000000 --- a/src/convert.rs +++ /dev/null @@ -1,402 +0,0 @@ -use std::str::FromStr; - -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, - - /// 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, -} - -impl FieldDatatype { - pub fn size(&self) -> usize { - match self { - FieldDatatype::U8 => std::mem::size_of::(), - FieldDatatype::U16 => std::mem::size_of::(), - FieldDatatype::U32 => std::mem::size_of::(), - FieldDatatype::I8 => std::mem::size_of::(), - FieldDatatype::I16 => std::mem::size_of::(), - FieldDatatype::I32 => std::mem::size_of::(), - FieldDatatype::F32 => std::mem::size_of::(), - FieldDatatype::F64 => std::mem::size_of::(), - FieldDatatype::RGB => std::mem::size_of::(), // packed in f32 - } - } -} - -impl FromStr for FieldDatatype { - type Err = MsgConversionError; - - fn from_str(s: &str) -> Result { - match s { - "f32" => Ok(FieldDatatype::F32), - "f64" => Ok(FieldDatatype::F64), - "i32" => Ok(FieldDatatype::I32), - "u8" => Ok(FieldDatatype::U8), - "u16" => Ok(FieldDatatype::U16), - "u32" => Ok(FieldDatatype::U32), - "i8" => Ok(FieldDatatype::I8), - "i16" => Ok(FieldDatatype::I16), - "rgb" => Ok(FieldDatatype::RGB), - _ => Err(MsgConversionError::UnsupportedFieldType(s.into())), - } - } -} - -/// 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 - } -} - -/// Convenience implementation for the RGB union. -impl GetFieldDatatype for crate::points::RGB { - fn field_datatype() -> FieldDatatype { - FieldDatatype::RGB - } -} - -impl TryFrom for FieldDatatype { - type Error = MsgConversionError; - - 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(MsgConversionError::UnsupportedFieldType(value.to_string())), - } - } -} - -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, - FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer - } - } -} - -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. -/// -/// This trait is needed to implement the `PointConvertible` trait. -/// -/// # Example -/// ``` -/// use ros_pointcloud2::prelude::*; -/// -/// #[derive(Clone, Debug, PartialEq, Copy)] -/// pub struct MyPointXYZI { -/// pub x: f32, -/// pub y: f32, -/// pub z: f32, -/// pub intensity: f32, -/// } -/// -/// impl Fields<4> for MyPointXYZI { -/// fn field_names_ordered() -> [&'static str; 4] { -/// ["x", "y", "z", "intensity"] -/// } -/// } -/// ``` -pub trait Fields { - fn field_names_ordered() -> [&'static str; N]; -} - -pub struct PointDataBuffer([u8; 8]); - -impl std::ops::Index for PointDataBuffer { - type Output = u8; - - fn index(&self, index: usize) -> &Self::Output { - &self.0[index] - } -} - -impl PointDataBuffer { - pub fn new(data: [u8; 8]) -> Self { - Self(data) - } - - pub fn as_slice(&self) -> &[u8] { - &self.0 - } - - pub fn raw(self) -> [u8; 8] { - self.0 - } - - pub fn from_slice(data: &[u8]) -> Self { - let mut buffer = [0; 8]; - data.iter().enumerate().for_each(|(i, &v)| buffer[i] = v); - Self(buffer) - } -} - -impl From<&[u8]> for PointDataBuffer { - fn from(data: &[u8]) -> Self { - Self::from_slice(data) - } -} - -impl From<[u8; N]> for PointDataBuffer { - fn from(data: [u8; N]) -> Self { - Self::from(data.as_slice()) - } -} - -impl From for PointDataBuffer { - fn from(x: i8) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: i16) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: u16) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: i32) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: u32) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: f32) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: f64) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: u8) -> Self { - x.to_le_bytes().into() - } -} - -impl From for PointDataBuffer { - fn from(x: points::RGB) -> Self { - x.raw().to_le_bytes().into() - } -} - -/// 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 + Into { - fn from_be_bytes(bytes: PointDataBuffer) -> Self; - fn from_le_bytes(bytes: PointDataBuffer) -> Self; -} - -impl FromBytes for i8 { - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::from_be_bytes([bytes[0]]) - } - - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::from_le_bytes([bytes[0]]) - } -} - -impl FromBytes for i16 { - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::from_be_bytes([bytes[0], bytes[1]]) - } - - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::from_le_bytes([bytes[0], bytes[1]]) - } -} - -impl FromBytes for u16 { - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::from_be_bytes([bytes[0], bytes[1]]) - } - - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::from_le_bytes([bytes[0], bytes[1]]) - } -} - -impl FromBytes for u32 { - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } -} - -impl FromBytes for f32 { - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } -} - -impl FromBytes for points::RGB { - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::new_from_packed_f32(f32::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])) - } - - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::new_from_packed_f32(f32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])) - } -} - -impl FromBytes for i32 { - #[inline] - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } -} - -impl FromBytes for f64 { - fn from_be_bytes(bytes: PointDataBuffer) -> 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: PointDataBuffer) -> Self { - Self::from_le_bytes([ - bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], - ]) - } -} - -impl FromBytes for u8 { - fn from_be_bytes(bytes: PointDataBuffer) -> Self { - Self::from_be_bytes([bytes[0]]) - } - - fn from_le_bytes(bytes: PointDataBuffer) -> Self { - Self::from_le_bytes([bytes[0]]) - } -} - -pub enum ByteSimilarity { - Equal, - Overlapping, - Different, -} - -#[derive(Default, Clone, Debug, PartialEq, Copy)] -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 e8f0bdc..65f7c9c 100644 --- a/src/iterator.rs +++ b/src/iterator.rs @@ -1,6 +1,7 @@ +//! Iterator implementations for PointCloud2Msg including a parallel iterator for rayon. use crate::{ - convert::{Endian, FieldDatatype}, - Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, RPCL2Point, + Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, + RPCL2Point, }; /// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg. diff --git a/src/lib.rs b/src/lib.rs index 99142a2..303bc5f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,76 +1,144 @@ //! A PointCloud2 message conversion library. //! -//! The library provides the [`ros_pointcloud2::PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators. +//! The library provides the [`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_into_par_iter`] -//! - [`ros_pointcloud2::PointCloud2Msg::try_from_par_iter`] +//! Vector conversions are optimized for direct copy. They are useful when you just move similar data around. They are usually a good default. +//! - [`PointCloud2Msg::try_from_vec`] requires `derive` feature (enabled by default) +//! - [`PointCloud2Msg::try_into_vec`] requires `derive` feature (enabled by default) //! -//! 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. +//! You can use the iterator functions for more control over the conversion process. +//! - [`PointCloud2Msg::try_from_iter`] +//! - [`PointCloud2Msg::try_into_iter`] //! -//! 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`. +//! These feature predictable but [slightly worse](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#performance) performance while avoiding memory allocations. //! -//! 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. +//! Use the parallel iterator for processing-heavy tasks. +//! - [`PointCloud2Msg::try_into_par_iter`] requires `rayon` feature +//! - [`PointCloud2Msg::try_from_par_iter`] requires `rayon` + `derive` feature //! -//! 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. +//! For ROS interoperability, there are integrations avialable with feature flags. If you miss a message type, please open an issue or a PR. +//! See the [`ros`] module for more information on how to integrate more libraries. //! -//! # Example PointXYZ +//! Common point types like [`points::PointXYZ`] or [`points::PointXYZI`] are provided. You can easily add any additional custom type. +//! See [custom_enum_field_filter.rs](https://github.com/stelzo/ros_pointcloud2/blob/main/examples/custom_enum_field_filter.rs) for an example. +//! +//! # Minimal Example //! ``` //! use ros_pointcloud2::prelude::*; //! //! let cloud_points = vec![ -//! PointXYZ::new(9.0006, 42.0, -6.2), -//! PointXYZ::new(f32::MAX, f32::MIN, f32::MAX), +//! PointXYZI::new(9.6, 42.0, -6.2, 0.1), +//! PointXYZI::new(46.0, 5.47, 0.5, 0.1), //! ]; //! let cloud_copy = cloud_points.clone(); // For equality test later. -//! -//! let in_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap(); -//! -//! // Convert to your ROS crate message type, we will use r2r here. +//! +//! let out_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap(); +//! +//! // Convert to your ROS crate message type. //! // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into(); //! // Publish ... +//! //! // ... now incoming from a topic. //! // let in_msg: PointCloud2Msg = msg.into(); -//! -//! let new_pcl = in_msg.try_into_iter().unwrap() +//! let in_msg = out_msg; +//! +//! let processed_cloud = in_msg.try_into_iter().unwrap() //! .map(|point: PointXYZ| { // Define the data you want from the point. //! // Some logic here. +//! PointXYZI::new(point.x, point.y, point.z, 0.1) +//! }).collect::>(); //! -//! point -//! }) -//! .collect::>(); -//! assert_eq!(new_pcl, cloud_copy); +//! assert_eq!(processed_cloud, 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. +//! - **r2r_msg** — Integration for the ROS2 library [r2r](https://github.com/sequenceplanner/r2r). +//! - **(rclrs_msg)** — Integration for ROS2 [rclrs](https://github.com/ros2-rust/ros2_rust) but it needs this [workaround](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#rclrs-ros2_rust) instead of a feature flag. +//! - **rosrust_msg** — Integration with the [rosrust](https://github.com/adnanademovic/rosrust) library for ROS1 message types. +//! - **derive** (default) — Needed for the `_vec` functions and helpful custom point derive macros for the [`PointConvertible`] trait. +//! - **rayon** — Parallel iterator support for `_par_iter` functions. [`PointCloud2Msg::try_from_par_iter`] additionally needs the 'derive' feature to be enabled. +//! - **nalgebra** — When enabled, predefined points offer a `xyz()` getter returning `nalgebra::Point3`. +//! +//! # Custom Points +//! Implement [`PointConvertible`] for your point with the `derive` feature or manually. +//! +//! ``` +//! use ros_pointcloud2::prelude::*; +//! +//! #[cfg_attr(feature = "derive", derive(PointConvertible, TypeLayout))] +//! #[derive(Clone, Debug, PartialEq, Copy, Default)] +//! pub struct MyPointXYZI { +//! pub x: f32, +//! pub y: f32, +//! pub z: f32, +//! #[cfg_attr(feature = "derive", rpcl2(name = "i"))] +//! pub intensity: f32, +//! } +//! +//! impl MyPointXYZI { +//! pub fn new(x: f32, y: f32, z: f32, intensity: f32) -> Self { +//! Self { x, y, z, intensity } +//! } +//! } +//! +//! // Manual implementation of PointConvertible without derive. +//! #[cfg(not(feature = "derive"))] +//! impl Fields<4> for MyPointXYZI { +//! fn field_names_ordered() -> [&'static str; 4] { +//! ["x", "y", "z", "i"] // Note the different field name for intensity. +//! } +//! } +//! +//! #[cfg(not(feature = "derive"))] +//! impl From> for MyPointXYZI { +//! fn from(point: RPCL2Point<4>) -> Self { +//! Self::new(point[0].get(), point[1].get(), point[2].get(), point[3].get()) +//! } +//! } +//! +//! #[cfg(not(feature = "derive"))] +//! impl From for RPCL2Point<4> { +//! fn from(point: MyPointXYZI) -> Self { +//! [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into() +//! } +//! } +//! +//! #[cfg(not(feature = "derive"))] +//! impl PointConvertible<4> for MyPointXYZI {} +//! +//! let first_p = MyPointXYZI::new(1.0, 2.0, 3.0, 0.5); +//! let cloud_points = vec![first_p, MyPointXYZI::new(4.0, 5.0, 6.0, 0.5)]; +//! let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap(); +//! let cloud_points_out: Vec = msg_out.try_into_iter().unwrap().collect(); +//! assert_eq!(first_p, *cloud_points_out.first().unwrap()); +//! ``` +//! +//! An example without `#[cfg_attr]` looks like this: +//! ```ignore +//! #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)] +//! pub struct MyPointXYZI { +//! pub x: f32, +//! pub y: f32, +//! pub z: f32, +//! #[rpcl2(name = "i")] +//! pub intensity: f32, +//! } +//! ``` + +#![crate_type = "lib"] +#![warn(clippy::print_stderr)] +#![warn(clippy::print_stdout)] -pub mod convert; pub mod points; pub mod prelude; -pub mod ros_types; +pub mod ros; pub mod iterator; use std::num::TryFromIntError; use std::str::FromStr; -use crate::convert::{FieldDatatype, FromBytes}; -use crate::ros_types::{HeaderMsg, PointFieldMsg}; -pub use convert::Fields; -use convert::{ByteSimilarity, Denseness, Endian}; +use crate::ros::{HeaderMsg, PointFieldMsg}; /// All errors that can occur while converting to or from the message type. #[derive(Debug)] @@ -138,10 +206,10 @@ fn system_endian() -> Endian { } } -/// A PointCloud2 message type. +/// The intermediate point cloud type for ROS integrations. /// -/// 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`]. +/// To assert consistency, the type should be build with the [`PointCloud2MsgBuilder`]. +/// See the offical [ROS message description](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud2.html) for more information on the fields. #[derive(Clone, Debug)] pub struct PointCloud2Msg { pub header: HeaderMsg, @@ -154,6 +222,30 @@ pub struct PointCloud2Msg { pub dense: Denseness, } +/// Endianess encoding hint for the message. +#[derive(Default, Clone, Debug, PartialEq, Copy)] +pub enum Endian { + Big, + #[default] + Little, +} + +/// Density flag for the message. Writing sparse point clouds is not supported. +#[derive(Default, Clone, Debug, PartialEq, Copy)] +pub enum Denseness { + #[default] + Dense, + Sparse, +} + +#[cfg(feature = "derive")] +enum ByteSimilarity { + Equal, + Overlapping, + Different, +} + +/// Creating a CloudDimensions type with the builder pattern to avoid invalid states when using 1-row point clouds. #[derive(Clone, Debug)] pub struct CloudDimensionsBuilder(usize); @@ -175,7 +267,7 @@ impl CloudDimensionsBuilder { } } -/// Creating a PointCloud2Msg with a builder pattern to avoid invalid states. +/// Creating a PointCloud2Msg with the builder pattern to avoid invalid states. #[derive(Clone, Debug, Default)] pub struct PointCloud2MsgBuilder { header: HeaderMsg, @@ -280,6 +372,7 @@ impl PointCloud2MsgBuilder { } } +/// Dimensions of the point cloud as width and height. #[derive(Clone, Debug, Default)] pub struct CloudDimensions { pub width: u32, @@ -288,52 +381,8 @@ pub struct CloudDimensions { impl PointCloud2Msg { #[cfg(feature = "derive")] - fn prepare_direct_copy() -> Result - where - C: PointConvertible, - { - let point: RPCL2Point = C::default().into(); - debug_assert!(point.fields.len() == N); - - let field_names = C::field_names_ordered(); - debug_assert!(field_names.len() == N); - - let layout = TypeLayoutInfo::try_from(C::type_layout())?; - debug_assert!(field_names.len() == layout.fields.len()); - - 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, - count, - } => { - fields.push(PointFieldMsg { - name: f_translated, - offset, - datatype, - ..Default::default() - }); - offset += size * count; - } - PointField::Padding(size) => { - offset += size; - } - } - } - - Ok(PointCloud2MsgBuilder::new() - .fields(fields) - .point_step(offset)) - } - - #[cfg(feature = "derive")] - fn assert_byte_similarity( - &self, - ) -> Result + #[inline(always)] + fn byte_similarity(&self) -> Result where C: PointConvertible, { @@ -381,44 +430,6 @@ impl PointCloud2Msg { }) } - #[inline(always)] - fn prepare() -> Result - where - C: PointConvertible, - { - let point: RPCL2Point = C::default().into(); - debug_assert!(point.fields.len() == N); - - let field_names = C::field_names_ordered(); - debug_assert!(field_names.len() == N); - - 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() - .zip(field_names.into_iter()) - .zip(fields.iter_mut()) - { - let datatype_code = meta_value.datatype.into(); - let _ = FieldDatatype::try_from(datatype_code)?; - - *field_val = PointFieldMsg { - name: field_name.into(), - offset: meta_offsets_acc, - datatype: datatype_code, - count: 1, - }; - - meta_offsets_acc += field_count * meta_value.datatype.size() as u32; - } - - Ok(PointCloud2MsgBuilder::new() - .fields(fields) - .point_step(meta_offsets_acc)) - } - /// Create a PointCloud2Msg from any iterable type. /// /// # Example @@ -438,9 +449,43 @@ impl PointCloud2Msg { where C: PointConvertible, { - let mut cloud = Self::prepare::()?; + let (mut cloud, point_step) = { + let point: RPCL2Point = C::default().into(); + debug_assert!(point.fields.len() == N); + + let field_names = C::field_names_ordered(); + debug_assert!(field_names.len() == N); + + 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() + .zip(field_names.into_iter()) + .zip(fields.iter_mut()) + { + let datatype_code = meta_value.datatype.into(); + let _ = FieldDatatype::try_from(datatype_code)?; + + *field_val = PointFieldMsg { + name: field_name.into(), + offset: meta_offsets_acc, + datatype: datatype_code, + count: 1, + }; + + meta_offsets_acc += field_count * meta_value.datatype.size() as u32; + } + + ( + PointCloud2MsgBuilder::new() + .fields(fields) + .point_step(meta_offsets_acc), + meta_offsets_acc, + ) + }; let mut cloud_width = 0; - let cloud_point_step = cloud.point_step; iterable.into_iter().for_each(|pointdata| { let point: RPCL2Point = pointdata.into(); @@ -456,7 +501,7 @@ impl PointCloud2Msg { }); cloud = cloud.width(cloud_width); - cloud = cloud.row_step(cloud_width * cloud_point_step); + cloud = cloud.row_step(cloud_width * point_step); cloud.build() } @@ -493,8 +538,47 @@ impl PointCloud2Msg { { 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 (mut cloud, point_step) = { + let point: RPCL2Point = C::default().into(); + debug_assert!(point.fields.len() == N); + + let field_names = C::field_names_ordered(); + debug_assert!(field_names.len() == N); + + let layout = TypeLayoutInfo::try_from(C::type_layout())?; + debug_assert!(field_names.len() == layout.fields.len()); + + 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, + count, + } => { + fields.push(PointFieldMsg { + name: f_translated, + offset, + datatype, + ..Default::default() + }); + offset += size * count; + } + PointField::Padding(size) => { + offset += size; + } + } + } + + ( + PointCloud2MsgBuilder::new() + .fields(fields) + .point_step(offset), + offset, + ) + }; let bytes_total = vec.len() * point_step as usize; cloud.data.resize(bytes_total, u8::default()); @@ -538,7 +622,7 @@ impl PointCloud2Msg { { match (system_endian(), self.endian) { (Endian::Big, Endian::Big) | (Endian::Little, Endian::Little) => { - let bytematch = match self.assert_byte_similarity::()? { + let bytematch = match self.byte_similarity::()? { ByteSimilarity::Equal => true, ByteSimilarity::Overlapping => false, ByteSimilarity::Different => return Ok(self.try_into_iter()?.collect()), @@ -554,6 +638,7 @@ impl PointCloud2Msg { vec.as_mut_ptr() as *mut u8, self.data.len(), ); + vec.set_len(cloud_width); } } else { unsafe { @@ -621,10 +706,11 @@ impl PointCloud2Msg { } /// Internal point representation. It is used to store the coordinates and meta data of a point. +/// /// In each iteration, an internal point representation is converted to the desired point type. /// Implement the `From` traits for your point type to use the conversion. /// -/// See the [`ros_pointcloud2::PointConvertible`] trait for more information. +/// See the [`PointConvertible`] trait for more information. pub struct RPCL2Point { fields: [PointData; N], } @@ -757,6 +843,33 @@ pub trait PointConvertible: { } +/// Matching field names from each data point. +/// Always make sure to use the same order as in your conversion implementation to have a correct mapping. +/// +/// This trait is needed to implement the `PointConvertible` trait. +/// +/// # Example +/// ``` +/// use ros_pointcloud2::prelude::*; +/// +/// #[derive(Clone, Debug, PartialEq, Copy)] +/// pub struct MyPointXYZI { +/// pub x: f32, +/// pub y: f32, +/// pub z: f32, +/// pub intensity: f32, +/// } +/// +/// impl Fields<4> for MyPointXYZI { +/// fn field_names_ordered() -> [&'static str; 4] { +/// ["x", "y", "z", "intensity"] +/// } +/// } +/// ``` +pub trait Fields { + fn field_names_ordered() -> [&'static str; N]; +} + #[cfg(feature = "derive")] enum PointField { Padding(u32), @@ -875,8 +988,8 @@ impl PointData { /// ``` pub fn get(&self) -> T { 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)), + Endian::Big => T::from_be_bytes(PointDataBuffer::new(self.bytes)), + Endian::Little => T::from_le_bytes(PointDataBuffer::new(self.bytes)), } } } @@ -929,9 +1042,365 @@ impl From for PointData { } } +/// Datatypes from the [`ros::PointFieldMsg`]. +#[derive(Default, Clone, Debug, PartialEq, Copy)] +pub enum FieldDatatype { + F32, + F64, + I32, + U8, + U16, + #[default] + U32, + I8, + I16, + + /// 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 [`points::RGB`] union is supported here and handled like a f32. + RGB, +} + +impl FieldDatatype { + pub fn size(&self) -> usize { + match self { + FieldDatatype::U8 => std::mem::size_of::(), + FieldDatatype::U16 => std::mem::size_of::(), + FieldDatatype::U32 => std::mem::size_of::(), + FieldDatatype::I8 => std::mem::size_of::(), + FieldDatatype::I16 => std::mem::size_of::(), + FieldDatatype::I32 => std::mem::size_of::(), + FieldDatatype::F32 => std::mem::size_of::(), + FieldDatatype::F64 => std::mem::size_of::(), + FieldDatatype::RGB => std::mem::size_of::(), // packed in f32 + } + } +} + +impl FromStr for FieldDatatype { + type Err = MsgConversionError; + + fn from_str(s: &str) -> Result { + match s { + "f32" => Ok(FieldDatatype::F32), + "f64" => Ok(FieldDatatype::F64), + "i32" => Ok(FieldDatatype::I32), + "u8" => Ok(FieldDatatype::U8), + "u16" => Ok(FieldDatatype::U16), + "u32" => Ok(FieldDatatype::U32), + "i8" => Ok(FieldDatatype::I8), + "i16" => Ok(FieldDatatype::I16), + "rgb" => Ok(FieldDatatype::RGB), + _ => Err(MsgConversionError::UnsupportedFieldType(s.into())), + } + } +} + +/// 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 + } +} + +/// Convenience implementation for the RGB union. +impl GetFieldDatatype for crate::points::RGB { + fn field_datatype() -> FieldDatatype { + FieldDatatype::RGB + } +} + +impl TryFrom for FieldDatatype { + type Error = MsgConversionError; + + 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(MsgConversionError::UnsupportedFieldType(value.to_string())), + } + } +} + +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, + FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer + } + } +} + +impl TryFrom<&ros::PointFieldMsg> for FieldDatatype { + type Error = MsgConversionError; + + fn try_from(value: &ros::PointFieldMsg) -> Result { + Self::try_from(value.datatype) + } +} + +/// Byte buffer alias for endian-aware point data reading and writing. +/// +/// It uses a fixed size buffer of 8 bytes since the largest supported datatype for PointFieldMsg is f64. +pub struct PointDataBuffer([u8; 8]); + +impl std::ops::Index for PointDataBuffer { + type Output = u8; + + fn index(&self, index: usize) -> &Self::Output { + &self.0[index] + } +} + +impl PointDataBuffer { + pub fn new(data: [u8; 8]) -> Self { + Self(data) + } + + pub fn as_slice(&self) -> &[u8] { + &self.0 + } + + pub fn raw(self) -> [u8; 8] { + self.0 + } + + pub fn from_slice(data: &[u8]) -> Self { + let mut buffer = [0; 8]; + data.iter().enumerate().for_each(|(i, &v)| buffer[i] = v); + Self(buffer) + } +} + +impl From<&[u8]> for PointDataBuffer { + fn from(data: &[u8]) -> Self { + Self::from_slice(data) + } +} + +impl From<[u8; N]> for PointDataBuffer { + fn from(data: [u8; N]) -> Self { + Self::from(data.as_slice()) + } +} + +impl From for PointDataBuffer { + fn from(x: i8) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: i16) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: u16) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: i32) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: u32) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: f32) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: f64) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: u8) -> Self { + x.to_le_bytes().into() + } +} + +impl From for PointDataBuffer { + fn from(x: points::RGB) -> Self { + x.raw().to_le_bytes().into() + } +} + +/// 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 + Into { + fn from_be_bytes(bytes: PointDataBuffer) -> Self; + fn from_le_bytes(bytes: PointDataBuffer) -> Self; +} + +impl FromBytes for i8 { + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::from_be_bytes([bytes[0]]) + } + + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::from_le_bytes([bytes[0]]) + } +} + +impl FromBytes for i16 { + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::from_be_bytes([bytes[0], bytes[1]]) + } + + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::from_le_bytes([bytes[0], bytes[1]]) + } +} + +impl FromBytes for u16 { + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::from_be_bytes([bytes[0], bytes[1]]) + } + + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::from_le_bytes([bytes[0], bytes[1]]) + } +} + +impl FromBytes for u32 { + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } +} + +impl FromBytes for f32 { + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } +} + +impl FromBytes for points::RGB { + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::new_from_packed_f32(f32::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])) + } + + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::new_from_packed_f32(f32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])) + } +} + +impl FromBytes for i32 { + #[inline] + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } +} + +impl FromBytes for f64 { + fn from_be_bytes(bytes: PointDataBuffer) -> 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: PointDataBuffer) -> Self { + Self::from_le_bytes([ + bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], + ]) + } +} + +impl FromBytes for u8 { + fn from_be_bytes(bytes: PointDataBuffer) -> Self { + Self::from_be_bytes([bytes[0]]) + } + + fn from_le_bytes(bytes: PointDataBuffer) -> Self { + Self::from_le_bytes([bytes[0]]) + } +} + #[cfg(test)] +#[cfg(feature = "derive")] mod tests { - use super::*; + use super::Fields; use rpcl2_derive::Fields; #[allow(dead_code)] diff --git a/src/points.rs b/src/points.rs index d1697fe..68e31b9 100644 --- a/src/points.rs +++ b/src/points.rs @@ -1,3 +1,4 @@ +//! Predefined point types commonly used in ROS. use crate::{Fields, PointConvertible, RPCL2Point}; #[cfg(feature = "derive")] diff --git a/src/prelude.rs b/src/prelude.rs index 9cbecf2..fd746fa 100644 --- a/src/prelude.rs +++ b/src/prelude.rs @@ -1,15 +1,17 @@ -/// Re-export commonly used types and traits. +//! Commonly used types and traits for predefined and custom point conversions. +pub use crate::{ + FieldDatatype, Fields, FromBytes, GetFieldDatatype, MsgConversionError, PointCloud2Msg, + PointConvertible, PointDataBuffer, RPCL2Point, +}; + pub use crate::points::*; +pub use crate::ros::*; #[cfg(feature = "rayon")] pub use rayon::prelude::*; -pub use crate::{Fields, MsgConversionError, PointCloud2Msg, PointConvertible, RPCL2Point}; - #[cfg(feature = "derive")] pub use type_layout::TypeLayout; #[cfg(feature = "derive")] pub use rpcl2_derive::*; - -pub use crate::convert::{FieldDatatype, FromBytes, GetFieldDatatype, PointDataBuffer}; diff --git a/src/ros_types.rs b/src/ros.rs similarity index 76% rename from src/ros_types.rs rename to src/ros.rs index d88fb00..6ff03db 100644 --- a/src/ros_types.rs +++ b/src/ros.rs @@ -1,13 +1,40 @@ +//! Types used to represent ROS messages and convert between different ROS crates. +//! +//! This intermediate layer allows various ROS libraries to be integrated to the conversion process. +//! +//! There are 2 functions needed to be implemented for a new ROS message library: +//! - `From` for converting from the library-generated message to [`crate::PointCloud2Msg`]. +//! ``` +//! #[cfg(feature = "fancy_ros_msg")] +//! impl From for crate::PointCloud2Msg { +//! fn from(msg: r2r::sensor_msgs::msg::PointCloud2) -> Self { +//! // Conversion code - avoid any point buffer copy! +//! } +//! } +//! ``` +//! +//! - `From` for converting from the [`crate::PointCloud2Msg`] to the library-generated message type. +//! ``` +//! #[cfg(feature = "fancy_ros_msg")] +//! impl From for fancy_ros::sensor_msgs::msg::PointCloud2 { +//! fn from(msg: crate::PointCloud2Msg) -> Self { +//! // Conversion code - avoid any point buffer copy! +//! } +//! } +//! ``` + +/// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages. #[cfg(not(feature = "rosrust_msg"))] #[derive(Clone, Debug, Default)] pub struct TimeMsg { - pub sec: u32, - pub nsec: u32, + pub sec: i32, + pub nanosec: u32, } #[cfg(feature = "rosrust_msg")] pub use rosrust::Time as TimeMsg; +/// Represents the [header of a ROS message](https://docs.ros2.org/latest/api/std_msgs/msg/Header.html). #[derive(Clone, Debug, Default)] pub struct HeaderMsg { pub seq: u32, @@ -15,6 +42,7 @@ pub struct HeaderMsg { pub frame_id: String, } +/// Describing a point encoded in the byte buffer of a PointCloud2 message. See the [official message description](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointField.html) for more information. #[derive(Clone, Debug)] pub struct PointFieldMsg { pub name: String, @@ -41,8 +69,8 @@ impl From for crate::PointCloud2Msg { header: HeaderMsg { seq: 0, stamp: TimeMsg { - sec: msg.header.stamp.sec as u32, - nsec: msg.header.stamp.nanosec, + sec: msg.header.stamp.sec, + nanosec: msg.header.stamp.nanosec, }, frame_id: msg.header.frame_id, }, @@ -69,9 +97,9 @@ impl From for crate::PointCloud2Msg { row_step: msg.row_step, data: msg.data, dense: if msg.is_dense { - crate::convert::Denseness::Dense + crate::Denseness::Dense } else { - crate::convert::Denseness::Sparse + crate::Denseness::Sparse }, } } @@ -83,8 +111,8 @@ impl From for r2r::sensor_msgs::msg::PointCloud2 { r2r::sensor_msgs::msg::PointCloud2 { header: r2r::std_msgs::msg::Header { stamp: r2r::builtin_interfaces::msg::Time { - sec: msg.header.stamp.sec as i32, - nanosec: msg.header.stamp.nsec, + sec: msg.header.stamp.sec, + nanosec: msg.header.stamp.nanosec, }, frame_id: msg.header.frame_id, }, @@ -108,8 +136,8 @@ impl From for r2r::sensor_msgs::msg::PointCloud2 { row_step: msg.row_step, data: msg.data, is_dense: match msg.dense { - crate::convert::Denseness::Dense => true, - crate::convert::Denseness::Sparse => false, + crate::Denseness::Dense => true, + crate::Denseness::Sparse => false, }, } } @@ -122,8 +150,8 @@ impl From for crate::PointCloud2Msg { header: HeaderMsg { seq: msg.header.seq, stamp: TimeMsg { - sec: msg.header.stamp.sec, - nsec: msg.header.stamp.nsec, + sec: msg.header.stamp.sec as i32, + nanosec: msg.header.stamp.nsec, }, frame_id: msg.header.frame_id, }, @@ -165,8 +193,8 @@ impl From for rosrust_msg::sensor_msgs::PointCloud2 { header: rosrust_msg::std_msgs::Header { seq: msg.header.seq, stamp: TimeMsg { - sec: msg.header.stamp.sec, - nsec: msg.header.stamp.nsec, + sec: msg.header.stamp.sec as u32, + nsec: msg.header.stamp.nanosec, }, frame_id: msg.header.frame_id, }, diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index 4edd9ac..3606bbd 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -75,6 +75,23 @@ fn write_cloud_from_vec() { println!("{:?}", msg); } +#[test] +#[cfg(feature = "derive")] +fn write_empty_cloud_vec() { + let cloud: Vec = vec![]; + let msg = PointCloud2Msg::try_from_vec(cloud); + assert!(msg.is_ok()); + assert!(msg.unwrap().data.is_empty()); +} + +#[test] +fn write_empty_cloud_iter() { + let cloud: Vec = vec![]; + let msg = PointCloud2Msg::try_from_iter(cloud); + assert!(msg.is_ok()); + assert!(msg.unwrap().data.is_empty()); +} + #[test] #[cfg(feature = "derive")] fn custom_xyz_f32() { @@ -377,8 +394,28 @@ fn write_xyzi_read_xyz() { convert_from_into_in_out_cloud!(write_cloud, PointXYZI, read_cloud, PointXYZ); } -#[cfg(feature = "derive")] #[test] +#[cfg(feature = "derive")] +fn write_xyzi_read_xyz_vec() { + let write_cloud = vec![ + PointXYZI::new(0.0, 1.0, 5.0, 0.0), + PointXYZI::new(1.0, 1.5, 5.0, 1.0), + PointXYZI::new(1.3, 1.6, 5.7, 2.0), + PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX), + ]; + + let read_cloud = [ + PointXYZ::new(0.0, 1.0, 5.0), + PointXYZ::new(1.0, 1.5, 5.0), + PointXYZ::new(1.3, 1.6, 5.7), + PointXYZ::new(f32::MAX, f32::MIN, f32::MAX), + ]; + + convert_from_into_in_out_cloud_vec!(write_cloud, PointXYZI, read_cloud, PointXYZ); +} + +#[test] +#[cfg(feature = "derive")] fn write_less_than_available() { #[derive(Debug, PartialEq, Clone, Default, TypeLayout)] #[repr(C)] @@ -458,3 +495,36 @@ fn write_less_than_available() { convert_from_into_in_out_cloud!(write_cloud, CustomPoint, read_cloud, CustomPoint); } + +#[test] +#[cfg(feature = "derive")] +#[allow(unused_variables)] +fn readme() { + use ros_pointcloud2::prelude::*; + + // PointXYZ (and many others) are provided by the crate. + let cloud_points = vec![ + PointXYZI::new(91.486, -4.1, 42.0001, 0.1), + PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MIN), + ]; + + let out_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap(); + + // Convert the ROS crate message type, we will use r2r here. + // let msg: r2r::sensor_msgs::msg::PointCloud2 = out_msg.into(); + // Publish ... + // ... now incoming from a topic. + // let in_msg: PointCloud2Msg = msg.into(); + let in_msg = out_msg; + + let processed_cloud = in_msg + .try_into_iter() + .unwrap() + .map(|point: PointXYZ| { + // Define the info you want to have from the Msg. + // Some logic here ... + + point + }) + .collect::>(); +}