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

no_std compatible

This commit is contained in:
stelzo 2024-05-15 18:36:53 +02:00
parent 56c29ac96c
commit 529c7e1a77
6 changed files with 185 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Cargo.toml
View File

@ -35,7 +35,7 @@ r2r = { version = "0.8.4", optional = true }
rayon = { version = "1", optional = true } rayon = { version = "1", optional = true }
nalgebra = { version = "0", optional = true } nalgebra = { version = "0", optional = true }
rpcl2_derive = { path = "./rpcl2_derive", optional = true } rpcl2_derive = { path = "./rpcl2_derive", optional = true }
type-layout = { version = "0.2", optional = true } type-layout = { git = "https://github.com/stelzo/type-layout", branch = "syn2", optional = true }
[dev-dependencies] [dev-dependencies]
rand = "0.8" rand = "0.8"
@ -51,8 +51,9 @@ r2r_msg = ["dep:r2r"]
rayon = ["dep:rayon"] rayon = ["dep:rayon"]
derive = ["dep:rpcl2_derive", "dep:type-layout"] derive = ["dep:rpcl2_derive", "dep:type-layout"]
nalgebra = ["dep:nalgebra"] nalgebra = ["dep:nalgebra"]
std = []
default = ["derive"] default = ["derive", "std"]
[package.metadata.docs.rs] [package.metadata.docs.rs]
features = ["derive", "nalgebra", "rayon"] features = ["derive", "nalgebra", "rayon"]

4
README.md
View File

@ -95,6 +95,10 @@ The results are measured on an Intel i7-14700 with benchmarks from [this reposit
For minimizing the conversion overhead in general, always use the functions that best fit your use case. For minimizing the conversion overhead in general, always use the functions that best fit your use case.
## `#[no_std]`
The `_iter` conversions are compatible with `#[no_std]` environments when an allocator is provided. This is due to the fact that names for point fields do not have a maximum length, and PointCloud2 data vectors can have arbitrary sizes. Use `default-features = false` to disable std for this crate.
## License ## License
[MIT](https://choosealicense.com/licenses/mit/) [MIT](https://choosealicense.com/licenses/mit/)

35
src/iterator.rs
View File

@ -1,10 +1,19 @@
//! Iterator implementations for PointCloud2Msg including a parallel iterator for rayon. //! Iterator implementations for [`PointCloud2Msg`] including a parallel iterator for rayon.
use crate::{ use crate::{
Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData,
RPCL2Point, RPCL2Point,
}; };
/// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg. #[cfg(not(feature = "std"))]
use alloc::string::String;
#[cfg(not(feature = "std"))]
use alloc::vec::Vec;
#[cfg(not(feature = "std"))]
use alloc::borrow::ToOwned;
/// The PointCloudIterator provides a an iterator abstraction of the [`PointCloud2Msg`].
/// ///
/// The iterator is defined at compile time, so the point has to be described via template arguments. /// The iterator is defined at compile time, so the point has to be described via template arguments.
/// ///
@ -19,7 +28,7 @@ use crate::{
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); /// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); /// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
/// ///
/// ros_pointcloud2 supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags. /// `ros_pointcloud2` supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags.
/// ///
pub struct PointCloudIterator<const N: usize, C> pub struct PointCloudIterator<const N: usize, C>
where where
@ -28,7 +37,7 @@ where
iteration: usize, iteration: usize,
iteration_back: usize, iteration_back: usize,
data: ByteBufferView<N>, data: ByteBufferView<N>,
phantom_c: std::marker::PhantomData<C>, // internally used for pdata names array phantom_c: core::marker::PhantomData<C>, // internally used for pdata names array
} }
#[cfg(feature = "rayon")] #[cfg(feature = "rayon")]
@ -156,7 +165,10 @@ where
} }
struct ByteBufferView<const N: usize> { struct ByteBufferView<const N: usize> {
#[cfg(feature = "rayon")]
data: std::sync::Arc<[u8]>, data: std::sync::Arc<[u8]>,
#[cfg(not(feature = "rayon"))]
data: Vec<u8>,
start_point_idx: usize, start_point_idx: usize,
end_point_idx: usize, end_point_idx: usize,
point_step_size: usize, point_step_size: usize,
@ -176,7 +188,10 @@ impl<const N: usize> ByteBufferView<N> {
endian: Endian, endian: Endian,
) -> Self { ) -> Self {
Self { Self {
#[cfg(feature = "rayon")]
data: std::sync::Arc::<[u8]>::from(data), data: std::sync::Arc::<[u8]>::from(data),
#[cfg(not(feature = "rayon"))]
data,
start_point_idx, start_point_idx,
end_point_idx, end_point_idx,
point_step_size, point_step_size,
@ -191,7 +206,7 @@ impl<const N: usize> ByteBufferView<N> {
self.end_point_idx - self.start_point_idx + 1 self.end_point_idx - self.start_point_idx + 1
} }
#[inline(always)] #[inline]
fn point_at(&self, idx: usize) -> RPCL2Point<N> { fn point_at(&self, idx: usize) -> RPCL2Point<N> {
let offset = (self.start_point_idx + idx) * self.point_step_size; let offset = (self.start_point_idx + idx) * self.point_step_size;
let mut pdata = [PointData::default(); N]; let mut pdata = [PointData::default(); N];
@ -244,8 +259,8 @@ where
{ {
type Error = MsgConversionError; type Error = MsgConversionError;
/// Convert a PointCloud2Msg into an iterator. /// Convert a [`PointCloud2Msg`] into an iterator.
/// Converting a PointCloud2Msg into an iterator is a fallible operation since the message could contain a subset of the required fields. /// The conversion to an iterator is a fallible operation since the message could contain a subset of the required fields.
/// ///
/// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small. /// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
/// It therefore has a constant time complexity O(1) for practical purposes. /// It therefore has a constant time complexity O(1) for practical purposes.
@ -332,7 +347,7 @@ where
iteration: 0, iteration: 0,
iteration_back: cloud_length - 1, iteration_back: cloud_length - 1,
data, data,
phantom_c: std::marker::PhantomData, phantom_c: core::marker::PhantomData,
}) })
} }
} }
@ -342,16 +357,18 @@ where
C: Fields<N>, C: Fields<N>,
{ {
#[inline] #[inline]
#[must_use]
fn from_byte_buffer_view(data: ByteBufferView<N>) -> Self { fn from_byte_buffer_view(data: ByteBufferView<N>) -> Self {
Self { Self {
iteration: 0, iteration: 0,
iteration_back: data.len() - 1, iteration_back: data.len() - 1,
data, data,
phantom_c: std::marker::PhantomData, phantom_c: core::marker::PhantomData,
} }
} }
#[inline] #[inline]
#[must_use]
pub fn split_at(self, point_index: usize) -> (Self, Self) { pub fn split_at(self, point_index: usize) -> (Self, Self) {
let (left_data, right_data) = self.data.split_at(point_index); let (left_data, right_data) = self.data.split_at(point_index);
( (

172
src/lib.rs
View File

@ -125,10 +125,17 @@
//! pub intensity: f32, //! pub intensity: f32,
//! } //! }
//! ``` //! ```
#![crate_type = "lib"] #![crate_type = "lib"]
#![warn(clippy::print_stderr)] #![warn(clippy::print_stderr)]
#![warn(clippy::print_stdout)] #![warn(clippy::print_stdout)]
#![warn(clippy::unwrap_used)]
#![warn(clippy::cargo)]
#![warn(clippy::std_instead_of_core)]
#![warn(clippy::alloc_instead_of_core)]
#![warn(clippy::std_instead_of_alloc)]
#![cfg_attr(not(feature = "std"), no_std)]
// Setup an allocator with #[global_allocator]
// see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html
pub mod points; pub mod points;
pub mod prelude; pub mod prelude;
@ -136,46 +143,65 @@ pub mod ros;
pub mod iterator; pub mod iterator;
use std::num::TryFromIntError;
use std::str::FromStr;
use crate::ros::{HeaderMsg, PointFieldMsg}; use crate::ros::{HeaderMsg, PointFieldMsg};
use core::str::FromStr;
#[cfg(not(feature = "std"))]
#[macro_use]
extern crate alloc;
#[cfg(not(feature = "std"))]
use alloc::string::String;
#[cfg(not(feature = "std"))]
use alloc::vec::Vec;
/// All errors that can occur while converting to or from the message type. /// All errors that can occur while converting to or from the message type.
#[derive(Debug)] #[derive(Debug)]
pub enum MsgConversionError { pub enum MsgConversionError {
InvalidFieldFormat, InvalidFieldFormat,
#[cfg(feature = "std")]
UnsupportedFieldType(String), UnsupportedFieldType(String),
#[cfg(not(feature = "std"))]
UnsupportedFieldType,
DataLengthMismatch, DataLengthMismatch,
FieldsNotFound(Vec<String>), FieldsNotFound(Vec<String>),
UnsupportedFieldCount, UnsupportedFieldCount,
NumberConversion, NumberConversion,
} }
impl From<TryFromIntError> for MsgConversionError { impl From<core::num::TryFromIntError> for MsgConversionError {
fn from(_: TryFromIntError) -> Self { fn from(_: core::num::TryFromIntError) -> Self {
MsgConversionError::NumberConversion MsgConversionError::NumberConversion
} }
} }
impl std::fmt::Display for MsgConversionError { impl core::fmt::Display for MsgConversionError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self { match self {
MsgConversionError::InvalidFieldFormat => { MsgConversionError::InvalidFieldFormat => {
write!(f, "The field does not match the expected datatype.") write!(f, "The field does not match the expected datatype.")
} }
#[cfg(feature = "std")]
MsgConversionError::UnsupportedFieldType(datatype) => { MsgConversionError::UnsupportedFieldType(datatype) => {
write!( write!(
f, f,
"The field datatype is not supported by the ROS message description: {}", "The field datatype is not supported by the ROS message description: {datatype}"
datatype )
}
#[cfg(not(feature = "std"))]
MsgConversionError::UnsupportedFieldType => {
write!(
f,
"There is an unsupported field type in the ROS message description."
) )
} }
MsgConversionError::DataLengthMismatch => { 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.") 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) => { MsgConversionError::FieldsNotFound(fields) => {
write!(f, "Some fields are not found in the message: {:?}", fields) write!(f, "Some fields are not found in the message: {fields:?}")
} }
MsgConversionError::UnsupportedFieldCount => { MsgConversionError::UnsupportedFieldCount => {
write!( write!(
@ -190,6 +216,7 @@ impl std::fmt::Display for MsgConversionError {
} }
} }
#[cfg(feature = "std")]
impl std::error::Error for MsgConversionError { impl std::error::Error for MsgConversionError {
fn source(&self) -> Option<&(dyn std::error::Error + 'static)> { fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
None None
@ -246,11 +273,12 @@ enum ByteSimilarity {
Different, Different,
} }
/// Creating a CloudDimensions type with the builder pattern to avoid invalid states when using 1-row point clouds. /// Creating a [`CloudDimensions`] type with the builder pattern to avoid invalid states when using 1-row point clouds.
#[derive(Clone, Debug)] #[derive(Clone, Debug)]
pub struct CloudDimensionsBuilder(usize); pub struct CloudDimensionsBuilder(usize);
impl CloudDimensionsBuilder { impl CloudDimensionsBuilder {
#[must_use]
pub fn new_with_width(width: usize) -> Self { pub fn new_with_width(width: usize) -> Self {
Self(width) Self(width)
} }
@ -263,12 +291,12 @@ impl CloudDimensionsBuilder {
Ok(CloudDimensions { Ok(CloudDimensions {
width, width,
height: if self.0 > 0 { 1 } else { 0 }, height: u32::from(self.0 > 0),
}) })
} }
} }
/// Creating a PointCloud2Msg with the builder pattern to avoid invalid states. /// Creating a [`PointCloud2Msg`] with the builder pattern to avoid invalid states.
#[derive(Clone, Debug, Default)] #[derive(Clone, Debug, Default)]
pub struct PointCloud2MsgBuilder { pub struct PointCloud2MsgBuilder {
header: HeaderMsg, header: HeaderMsg,
@ -282,50 +310,63 @@ pub struct PointCloud2MsgBuilder {
} }
impl PointCloud2MsgBuilder { impl PointCloud2MsgBuilder {
#[must_use]
pub fn new() -> Self { pub fn new() -> Self {
Self::default() Self::default()
} }
#[must_use]
pub fn header(mut self, header: HeaderMsg) -> Self { pub fn header(mut self, header: HeaderMsg) -> Self {
self.header = header; self.header = header;
self self
} }
#[must_use]
pub fn width(mut self, width: u32) -> Self { pub fn width(mut self, width: u32) -> Self {
self.width = width; self.width = width;
self self
} }
#[must_use]
pub fn fields(mut self, fields: Vec<PointFieldMsg>) -> Self { pub fn fields(mut self, fields: Vec<PointFieldMsg>) -> Self {
self.fields = fields; self.fields = fields;
self self
} }
#[must_use]
pub fn endian(mut self, is_big_endian: bool) -> Self { pub fn endian(mut self, is_big_endian: bool) -> Self {
self.is_big_endian = is_big_endian; self.is_big_endian = is_big_endian;
self self
} }
#[must_use]
pub fn point_step(mut self, point_step: u32) -> Self { pub fn point_step(mut self, point_step: u32) -> Self {
self.point_step = point_step; self.point_step = point_step;
self self
} }
#[must_use]
pub fn row_step(mut self, row_step: u32) -> Self { pub fn row_step(mut self, row_step: u32) -> Self {
self.row_step = row_step; self.row_step = row_step;
self self
} }
#[must_use]
pub fn data(mut self, data: Vec<u8>) -> Self { pub fn data(mut self, data: Vec<u8>) -> Self {
self.data = data; self.data = data;
self self
} }
#[must_use]
pub fn dense(mut self, is_dense: bool) -> Self { pub fn dense(mut self, is_dense: bool) -> Self {
self.is_dense = is_dense; self.is_dense = is_dense;
self self
} }
/// Build the [`PointCloud2Msg`] from the builder.
///
/// # Errors
/// Returns an error if the fields are empty, the field count is not 1, the field format is invalid, the data length does not match the point step, or the field size is too large.
pub fn build(self) -> Result<PointCloud2Msg, MsgConversionError> { pub fn build(self) -> Result<PointCloud2Msg, MsgConversionError> {
if self.fields.is_empty() { if self.fields.is_empty() {
return Err(MsgConversionError::FieldsNotFound(vec![])); return Err(MsgConversionError::FieldsNotFound(vec![]));
@ -382,7 +423,7 @@ pub struct CloudDimensions {
impl PointCloud2Msg { impl PointCloud2Msg {
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
#[inline(always)] #[inline]
fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError> fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError>
where where
C: PointConvertible<N>, C: PointConvertible<N>,
@ -405,7 +446,8 @@ impl PointCloud2Msg {
size, size,
count, count,
} => { } => {
let f_translated = field_names[field_counter].to_string(); let f_translated = String::from_str(field_names[field_counter])
.expect("Field name is not a valid string.");
field_counter += 1; field_counter += 1;
if msg_f.name != f_translated if msg_f.name != f_translated
@ -431,7 +473,7 @@ impl PointCloud2Msg {
}) })
} }
/// Create a PointCloud2Msg from any iterable type. /// Create a [`PointCloud2Msg`] from any iterable type.
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -493,7 +535,7 @@ impl PointCloud2Msg {
point.fields.iter().for_each(|pdata| { point.fields.iter().for_each(|pdata| {
let truncated_bytes = unsafe { let truncated_bytes = unsafe {
std::slice::from_raw_parts(pdata.bytes.as_ptr(), pdata.datatype.size()) core::slice::from_raw_parts(pdata.bytes.as_ptr(), pdata.datatype.size())
}; };
cloud.data.extend_from_slice(truncated_bytes); cloud.data.extend_from_slice(truncated_bytes);
}); });
@ -518,7 +560,7 @@ impl PointCloud2Msg {
Self::try_from_vec(iterable.collect::<Vec<_>>()) Self::try_from_vec(iterable.collect::<Vec<_>>())
} }
/// Create a PointCloud2Msg from a Vec of points. /// Create a [`PointCloud2Msg`] from a Vec of points.
/// Since the point type is known at compile time, the conversion is done by direct copy. /// Since the point type is known at compile time, the conversion is done by direct copy.
/// ///
/// # Example /// # Example
@ -532,6 +574,9 @@ impl PointCloud2Msg {
/// ///
/// let msg_out = PointCloud2Msg::try_from_vec(cloud_points).unwrap(); /// let msg_out = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
/// ``` /// ```
///
/// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError> pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError>
where where
@ -552,7 +597,8 @@ impl PointCloud2Msg {
let mut offset = 0; let mut offset = 0;
let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(layout.fields.len()); let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(layout.fields.len());
for f in layout.fields.into_iter() { for f in layout.fields.into_iter() {
let f_translated = field_names[fields.len()].to_string(); let f_translated = String::from_str(field_names[fields.len()])
.expect("Field name is not a valid string.");
match f { match f {
PointField::Field { PointField::Field {
datatype, datatype,
@ -585,9 +631,9 @@ impl PointCloud2Msg {
cloud.data.resize(bytes_total, u8::default()); cloud.data.resize(bytes_total, u8::default());
let raw_data: *mut C = cloud.data.as_ptr() as *mut C; let raw_data: *mut C = cloud.data.as_ptr() as *mut C;
unsafe { unsafe {
std::ptr::copy_nonoverlapping( core::ptr::copy_nonoverlapping(
vec.as_ptr() as *const u8, vec.as_ptr().cast::<u8>(),
raw_data as *mut u8, raw_data.cast::<u8>(),
bytes_total, bytes_total,
); );
} }
@ -601,7 +647,7 @@ impl PointCloud2Msg {
} }
} }
/// Convert the PointCloud2Msg to a Vec of points. /// Convert the [`PointCloud2Msg`] to a Vec of points.
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -616,6 +662,9 @@ impl PointCloud2Msg {
/// let cloud_points_out: Vec<PointXYZ> = msg_out.try_into_vec().unwrap(); /// let cloud_points_out: Vec<PointXYZ> = msg_out.try_into_vec().unwrap();
/// assert_eq!(1.0, cloud_points_out.get(0).unwrap().x); /// assert_eq!(1.0, cloud_points_out.get(0).unwrap().x);
/// ``` /// ```
///
/// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError> pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError>
where where
@ -631,12 +680,12 @@ impl PointCloud2Msg {
let cloud_width = self.dimensions.width as usize; let cloud_width = self.dimensions.width as usize;
let point_step = self.point_step as usize; let point_step = self.point_step as usize;
let mut vec = Vec::with_capacity(cloud_width); let mut vec: Vec<C> = Vec::with_capacity(cloud_width);
if bytematch { if bytematch {
unsafe { unsafe {
std::ptr::copy_nonoverlapping( core::ptr::copy_nonoverlapping(
self.data.as_ptr(), self.data.as_ptr(),
vec.as_mut_ptr() as *mut u8, vec.as_mut_ptr().cast::<u8>(),
self.data.len(), self.data.len(),
); );
vec.set_len(cloud_width); vec.set_len(cloud_width);
@ -644,7 +693,7 @@ impl PointCloud2Msg {
} else { } else {
unsafe { unsafe {
for i in 0..cloud_width { for i in 0..cloud_width {
let point_ptr = self.data.as_ptr().add(i * point_step) as *const C; let point_ptr = self.data.as_ptr().add(i * point_step).cast::<C>();
let point = point_ptr.read(); let point = point_ptr.read();
vec.push(point); vec.push(point);
} }
@ -657,7 +706,7 @@ impl PointCloud2Msg {
} }
} }
/// Convert the PointCloud2Msg to an iterator. /// Convert the [`PointCloud2Msg`] to an iterator.
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -671,6 +720,8 @@ impl PointCloud2Msg {
/// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap(); /// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
/// let cloud_points_out = msg_out.try_into_iter().unwrap().collect::<Vec<PointXYZ>>(); /// let cloud_points_out = msg_out.try_into_iter().unwrap().collect::<Vec<PointXYZ>>();
/// ``` /// ```
/// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
pub fn try_into_iter<const N: usize, C>( pub fn try_into_iter<const N: usize, C>(
self, self,
) -> Result<impl Iterator<Item = C>, MsgConversionError> ) -> Result<impl Iterator<Item = C>, MsgConversionError>
@ -724,7 +775,7 @@ impl<const N: usize> Default for RPCL2Point<N> {
} }
} }
impl<const N: usize> std::ops::Index<usize> for RPCL2Point<N> { impl<const N: usize> core::ops::Index<usize> for RPCL2Point<N> {
type Output = PointData; type Output = PointData;
fn index(&self, index: usize) -> &Self::Output { fn index(&self, index: usize) -> &Self::Output {
@ -931,7 +982,7 @@ impl TryFrom<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
/// ``` /// ```
#[derive(Debug, Clone, Copy)] #[derive(Debug, Clone, Copy)]
pub struct PointData { pub struct PointData {
bytes: [u8; std::mem::size_of::<f64>()], bytes: [u8; core::mem::size_of::<f64>()],
endian: Endian, endian: Endian,
datatype: FieldDatatype, datatype: FieldDatatype,
} }
@ -939,7 +990,7 @@ pub struct PointData {
impl Default for PointData { impl Default for PointData {
fn default() -> Self { fn default() -> Self {
Self { Self {
bytes: [u8::default(); std::mem::size_of::<f64>()], bytes: [u8::default(); core::mem::size_of::<f64>()],
datatype: FieldDatatype::F32, datatype: FieldDatatype::F32,
endian: Endian::default(), endian: Endian::default(),
} }
@ -947,13 +998,13 @@ impl Default for PointData {
} }
impl PointData { impl PointData {
/// Create a new PointData from a value. /// Create a new [`PointData`] from a value.
/// ///
/// # Example /// # Example
/// ``` /// ```
/// let pdata = ros_pointcloud2::PointData::new(1.0); /// let pdata = ros_pointcloud2::PointData::new(1.0);
/// ``` /// ```
#[inline(always)] #[inline]
pub fn new<T: FromBytes>(value: T) -> Self { pub fn new<T: FromBytes>(value: T) -> Self {
Self { Self {
bytes: value.into().raw(), bytes: value.into().raw(),
@ -962,24 +1013,24 @@ impl PointData {
} }
} }
#[inline(always)] #[inline]
fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self { fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self {
debug_assert!(data.len() >= offset + datatype.size()); debug_assert!(data.len() >= offset + datatype.size());
let bytes = [u8::default(); std::mem::size_of::<f64>()]; let bytes = [u8::default(); core::mem::size_of::<f64>()];
unsafe { unsafe {
let data_ptr = data.as_ptr().add(offset); let data_ptr = data.as_ptr().add(offset);
let bytes_ptr = bytes.as_ptr() as *mut u8; let bytes_ptr = bytes.as_ptr() as *mut u8;
std::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size()); core::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size());
} }
Self { Self {
bytes, bytes,
datatype,
endian, endian,
datatype,
} }
} }
/// Get the numeric value from the PointData description. /// Get the numeric value from the [`PointData`] description.
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -987,6 +1038,7 @@ impl PointData {
/// let pdata = ros_pointcloud2::PointData::new(original_data); /// let pdata = ros_pointcloud2::PointData::new(original_data);
/// let my_data: f64 = pdata.get(); /// let my_data: f64 = pdata.get();
/// ``` /// ```
#[must_use]
pub fn get<T: FromBytes>(&self) -> T { pub fn get<T: FromBytes>(&self) -> T {
match self.endian { match self.endian {
Endian::Big => T::from_be_bytes(PointDataBuffer::new(self.bytes)), Endian::Big => T::from_be_bytes(PointDataBuffer::new(self.bytes)),
@ -1062,22 +1114,22 @@ pub enum FieldDatatype {
} }
impl FieldDatatype { impl FieldDatatype {
#[must_use]
pub fn size(&self) -> usize { pub fn size(&self) -> usize {
match self { match self {
FieldDatatype::U8 => std::mem::size_of::<u8>(), FieldDatatype::U8 => core::mem::size_of::<u8>(),
FieldDatatype::U16 => std::mem::size_of::<u16>(), FieldDatatype::U16 => core::mem::size_of::<u16>(),
FieldDatatype::U32 => std::mem::size_of::<u32>(), FieldDatatype::U32 => core::mem::size_of::<u32>(),
FieldDatatype::I8 => std::mem::size_of::<i8>(), FieldDatatype::I8 => core::mem::size_of::<i8>(),
FieldDatatype::I16 => std::mem::size_of::<i16>(), FieldDatatype::I16 => core::mem::size_of::<i16>(),
FieldDatatype::I32 => std::mem::size_of::<i32>(), FieldDatatype::I32 => core::mem::size_of::<i32>(),
FieldDatatype::F32 => std::mem::size_of::<f32>(), FieldDatatype::F32 | FieldDatatype::RGB => core::mem::size_of::<f32>(), // packed in f32
FieldDatatype::F64 => std::mem::size_of::<f64>(), FieldDatatype::F64 => core::mem::size_of::<f64>(),
FieldDatatype::RGB => std::mem::size_of::<f32>(), // packed in f32
} }
} }
} }
impl FromStr for FieldDatatype { impl core::str::FromStr for FieldDatatype {
type Err = MsgConversionError; type Err = MsgConversionError;
fn from_str(s: &str) -> Result<Self, Self::Err> { fn from_str(s: &str) -> Result<Self, Self::Err> {
@ -1091,7 +1143,10 @@ impl FromStr for FieldDatatype {
"i8" => Ok(FieldDatatype::I8), "i8" => Ok(FieldDatatype::I8),
"i16" => Ok(FieldDatatype::I16), "i16" => Ok(FieldDatatype::I16),
"rgb" => Ok(FieldDatatype::RGB), "rgb" => Ok(FieldDatatype::RGB),
#[cfg(feature = "std")]
_ => Err(MsgConversionError::UnsupportedFieldType(s.into())), _ => Err(MsgConversionError::UnsupportedFieldType(s.into())),
#[cfg(not(feature = "std"))]
_ => Err(MsgConversionError::UnsupportedFieldType),
} }
} }
} }
@ -1169,7 +1224,10 @@ impl TryFrom<u8> for FieldDatatype {
6 => Ok(FieldDatatype::U32), 6 => Ok(FieldDatatype::U32),
7 => Ok(FieldDatatype::F32), 7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64), 8 => Ok(FieldDatatype::F64),
#[cfg(feature = "std")]
_ => Err(MsgConversionError::UnsupportedFieldType(value.to_string())), _ => Err(MsgConversionError::UnsupportedFieldType(value.to_string())),
#[cfg(not(feature = "std"))]
_ => Err(MsgConversionError::UnsupportedFieldType),
} }
} }
} }
@ -1183,9 +1241,8 @@ impl From<FieldDatatype> for u8 {
FieldDatatype::U16 => 4, FieldDatatype::U16 => 4,
FieldDatatype::I32 => 5, FieldDatatype::I32 => 5,
FieldDatatype::U32 => 6, FieldDatatype::U32 => 6,
FieldDatatype::F32 => 7, FieldDatatype::F32 | FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer
FieldDatatype::F64 => 8, FieldDatatype::F64 => 8,
FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer
} }
} }
} }
@ -1200,10 +1257,10 @@ impl TryFrom<&ros::PointFieldMsg> for FieldDatatype {
/// Byte buffer alias for endian-aware point data reading and writing. /// 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. /// It uses a fixed size buffer of 8 bytes since the largest supported datatype for [`ros::PointFieldMsg`] is f64.
pub struct PointDataBuffer([u8; 8]); pub struct PointDataBuffer([u8; 8]);
impl std::ops::Index<usize> for PointDataBuffer { impl core::ops::Index<usize> for PointDataBuffer {
type Output = u8; type Output = u8;
fn index(&self, index: usize) -> &Self::Output { fn index(&self, index: usize) -> &Self::Output {
@ -1212,18 +1269,22 @@ impl std::ops::Index<usize> for PointDataBuffer {
} }
impl PointDataBuffer { impl PointDataBuffer {
#[must_use]
pub fn new(data: [u8; 8]) -> Self { pub fn new(data: [u8; 8]) -> Self {
Self(data) Self(data)
} }
#[must_use]
pub fn as_slice(&self) -> &[u8] { pub fn as_slice(&self) -> &[u8] {
&self.0 &self.0
} }
#[must_use]
pub fn raw(self) -> [u8; 8] { pub fn raw(self) -> [u8; 8] {
self.0 self.0
} }
#[must_use]
pub fn from_slice(data: &[u8]) -> Self { pub fn from_slice(data: &[u8]) -> Self {
let mut buffer = [0; 8]; let mut buffer = [0; 8];
data.iter().enumerate().for_each(|(i, &v)| buffer[i] = v); data.iter().enumerate().for_each(|(i, &v)| buffer[i] = v);
@ -1298,7 +1359,7 @@ impl From<points::RGB> for PointDataBuffer {
} }
/// This trait is used to convert a byte slice to a primitive type. /// This trait is used to convert a byte slice to a primitive type.
/// All PointField types are supported. /// All [`PointField`] types are supported.
pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype + Into<PointDataBuffer> { pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype + Into<PointDataBuffer> {
fn from_be_bytes(bytes: PointDataBuffer) -> Self; fn from_be_bytes(bytes: PointDataBuffer) -> Self;
fn from_le_bytes(bytes: PointDataBuffer) -> Self; fn from_le_bytes(bytes: PointDataBuffer) -> Self;
@ -1404,6 +1465,9 @@ mod tests {
use super::Fields; use super::Fields;
use rpcl2_derive::Fields; use rpcl2_derive::Fields;
#[cfg(not(feature = "std"))]
use alloc::string::String;
#[allow(dead_code)] #[allow(dead_code)]
#[derive(Fields)] #[derive(Fields)]
struct TestStruct { struct TestStruct {

33
src/points.rs
View File

@ -4,6 +4,9 @@ use crate::{Fields, PointConvertible, RPCL2Point};
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
use type_layout::TypeLayout; use type_layout::TypeLayout;
#[cfg(all(not(feature = "std"), feature = "derive"))]
use alloc::vec::Vec;
/// A packed RGB color encoding as used in ROS tools. /// A packed RGB color encoding as used in ROS tools.
#[derive(Clone, Copy)] #[derive(Clone, Copy)]
#[repr(C)] #[repr(C)]
@ -28,13 +31,13 @@ impl PartialEq for RGB {
} }
impl core::fmt::Display for RGB { impl core::fmt::Display for RGB {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
write!(f, "#{:02X}{:02X}{:02X}", self.r(), self.g(), self.b()) write!(f, "#{:02X}{:02X}{:02X}", self.r(), self.g(), self.b())
} }
} }
impl core::fmt::Debug for RGB { impl core::fmt::Debug for RGB {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.debug_struct("RGB") f.debug_struct("RGB")
.field("r", &self.r()) .field("r", &self.r())
.field("g", &self.g()) .field("g", &self.g())
@ -44,32 +47,39 @@ impl core::fmt::Debug for RGB {
} }
impl RGB { impl RGB {
#[must_use]
pub fn new(r: u8, g: u8, b: u8) -> Self { pub fn new(r: u8, g: u8, b: u8) -> Self {
Self { Self {
unpacked: [b, g, r, 0], unpacked: [b, g, r, 0],
} }
} }
#[must_use]
pub fn new_from_packed_f32(packed: f32) -> Self { pub fn new_from_packed_f32(packed: f32) -> Self {
Self { packed } Self { packed }
} }
#[must_use]
pub fn new_from_packed(packed: u32) -> Self { pub fn new_from_packed(packed: u32) -> Self {
Self::new_from_packed_f32(f32::from_bits(packed)) Self::new_from_packed_f32(f32::from_bits(packed))
} }
#[must_use]
pub fn raw(&self) -> f32 { pub fn raw(&self) -> f32 {
unsafe { self.packed } unsafe { self.packed }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
unsafe { self.unpacked[2] } unsafe { self.unpacked[2] }
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
unsafe { self.unpacked[1] } unsafe { self.unpacked[1] }
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
unsafe { self.unpacked[0] } unsafe { self.unpacked[0] }
} }
@ -129,6 +139,7 @@ impl From<PointXYZ> for nalgebra::Point3<f32> {
} }
impl PointXYZ { impl PointXYZ {
#[must_use]
pub fn new(x: f32, y: f32, z: f32) -> Self { pub fn new(x: f32, y: f32, z: f32) -> Self {
Self { x, y, z } Self { x, y, z }
} }
@ -289,19 +300,23 @@ pub struct PointXYZRGB {
} }
impl PointXYZRGB { impl PointXYZRGB {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8) -> Self { pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8) -> Self {
let rgb = RGB::new(r, g, b); let rgb = RGB::new(r, g, b);
Self { x, y, z, rgb } Self { x, y, z, rgb }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -361,19 +376,23 @@ pub struct PointXYZRGBA {
} }
impl PointXYZRGBA { impl PointXYZRGBA {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, a: u8) -> Self { pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, a: u8) -> Self {
let rgb = RGB::new(r, g, b); let rgb = RGB::new(r, g, b);
Self { x, y, z, rgb, a } Self { x, y, z, rgb, a }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -436,6 +455,7 @@ pub struct PointXYZRGBNormal {
} }
impl PointXYZRGBNormal { impl PointXYZRGBNormal {
#[must_use]
pub fn new( pub fn new(
x: f32, x: f32,
y: f32, y: f32,
@ -456,14 +476,17 @@ impl PointXYZRGBNormal {
} }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -530,6 +553,7 @@ pub struct PointXYZINormal {
} }
impl PointXYZINormal { impl PointXYZINormal {
#[must_use]
pub fn new( pub fn new(
x: f32, x: f32,
y: f32, y: f32,
@ -613,6 +637,7 @@ unsafe impl Send for PointXYZRGBL {}
unsafe impl Sync for PointXYZRGBL {} unsafe impl Sync for PointXYZRGBL {}
impl PointXYZRGBL { impl PointXYZRGBL {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, label: u32) -> Self { pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, label: u32) -> Self {
let rgb = RGB::new(r, g, b); let rgb = RGB::new(r, g, b);
Self { Self {
@ -624,14 +649,17 @@ impl PointXYZRGBL {
} }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -690,6 +718,7 @@ pub struct PointXYZNormal {
} }
impl PointXYZNormal { impl PointXYZNormal {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, normal_x: f32, normal_y: f32, normal_z: f32) -> Self { pub fn new(x: f32, y: f32, z: f32, normal_x: f32, normal_y: f32, normal_z: f32) -> Self {
Self { Self {
x, x,

3
src/ros.rs
View File

@ -23,6 +23,9 @@
//! } //! }
//! ``` //! ```
#[cfg(not(feature = "std"))]
use alloc::string::String;
/// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages. /// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages.
#[derive(Clone, Debug, Default)] #[derive(Clone, Debug, Default)]
pub struct TimeMsg { pub struct TimeMsg {