mirror
/
ros_pointcloud2
mirror of https://github.com/stelzo/ros_pointcloud2.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ros_pointcloud2/src/lib.rs

1422 lines
42 KiB
Rust
Raw Blame History

//! A PointCloud2 message conversion library.
//!
//! The library provides the [`PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators.
//!
//! 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)
//!
//! You can use the iterator functions for more control over the conversion process.
//! - [`PointCloud2Msg::try_from_iter`]
//! - [`PointCloud2Msg::try_into_iter`]
//!
//! These feature predictable but [slightly worse](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#performance) performance while avoiding memory allocations.
//!
//! 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
//!
//! 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.
//!
//! 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![
//! 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 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 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::<Vec<_>>();
//!
//! assert_eq!(processed_cloud, cloud_copy);
//! ```
//!
//! # Features
//! - **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<RPCL2Point<4>> 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<MyPointXYZI> 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<MyPointXYZI> = 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 points;
pub mod prelude;
pub mod ros;
pub mod iterator;
use std::num::TryFromIntError;
use std::str::FromStr;
use crate::ros::{HeaderMsg, PointFieldMsg};
/// All errors that can occur while converting to or from the message type.
#[derive(Debug)]
pub enum MsgConversionError {
InvalidFieldFormat,
UnsupportedFieldType(String),
DataLengthMismatch,
FieldsNotFound(Vec<String>),
UnsupportedFieldCount,
NumberConversion,
}
impl From<TryFromIntError> for MsgConversionError {
fn from(_: TryFromIntError) -> Self {
MsgConversionError::NumberConversion
}
}
impl std::fmt::Display for MsgConversionError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
match self {
MsgConversionError::InvalidFieldFormat => {
write!(f, "The field does not match the expected datatype.")
}
MsgConversionError::UnsupportedFieldType(datatype) => {
write!(
f,
"The field datatype is not supported by the ROS message description: {}",
datatype
)
}
MsgConversionError::DataLengthMismatch => {
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, "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::error::Error for MsgConversionError {
fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
None
}
}
#[cfg(feature = "derive")]
fn system_endian() -> Endian {
if cfg!(target_endian = "big") {
Endian::Big
} else if cfg!(target_endian = "little") {
Endian::Little
} else {
panic!("Unsupported Endian");
}
}
/// The intermediate point cloud type for ROS integrations.
///
/// 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,
pub dimensions: CloudDimensions,
pub fields: Vec<PointFieldMsg>,
pub endian: Endian,
pub point_step: u32,
pub row_step: u32,
pub data: Vec<u8>,
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);
impl CloudDimensionsBuilder {
pub fn new_with_width(width: usize) -> Self {
Self(width)
}
pub fn build(self) -> Result<CloudDimensions, MsgConversionError> {
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 the builder pattern to avoid invalid states.
#[derive(Clone, Debug, Default)]
pub struct PointCloud2MsgBuilder {
header: HeaderMsg,
width: u32,
fields: Vec<PointFieldMsg>,
is_big_endian: bool,
point_step: u32,
row_step: u32,
data: Vec<u8>,
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<PointFieldMsg>) -> 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<u8>) -> 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<PointCloud2Msg, MsgConversionError> {
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::<Result<Vec<_>, _>>()?
.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
},
})
}
}
/// Dimensions of the point cloud as width and height.
#[derive(Clone, Debug, Default)]
pub struct CloudDimensions {
pub width: u32,
pub height: u32,
}
impl PointCloud2Msg {
#[cfg(feature = "derive")]
#[inline(always)]
fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError>
where
C: PointConvertible<N>,
{
let point: RPCL2Point<N> = 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: u32 = 0;
let mut field_counter = 0;
for (f, msg_f) in layout.fields.iter().zip(self.fields.iter()) {
match f {
PointField::Field {
datatype,
size,
count,
} => {
let f_translated = field_names[field_counter].to_string();
field_counter += 1;
if msg_f.name != f_translated
|| msg_f.offset != offset
|| msg_f.datatype != *datatype
|| msg_f.count != 1
{
return Ok(ByteSimilarity::Different);
}
offset += size * count;
}
PointField::Padding(size) => {
offset += size;
}
}
}
Ok(if offset == self.point_step {
ByteSimilarity::Equal
} else {
ByteSimilarity::Overlapping
})
}
/// Create a PointCloud2Msg from any iterable type.
///
/// # Example
/// ```
/// use ros_pointcloud2::prelude::*;
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ::new(1.0, 2.0, 3.0),
/// PointXYZ::new(4.0, 5.0, 6.0),
/// ];
///
// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
/// ```
pub fn try_from_iter<const N: usize, C>(
iterable: impl IntoIterator<Item = C>,
) -> Result<Self, MsgConversionError>
where
C: PointConvertible<N>,
{
let (mut cloud, point_step) = {
let point: RPCL2Point<N> = 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;
iterable.into_iter().for_each(|pointdata| {
let point: RPCL2Point<N> = 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 = cloud.width(cloud_width);
cloud = cloud.row_step(cloud_width * point_step);
cloud.build()
}
/// 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<const N: usize, C>(
iterable: impl rayon::iter::ParallelIterator<Item = C>,
) -> Result<Self, MsgConversionError>
where
C: PointConvertible<N> + Send + Sync,
{
Self::try_from_vec(iterable.collect::<Vec<_>>())
}
/// 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<PointXYZ> = 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<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError>
where
C: PointConvertible<N>,
{
match (system_endian(), Endian::default()) {
(Endian::Big, Endian::Big) | (Endian::Little, Endian::Little) => {
let (mut cloud, point_step) = {
let point: RPCL2Point<N> = 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<PointFieldMsg> = 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());
let raw_data: *mut C = cloud.data.as_ptr() as *mut C;
unsafe {
std::ptr::copy_nonoverlapping(
vec.as_ptr() as *const u8,
raw_data as *mut u8,
bytes_total,
);
}
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<PointXYZI> = 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<PointXYZ> = 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<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError>
where
C: PointConvertible<N>,
{
match (system_endian(), self.endian) {
(Endian::Big, Endian::Big) | (Endian::Little, Endian::Little) => {
let bytematch = match self.byte_similarity::<N, C>()? {
ByteSimilarity::Equal => true,
ByteSimilarity::Overlapping => false,
ByteSimilarity::Different => return Ok(self.try_into_iter()?.collect()),
};
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 {
unsafe {
std::ptr::copy_nonoverlapping(
self.data.as_ptr(),
vec.as_mut_ptr() as *mut u8,
self.data.len(),
);
vec.set_len(cloud_width);
}
} else {
unsafe {
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);
}
}
}
Ok(vec)
}
_ => 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<PointXYZI> = 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::<Vec<PointXYZ>>();
/// ```
pub fn try_into_iter<const N: usize, C>(
self,
) -> Result<impl Iterator<Item = C>, MsgConversionError>
where
C: PointConvertible<N>,
{
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<PointXYZI> = 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_par_iter().unwrap().collect::<Vec<PointXYZ>>();
/// assert_eq!(2, cloud_points_out.len());
/// ```
#[cfg(feature = "rayon")]
pub fn try_into_par_iter<const N: usize, C>(
self,
) -> Result<impl rayon::iter::ParallelIterator<Item = C>, MsgConversionError>
where
C: PointConvertible<N> + Send + Sync,
{
iterator::PointCloudIterator::try_from(self)
}
}
/// 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 [`PointConvertible`] trait for more information.
pub struct RPCL2Point<const N: usize> {
fields: [PointData; N],
}
impl<const N: usize> Default for RPCL2Point<N> {
fn default() -> Self {
Self {
fields: [PointData::default(); N],
}
}
}
impl<const N: usize> std::ops::Index<usize> for RPCL2Point<N> {
type Output = PointData;
fn index(&self, index: usize) -> &Self::Output {
&self.fields[index]
}
}
impl<const N: usize> From<[PointData; N]> for RPCL2Point<N> {
fn from(fields: [PointData; N]) -> Self {
Self { fields }
}
}
/// Trait to enable point conversions on the fly.
///
/// # Example
/// ```
/// use ros_pointcloud2::prelude::*;
///
/// #[derive(Clone, Debug, PartialEq, Copy, Default)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
///
/// impl From<MyPointXYZI> for RPCL2Point<4> {
/// fn from(point: MyPointXYZI) -> Self {
/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
/// }
/// }
///
/// impl From<RPCL2Point<4>> 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(not(feature = "derive"))]
pub trait PointConvertible<const N: usize>:
From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Clone + 'static + Default
{
}
/// 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<MyPointXYZI> for RPCL2Point<4> {
/// fn from(point: MyPointXYZI) -> Self {
/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
/// }
/// }
///
/// impl From<RPCL2Point<4>> 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<const N: usize>:
type_layout::TypeLayout + From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + 'static + Default
{
}
/// 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<const N: usize> {
fn field_names_ordered() -> [&'static str; N];
}
#[cfg(feature = "derive")]
enum PointField {
Padding(u32),
Field { size: u32, datatype: u8, count: u32 },
}
#[cfg(feature = "derive")]
struct TypeLayoutInfo {
fields: Vec<PointField>,
}
#[cfg(feature = "derive")]
impl TryFrom<type_layout::Field> for PointField {
type Error = MsgConversionError;
fn try_from(f: type_layout::Field) -> Result<Self, Self::Error> {
match f {
type_layout::Field::Field { name: _, ty, size } => {
let typename: String = ty.into_owned().to_lowercase();
let datatype = FieldDatatype::from_str(typename.as_str())?;
Ok(Self::Field {
size: size.try_into()?,
datatype: datatype.into(),
count: 1,
})
}
type_layout::Field::Padding { size } => Ok(Self::Padding(size.try_into()?)),
}
}
}
#[cfg(feature = "derive")]
impl TryFrom<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
type Error = MsgConversionError;
fn try_from(t: type_layout::TypeLayoutInfo) -> Result<Self, Self::Error> {
let fields: Vec<PointField> = t
.fields
.into_iter()
.map(PointField::try_from)
.collect::<Result<Vec<_>, _>>()?;
Ok(Self { fields })
}
}
/// Single data representation for a point.
///
/// 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
/// ```
/// use ros_pointcloud2::PointData;
///
/// let original_data: f64 = 1.0;
/// let meta = PointData::new(original_data);
/// let my_data: f64 = meta.get();
/// ```
#[derive(Debug, Clone, Copy)]
pub struct PointData {
bytes: [u8; std::mem::size_of::<f64>()],
endian: Endian,
datatype: FieldDatatype,
}
impl Default for PointData {
fn default() -> Self {
Self {
bytes: [u8::default(); std::mem::size_of::<f64>()],
datatype: FieldDatatype::F32,
endian: Endian::default(),
}
}
}
impl PointData {
/// Create a new PointData from a value.
///
/// # Example
/// ```
/// let meta = ros_pointcloud2::PointData::new(1.0);
/// ```
#[inline(always)]
pub fn new<T: FromBytes>(value: T) -> Self {
Self {
bytes: value.into().raw(),
datatype: T::field_datatype(),
..Default::default()
}
}
#[inline(always)]
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::<f64>()];
unsafe {
let data_ptr = data.as_ptr().add(offset);
let bytes_ptr = bytes.as_ptr() as *mut u8;
std::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size());
}
Self {
bytes,
datatype,
endian,
}
}
/// Get the numeric value from the PointData description.
///
/// # Example
/// ```
/// let original_data: f64 = 1.0;
/// let meta = ros_pointcloud2::PointData::new(original_data);
/// let my_data: f64 = meta.get();
/// ```
pub fn get<T: FromBytes>(&self) -> T {
match self.endian {
Endian::Big => T::from_be_bytes(PointDataBuffer::new(self.bytes)),
Endian::Little => T::from_le_bytes(PointDataBuffer::new(self.bytes)),
}
}
}
impl From<f32> for PointData {
fn from(value: f32) -> Self {
Self::new(value)
}
}
impl From<f64> for PointData {
fn from(value: f64) -> Self {
Self::new(value)
}
}
impl From<i32> for PointData {
fn from(value: i32) -> Self {
Self::new(value)
}
}
impl From<u8> for PointData {
fn from(value: u8) -> Self {
Self::new(value)
}
}
impl From<u16> for PointData {
fn from(value: u16) -> Self {
Self::new(value)
}
}
impl From<u32> for PointData {
fn from(value: u32) -> Self {
Self::new(value)
}
}
impl From<i8> for PointData {
fn from(value: i8) -> Self {
Self::new(value)
}
}
impl From<i16> for PointData {
fn from(value: i16) -> Self {
Self::new(value)
}
}
/// 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::<u8>(),
FieldDatatype::U16 => std::mem::size_of::<u16>(),
FieldDatatype::U32 => std::mem::size_of::<u32>(),
FieldDatatype::I8 => std::mem::size_of::<i8>(),
FieldDatatype::I16 => std::mem::size_of::<i16>(),
FieldDatatype::I32 => std::mem::size_of::<i32>(),
FieldDatatype::F32 => std::mem::size_of::<f32>(),
FieldDatatype::F64 => std::mem::size_of::<f64>(),
FieldDatatype::RGB => std::mem::size_of::<f32>(), // packed in f32
}
}
}
impl FromStr for FieldDatatype {
type Err = MsgConversionError;
fn from_str(s: &str) -> Result<Self, Self::Err> {
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<u8> for FieldDatatype {
type Error = MsgConversionError;
fn try_from(value: u8) -> Result<Self, Self::Error> {
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<FieldDatatype> 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, Self::Error> {
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<usize> 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<const N: usize> From<[u8; N]> for PointDataBuffer {
fn from(data: [u8; N]) -> Self {
Self::from(data.as_slice())
}
}
impl From<i8> for PointDataBuffer {
fn from(x: i8) -> Self {
x.to_le_bytes().into()
}
}
impl From<i16> for PointDataBuffer {
fn from(x: i16) -> Self {
x.to_le_bytes().into()
}
}
impl From<u16> for PointDataBuffer {
fn from(x: u16) -> Self {
x.to_le_bytes().into()
}
}
impl From<i32> for PointDataBuffer {
fn from(x: i32) -> Self {
x.to_le_bytes().into()
}
}
impl From<u32> for PointDataBuffer {
fn from(x: u32) -> Self {
x.to_le_bytes().into()
}
}
impl From<f32> for PointDataBuffer {
fn from(x: f32) -> Self {
x.to_le_bytes().into()
}
}
impl From<f64> for PointDataBuffer {
fn from(x: f64) -> Self {
x.to_le_bytes().into()
}
}
impl From<u8> for PointDataBuffer {
fn from(x: u8) -> Self {
x.to_le_bytes().into()
}
}
impl From<points::RGB> 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<PointDataBuffer> {
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::Fields;
use rpcl2_derive::Fields;
#[allow(dead_code)]
#[derive(Fields)]
struct TestStruct {
field1: String,
#[rpcl2(name = "renamed_field")]
field2: i32,
field3: f64,
field4: bool,
}
#[test]
fn test_struct_names() {
let names = TestStruct::field_names_ordered();
assert_eq!(names, ["field1", "renamed_field", "field3", "field4"]);
}
}
Reference in New Issue View Git Blame Copy Permalink