diff --git a/README.md b/README.md index 990c657..4bcd698 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,6 @@ Providing an easy to use, generics defined, point-wise iterator abstraction over To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `PointCloud2Msg`. ## Examples - -Using a `Vec<>`. ```rust use ros_pointcloud2::{ pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, @@ -19,24 +17,15 @@ use ros_pointcloud2::{ // Your points (here using the predefined type PointXYZ). let cloud_points = vec![ - PointXYZ { - x: 91.486, - y: -4.1, - z: 42.0001, - }, - - PointXYZ { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - }, + PointXYZ {x: 91.486, y: -4.1, z: 42.0001,}, + PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,}, ]; // For equality test later let cloud_copy = cloud_points.clone(); // Vector -> Writer -> Message. -// You can also just give the Vec or anything that implements `IntoIter`. +// You can also just give the Vec or anything that implements `IntoIterator`. let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter()) .try_into() // iterating points here O(n) .unwrap(); @@ -49,7 +38,7 @@ let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter()) // ... now incoming from a topic. // let internal_msg: PointCloud2Msg = msg.into(); -// Message -> Reader. The Reader implements the Iterator trait. +// Message -> Reader -> your pipeline. The Reader implements the Iterator trait. let reader = ReaderXYZ::try_from(internal_msg).unwrap(); let new_cloud_points = reader .map(|point: PointXYZ| { diff --git a/src/convert.rs b/src/convert.rs index bb27969..c5d055f 100644 --- a/src/convert.rs +++ b/src/convert.rs @@ -8,10 +8,8 @@ pub enum FieldDatatype { I32, U8, U16, - #[default] U32, - I8, I16, } @@ -135,41 +133,32 @@ pub(crate) fn check_coord( } /// Matching field names from each meta data per point to the PointField name. -/// Always make sure to use the same order as in your Into<> implementation to have a correct mapping. +/// Always make sure to use the same order as in your conversion implementation to have a correct mapping. +/// +/// This trait is needed to implement the `PointConvertible` trait. +/// +/// # Example for full point conversion. +/// ``` +/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of}; +/// +/// #[derive(Clone, Debug, PartialEq, Copy)] +/// pub struct MyPointXYZI { +/// pub x: f32, +/// pub y: f32, +/// pub z: f32, +/// pub intensity: f32, +/// } +/// +/// impl MetaNames<1> for MyPointXYZI { +/// fn meta_names() -> [&'static str; 1] { +/// ["intensity"] +/// } +/// } +/// ``` pub trait MetaNames { fn meta_names() -> [&'static str; METADIM]; } -#[inline(always)] -pub(crate) fn add_point_to_byte_buffer< - T: FromBytes, - const SIZE: usize, - const DIM: usize, - const METADIM: usize, - C: PointConvertible, ->( - coords: C, - cloud: &mut PointCloud2Msg, -) -> Result { - let point: Point = coords.into(); - - // (x, y, z...) - point - .coords - .iter() - .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); - - // meta data description - point.meta.iter().for_each(|meta| { - let truncated_bytes = &meta.bytes[0..meta.datatype.size()]; - cloud.data.extend_from_slice(truncated_bytes); - }); - - cloud.width += 1; - - Ok(true) -} - /// 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 { diff --git a/src/lib.rs b/src/lib.rs index f3967e0..dd868a1 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,6 +1,6 @@ //! A library to work with the PointCloud2 message type from ROS. //! -//! ros_pointcloud2 mainly provides iterator-based [`ros_pointcloud2::Reader`] and [`ros_pointcloud2::Writer`] to interact +//! ros_pointcloud2 mainly provides iterator-based [`ros_pointcloud2::reader`] and [`ros_pointcloud2::writer`] to interact //! with the internal message type [`ros_pointcloud2::PointCloud2Msg`] without forcing an iteration. //! //! For ROS interoperability, the message type generated by the respective crate must be converted to @@ -23,25 +23,15 @@ //! //! // Your points (here using the predefined type PointXYZ). //! let cloud_points = vec![ -//! PointXYZ { -//! x: 9.0006, -//! y: 42.0, -//! z: -6.2, -//! }, -//! -//! PointXYZ { -//! x: f32::MAX, -//! y: f32::MIN, -//! z: f32::MAX, -//! }, +//! PointXYZ {x: 9.0006, y: 42.0, z: -6.2,}, +//! PointXYZ {x: f32::MAX, y: f32::MIN,z: f32::MAX,}, //! ]; //! //! // For equality test later //! let cloud_copy = cloud_points.clone(); //! //! // Vector -> Writer -> Message. -//! // Vector -> Writer -> Message. -//! // You can also just give the Vec or anything that implements `IntoIter`. +//! // You can also just give the Vec or anything that implements `IntoIterator`. //! let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter()) //! .try_into() // iterating points here O(n) //! .unwrap(); @@ -54,7 +44,7 @@ //! // ... now incoming from a topic. //! // let internal_msg: PointCloud2Msg = msg.into(); //! -//! // Message -> Reader. The Reader implements the Iterator trait. +//! // Message -> Reader -> your pipeline. The Reader implements the Iterator trait. //! let reader = ReaderXYZ::try_from(internal_msg).unwrap(); //! let new_cloud_points = reader //! .map(|point: PointXYZ| { @@ -91,10 +81,14 @@ macro_rules! size_of { pub mod reader; pub mod writer; +pub use convert::MetaNames; pub use ros_types::PointCloud2Msg; -use crate::convert::*; -use crate::pcl_utils::*; +use crate::convert::{ + FieldDatatype, + FromBytes, +}; + use crate::ros_types::PointFieldMsg; /// All errors that can occur for creating Reader and Writer. @@ -119,6 +113,42 @@ pub enum ConversionError { DataLengthMismatch, } +/// Internal point representation. It is used to store the coordinates and meta data of a point. +/// In each iteration, the Reader will convert the internal representation to the desired point type. +/// Implement the `From` traits for your point type to use the conversion as part of the [`PointConvertible`] trait. +/// +/// # Example +/// ``` +/// use ros_pointcloud2::Point; +/// +/// #[derive(Clone, Debug, PartialEq, Copy)] +/// pub struct MyPointXYZI { +/// pub x: f32, +/// pub y: f32, +/// pub z: f32, +/// pub intensity: f32, +/// } +/// +/// impl From for Point { +/// fn from(point: MyPointXYZI) -> Self { +/// Point { +/// coords: [point.x, point.y, point.z], +/// meta: [point.intensity.into()], +/// } +/// } +/// } +/// +/// impl From> for MyPointXYZI { +/// fn from(point: Point) -> Self { +/// Self { +/// x: point.coords[0], +/// y: point.coords[1], +/// z: point.coords[2], +/// intensity: point.meta[0].get(), +/// } +/// } +/// } +/// ``` pub struct Point { pub coords: [T; DIM], pub meta: [PointMeta; METADIM], @@ -126,6 +156,47 @@ pub struct Point { /// Trait to convert a point to a tuple of coordinates and meta data. /// Implement this trait for your point type to use the conversion. +/// +/// # Example +/// ``` +/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of}; +/// +/// #[derive(Clone, Debug, PartialEq, Copy)] +/// pub struct MyPointXYZI { +/// pub x: f32, +/// pub y: f32, +/// pub z: f32, +/// pub intensity: f32, +/// } +/// +/// impl From for Point { +/// fn from(point: MyPointXYZI) -> Self { +/// Point { +/// coords: [point.x, point.y, point.z], +/// meta: [point.intensity.into()], +/// } +/// } +/// } +/// +/// impl From> for MyPointXYZI { +/// fn from(point: Point) -> Self { +/// Self { +/// x: point.coords[0], +/// y: point.coords[1], +/// z: point.coords[2], +/// intensity: point.meta[0].get(), +/// } +/// } +/// } +/// +/// impl MetaNames<1> for MyPointXYZI { +/// fn meta_names() -> [&'static str; 1] { +/// ["intensity"] +/// } +/// } +/// +/// impl PointConvertible for MyPointXYZI {} +/// ``` pub trait PointConvertible: From> + Into> + MetaNames + Clone + 'static where @@ -155,14 +226,14 @@ pub struct PointMeta { impl Default for PointMeta { fn default() -> Self { Self { - bytes: [0; std::mem::size_of::()], + bytes: [u8::default(); std::mem::size_of::()], datatype: FieldDatatype::F32, } } } impl PointMeta { - /// Create a new PointMeta from a value + /// Create a new PointMeta from a value. /// /// # Example /// ``` diff --git a/src/reader.rs b/src/reader.rs index 7aa5c93..61aa956 100644 --- a/src/reader.rs +++ b/src/reader.rs @@ -1,4 +1,19 @@ -use crate::*; +use crate::{ + pcl_utils::*, + Point, + PointCloud2Msg, + PointConvertible, + ConversionError, + MetaNames, + PointMeta, + convert::{ + FromBytes, + FieldDatatype, + load_loadable, + Endianness, + check_coord, + }, +}; /// Convenience type for a Reader that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type. pub type ReaderF32 = @@ -124,7 +139,7 @@ pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>; /// ## Example /// ``` /// use ros_pointcloud2::{ -/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta, +/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta, /// }; /// /// type Xyz = f32; // coordinate type diff --git a/src/ros_types.rs b/src/ros_types.rs index 8061a6b..52febfa 100644 --- a/src/ros_types.rs +++ b/src/ros_types.rs @@ -142,75 +142,16 @@ impl From for rosrust_msg::sensor_msgs::PointCloud2 { fn from(msg: PointCloud2Msg) -> Self { rosrust_msg::sensor_msgs::PointCloud2 { header: rosrust_msg::std_msgs::Header { - seq: msg.header.seq, - stamp: TimeMsg { - sec: msg.header.stamp.sec, - nsec: msg.header.stamp.nsec, + seq: self.header.seq, + stamp: rosrust::Time { + sec: self.header.stamp.sec, + nsec: self.header.stamp.nsec, }, - frame_id: msg.header.frame_id, - }, - height: msg.height, - width: msg.width, - fields: msg - .fields - .into_iter() - .map(|field| rosrust_msg::sensor_msgs::PointField { - name: field.name, - offset: field.offset, - datatype: field.datatype, - count: field.count, - }) - .collect(), - is_bigendian: msg.is_bigendian, - point_step: msg.point_step, - row_step: msg.row_step, - data: msg.data, - is_dense: msg.is_dense, - } - } -} - -#[cfg(feature = "rclrs_msg")] -impl From for PointCloud2Msg { - fn from(msg: sensor_msgs::msg::PointCloud2) -> Self { - Self { - header: HeaderMsg { - seq: 0, - stamp: msg.header.stamp, - frame_id: msg.header.frame_id, - }, - height: msg.height, - width: msg.width, - fields: msg - .fields - .into_iter() - .map(|field| PointFieldMsg { - name: field.name, - offset: field.offset, - datatype: field.datatype, - count: field.count, - }) - .collect(), - is_bigendian: msg.is_bigendian, - point_step: msg.point_step, - row_step: msg.row_step, - data: msg.data, - is_dense: msg.is_dense, - } - } -} - -#[cfg(feature = "rclrs_msg")] -impl Into for PointCloud2Msg { - fn into(self) -> sensor_msgs::msg::PointCloud2 { - sensor_msgs::msg::PointCloud2 { - header: std_msgs::msg::Header { - stamp: self.header.stamp, frame_id: self.header.frame_id, }, - height: self.height, - width: self.width, - fields: self + height: msg.height, + width: msg.width, + fields: msg .fields .into_iter() .map(|field| sensor_msgs::msg::PointField { @@ -220,11 +161,11 @@ impl Into for PointCloud2Msg { count: field.count, }) .collect(), - is_bigendian: self.is_bigendian, - point_step: self.point_step, - row_step: self.row_step, - data: self.data, - is_dense: self.is_dense, + is_bigendian: msg.is_bigendian, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + is_dense: msg.is_dense, } } } \ No newline at end of file diff --git a/src/writer.rs b/src/writer.rs index fdbec99..d07392c 100644 --- a/src/writer.rs +++ b/src/writer.rs @@ -1,4 +1,15 @@ -use crate::*; +use crate::{ + pcl_utils::*, + Point, + PointCloud2Msg, + PointConvertible, + ConversionError, + ros_types::PointFieldMsg, + convert::{ + FromBytes, + FieldDatatype, + }, +}; /// Convenience type for a Writer that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type. pub type WriterF32 = @@ -105,7 +116,7 @@ pub type WriterXYZL = WriterF32<3, 1, PointXYZL>; /// ## Example /// ``` /// use ros_pointcloud2::{ -/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta, +/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta, /// }; /// /// type Xyz = f32; // coordinate type @@ -303,3 +314,33 @@ where } } } + +#[inline(always)] +fn add_point_to_byte_buffer< + T: FromBytes, + const SIZE: usize, + const DIM: usize, + const METADIM: usize, + C: PointConvertible, +>( + coords: C, + cloud: &mut PointCloud2Msg, +) -> Result { + let point: Point = coords.into(); + + // (x, y, z...) + point + .coords + .iter() + .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); + + // meta data description + point.meta.iter().for_each(|meta| { + let truncated_bytes = &meta.bytes[0..meta.datatype.size()]; + cloud.data.extend_from_slice(truncated_bytes); + }); + + cloud.width += 1; + + Ok(true) +} diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index 35d4eee..9d77b0a 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -65,7 +65,7 @@ fn custom_xyz_f32() { } } - impl convert::MetaNames for CustomPoint { + impl MetaNames for CustomPoint { fn meta_names() -> [&'static str; METADIM] { [] } @@ -130,7 +130,7 @@ fn custom_xyzi_f32() { } } - impl convert::MetaNames for CustomPoint { + impl MetaNames for CustomPoint { fn meta_names() -> [&'static str; METADIM] { ["intensity"] } @@ -215,7 +215,7 @@ fn custom_rgba_f32() { } } - impl convert::MetaNames for CustomPoint { + impl MetaNames for CustomPoint { fn meta_names() -> [&'static str; METADIM] { ["r", "g", "b", "a"] } @@ -741,7 +741,7 @@ fn write_less_than_available() { } } - impl convert::MetaNames for CustomPoint { + impl MetaNames for CustomPoint { fn meta_names() -> [&'static str; METADIM] { [] }