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

redo api

This commit is contained in:
stelzo 2024-04-29 17:27:32 +02:00
parent 6f55407a20
commit 77c222414f
No known key found for this signature in database
GPG Key ID: FC4EF89052319374
10 changed files with 989 additions and 538 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Cargo.toml
View File

@ -14,7 +14,6 @@ homepage = "https://github.com/stelzo/ros_pointcloud2"
exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**/build/**", "**/dist/**", "**/docs/**", "**/doc/**"] exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**/build/**", "**/dist/**", "**/docs/**", "**/doc/**"]
[dependencies] [dependencies]
fallible-iterator = "0.3.0"
rosrust_msg = { version = "0.1", optional = true } rosrust_msg = { version = "0.1", optional = true }
rosrust = { version = "0.9.11", optional = true } rosrust = { version = "0.9.11", optional = true }
r2r = { version = "0.8.4", optional = true } r2r = { version = "0.8.4", optional = true }

78
README.md
View File

@ -5,16 +5,14 @@
</p> </p>
</p> </p>
Providing a memory efficient way for message conversion while allowing user defined types without the cost of iterations. Providing a easy to use, generics defined, point-wise iterator abstraction of the byte buffer of `PointCloud2` to minimize iterations in your processing pipeline.
Instead of converting the entire cloud into a `Vec`, you get an iterable type that converts each point from the message on the fly.
To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `ros_types::PointCloud2Msg`. 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`.
## Example
```rust ```rust
use ros_pointcloud2::{ use ros_pointcloud2::{
fallible_iterator::FallibleIterator, pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
pcl_utils::PointXYZ,
ros_types::PointCloud2Msg,
ConvertXYZ,
}; };
// Your points (here using the predefined type PointXYZ). // Your points (here using the predefined type PointXYZ).
@ -31,32 +29,41 @@ let cloud_points = vec![
}, },
]; ];
let cloud_copy = cloud_points.clone(); // For checking equality later. // For equality test later
let cloud_copy = cloud_points.clone();
// Vector -> Converter -> Message // Some changes to demonstrate lazy iterator usage
let internal_msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points) let changed_it = cloud_points.iter().map(|p| {
.unwrap() p.x = 0.5;
.try_into() });
// Vector -> Writer -> Message
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points)
.try_into() // iterating points here O(n)
.unwrap(); .unwrap();
// Convert to your favorite ROS crate message type, we will use rosrust here. // Anything that implements `IntoIterator` also works - like another iterator.
// let msg: rosrust_msg::sensor_msgs::PointCloud2 = internal_msg.into(); let internal_msg_changed: PointCloud2Msg = WriterXYZ::from(changed_it)
.try_into() // iterating points here O(n)
.unwrap();
// Back to this crate's message type. // Convert to your ROS crate message type, we will use r2r here.
// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
// Publish ...
// ... now incoming from a topic.
// let internal_msg: PointCloud2Msg = msg.into(); // let internal_msg: PointCloud2Msg = msg.into();
// Message -> Converter -> Vector // Message -> Reader. The Reader implements the Iterator trait.
let convert: ConvertXYZ = ConvertXYZ::try_from(internal_msg).unwrap(); let reader = ReaderXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = convert let new_cloud_points = reader
.map(|point: PointXYZ| { .map(|point: PointXYZ| {
// Insert your point business logic here // Some logic here
// or use other methods like .for_each().
point
// We are using a fallible iterator so we need to return a Result.
Ok(point)
}) })
.collect::<Vec<PointXYZ>>() .collect::<Vec<PointXYZ>>();
.unwrap(); // Handle point conversion or byte errors here.
assert_eq!(new_cloud_points, cloud_copy); assert_eq!(new_cloud_points, cloud_copy);
``` ```
@ -226,10 +233,25 @@ let to_custom_type = MyConverter::try_from(custom_msg).unwrap();
## Future Work ## Future Work
- Benchmark vs PCL - Benchmark vs PCL
- Proper error passing to the iterator result (currently merged into `PointConversionError`)
- remove allocations
- introduce no-panic for maximum stability
- Add more predefined types - Add more predefined types
- Optional derive macros for custom point implementations
## Changelog 1.0.0
- `Convert` is now `Reader` and `Writer`.
`Convert` had to translate both ways and keep a different state depending on the way it was created. This led to an edge case where the user could create a point cloud that always returns uninitialized points. While the reproduction was unlikely to occur in normal use cases, the possibility of it alone is unnecessary ambiguity.
The new `Reader` and `Writer` not only eliminates ambiguity but also makes the code much more readable and concise.
- Zero-cost iterator abstraction. `Reader` and `Writer` now either take or implement iterable traits. This means, you can read a message, implement a `filter` function and pass the resulting iterator directly to the `Writer`. The entire pipeline is then compiled to a typesafe message-to-message filter with a single iteration.
- Divide error types to message and human errors.
Corrupted messages with valid descriptions and correct byte buffer length can not be classified as corrupted at runtime, thus point conversions can be assumed to always work when passing these checks. There now are less error types and the point conversions within the iteration can omit the `try_*` prefix. Discrepancies within the point cloud message and the described point are checked at `Reader` and `Writer` creation instead.
Human errors are possible with wrong point types or internal crate bugs that could lead to out-of-bound panics. These cases are checked in debug mode via debug_assert now. In release builds, the crate fully leans into performance optimizations and assumes a correct type description via generics (mainly the first 2 parameters coord_type and sizeof(coord_type)). Previously, these errors resulted in an error at runtime but the only possible source for them is the crate user, so the decision was made to promote them to a panic like a compile error. TODO check coord_type and sizeof at compile time statically?
- More Documentation and more relevant examples for every function and type. The example for custom points is moved from the Readme into the documentation as well.
- Performance and efficiency. By leaning into iterators whenever possible, the amount of bound checks have been reduced. There are less iterations in every function and dynamic memory usage is reduced.
- Removed dependencies. By switching to normal iterators and intializing different start values, the crate only depends on the std now.
API changes
- replace every use of `([T; DIM], [ros_pointcloud2::PointMeta; METADIM])` with `ros_pointcloud2::Point<T, DIM, METADIM>`
-
## License ## License
[MIT](https://choosealicense.com/licenses/mit/) [MIT](https://choosealicense.com/licenses/mit/)

131
src/convert.rs
View File

@ -19,6 +19,21 @@ impl Default for FieldDatatype {
} }
} }
impl FieldDatatype {
pub fn size(&self) -> usize {
match self {
FieldDatatype::U8 => 1,
FieldDatatype::U16 => 2,
FieldDatatype::U32 => 4,
FieldDatatype::I8 => 1,
FieldDatatype::I16 => 2,
FieldDatatype::I32 => 4,
FieldDatatype::F32 => 4,
FieldDatatype::F64 => 8,
}
}
}
/// Getter trait for the datatype of a field value. /// Getter trait for the datatype of a field value.
pub trait GetFieldDatatype { pub trait GetFieldDatatype {
fn field_datatype() -> FieldDatatype; fn field_datatype() -> FieldDatatype;
@ -72,30 +87,36 @@ impl GetFieldDatatype for i16 {
} }
} }
pub(crate) fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 { impl TryFrom<u8> for FieldDatatype {
match geo_type { type Error = ConversionError;
FieldDatatype::I8 => 1,
FieldDatatype::U8 => 2, fn try_from(value: u8) -> Result<Self, Self::Error> {
FieldDatatype::I16 => 3, match value {
FieldDatatype::U16 => 4, 1 => Ok(FieldDatatype::I8),
FieldDatatype::I32 => 5, 2 => Ok(FieldDatatype::U8),
FieldDatatype::U32 => 6, 3 => Ok(FieldDatatype::I16),
FieldDatatype::F32 => 7, 4 => Ok(FieldDatatype::U16),
FieldDatatype::F64 => 8, 5 => Ok(FieldDatatype::I32),
6 => Ok(FieldDatatype::U32),
7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64),
_ => Err(ConversionError::UnsupportedFieldType),
}
} }
} }
pub(crate) fn convert_msg_code_to_type(code: u8) -> Result<FieldDatatype, ConversionError> { impl Into<u8> for FieldDatatype {
match code { fn into(self) -> u8 {
7 => Ok(FieldDatatype::F32), match self {
8 => Ok(FieldDatatype::F64), FieldDatatype::I8 => 1,
5 => Ok(FieldDatatype::I32), FieldDatatype::U8 => 2,
2 => Ok(FieldDatatype::U8), FieldDatatype::I16 => 3,
4 => Ok(FieldDatatype::U16), FieldDatatype::U16 => 4,
6 => Ok(FieldDatatype::U32), FieldDatatype::I32 => 5,
1 => Ok(FieldDatatype::I8), FieldDatatype::U32 => 6,
3 => Ok(FieldDatatype::I16), FieldDatatype::F32 => 7,
_ => Err(ConversionError::UnsupportedFieldType), FieldDatatype::F64 => 8,
}
} }
} }
@ -107,7 +128,7 @@ pub(crate) fn check_coord(
match coord { match coord {
Some(y_idx) => { Some(y_idx) => {
let field = &fields[y_idx]; let field = &fields[y_idx];
if field.datatype != convert_to_msg_code(xyz_field_type) { if field.datatype != (*xyz_field_type).into() {
return Err(ConversionError::InvalidFieldFormat); return Err(ConversionError::InvalidFieldFormat);
} }
Ok(field.clone()) Ok(field.clone())
@ -122,19 +143,6 @@ pub trait MetaNames<const METADIM: usize> {
fn meta_names() -> [String; METADIM]; fn meta_names() -> [String; METADIM];
} }
pub(crate) fn datatype_size(datatype: &FieldDatatype) -> usize {
match datatype {
FieldDatatype::U8 => 1,
FieldDatatype::U16 => 2,
FieldDatatype::U32 => 4,
FieldDatatype::I8 => 1,
FieldDatatype::I16 => 2,
FieldDatatype::I32 => 4,
FieldDatatype::F32 => 4,
FieldDatatype::F64 => 8,
}
}
#[inline(always)] #[inline(always)]
pub(crate) fn add_point_to_byte_buffer< pub(crate) fn add_point_to_byte_buffer<
T: FromBytes, T: FromBytes,
@ -146,19 +154,17 @@ pub(crate) fn add_point_to_byte_buffer<
coords: C, coords: C,
cloud: &mut PointCloud2Msg, cloud: &mut PointCloud2Msg,
) -> Result<bool, ConversionError> { ) -> Result<bool, ConversionError> {
let (coords_data, coords_meta): ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() { let point: Point<T, DIM, METADIM> = coords.into();
Ok(meta) => meta,
Err(_) => return Err(ConversionError::PointConversionError),
};
// (x, y, z...) // (x, y, z...)
coords_data point
.coords
.iter() .iter()
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
// meta data description // meta data description
coords_meta.iter().for_each(|meta| { point.meta.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)]; let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
cloud.data.extend_from_slice(truncated_bytes); cloud.data.extend_from_slice(truncated_bytes);
}); });
@ -300,35 +306,28 @@ pub(crate) fn load_loadable<T: FromBytes, const SIZE: usize>(
start_idx: usize, start_idx: usize,
data: &[u8], data: &[u8],
endian: &Endianness, endian: &Endianness,
) -> Option<T> { ) -> T {
match endian { match endian {
Endianness::Big => Some(T::from_be_bytes( Endianness::Big => T::from_be_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
load_bytes::<SIZE>(start_idx, data)?.as_slice(), Endianness::Little => T::from_le_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
)),
Endianness::Little => Some(T::from_le_bytes(
load_bytes::<SIZE>(start_idx, data)?.as_slice(),
)),
} }
} }
fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> Option<[u8; S]> { /// Note: check if the data slice is long enough to load the bytes beforehand!
if start_idx + S > data.len() { fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> [u8; S] {
return None; let mut target = [u8::default(); S];
}
let mut buff: [u8; S] = [0; S];
for (byte, buff_val) in buff.iter_mut().enumerate().take(S) {
let raw_byte = data.get(start_idx + byte);
match raw_byte {
None => {
return None;
}
Some(some_byte) => {
*buff_val = *some_byte;
}
}
}
Some(buff) debug_assert!(target.len() == S);
debug_assert!(data.len() >= start_idx + S);
let source = unsafe { data.get_unchecked(start_idx..start_idx + S) };
target
.iter_mut()
.zip(source.iter())
.for_each(|(target, source)| {
*target = *source;
});
target
} }
#[cfg(test)] #[cfg(test)]

147
src/lib.rs
View File

@ -1,13 +1,18 @@
// TODO test reading writing different types //#![doc = include_str!("../README.md")]
// TODO read less info from the point cloud than is available
// TODO write less info to the point cloud than is available
// TODO explain templating arguments more for reader and writer
#![doc = include_str!("../README.md")]
pub mod convert; pub mod convert;
pub mod pcl_utils; pub mod pcl_utils;
pub mod ros_types; pub mod ros_types;
/// Macro to get the size of a type at compile time. This is a convenience macro to avoid writing out the full std::mem::size_of::<T>().
/// Use it for your custom Reader and Writer implementations.
/// # Example
/// ```
/// use ros_pointcloud2::{
/// writer::Writer, size_of, pcl_utils::PointXYZ,
/// };
///
/// type MyWriterXYZ = Writer<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
/// ```
#[macro_export] #[macro_export]
macro_rules! size_of { macro_rules! size_of {
($t:ty) => { ($t:ty) => {
@ -24,37 +29,43 @@ use crate::convert::*;
use crate::pcl_utils::*; use crate::pcl_utils::*;
use crate::ros_types::PointFieldMsg; use crate::ros_types::PointFieldMsg;
pub use fallible_iterator::FallibleIterator; /// All errors that can occur for creating Reader and Writer.
/// Errors that can occur when converting between PointCloud2 and a custom type.
#[derive(Debug)] #[derive(Debug)]
pub enum ConversionError { pub enum ConversionError {
/// The coordinate field does not match the expected datatype.
InvalidFieldFormat, InvalidFieldFormat,
/// Not enough meta or dimensional fields in the PointCloud2 message.
NotEnoughFields, NotEnoughFields,
/// The dimensionality of the point inside the message is too high.
TooManyDimensions, TooManyDimensions,
/// The field type is not supported by the ROS message description.
UnsupportedFieldType, UnsupportedFieldType,
/// There are no points in the point cloud.
NoPoints, NoPoints,
/// The length of the byte buffer in the message does not match the expected length computed from the fields.
DataLengthMismatch, DataLengthMismatch,
MetaIndexLengthMismatch, }
EndOfBuffer,
PointConversionError, pub struct Point<T, const DIM: usize, const METADIM: usize> {
MetaDatatypeMismatch, pub coords: [T; DIM],
pub meta: [PointMeta; METADIM],
} }
/// Trait to convert a point to a tuple of coordinates and meta data. /// Trait to convert a point to a tuple of coordinates and meta data.
/// Implement this trait for your point type to use the conversion. /// Implement this trait for your point type to use the conversion.
pub trait PointConvertible<T, const SIZE: usize, const DIM: usize, const METADIM: usize>: pub trait PointConvertible<T, const SIZE: usize, const DIM: usize, const METADIM: usize>:
TryInto<([T; DIM], [PointMeta; METADIM])> From<Point<T, DIM, METADIM>> + Into<Point<T, DIM, METADIM>> + MetaNames<METADIM> + Clone + 'static
+ TryFrom<([T; DIM], [PointMeta; METADIM])>
+ MetaNames<METADIM>
+ Clone
+ 'static
where where
T: FromBytes, T: FromBytes,
{ {
} }
/// Meta data representation for a point /// Metadata representation for a point.
/// ///
/// This struct is used to store meta data in a fixed size byte buffer along the with the /// This struct is used to store meta data in a fixed size byte buffer along the with the
/// datatype that is encoded so that it can be decoded later. /// datatype that is encoded so that it can be decoded later.
@ -65,7 +76,7 @@ where
/// ///
/// let original_data: f64 = 1.0; /// let original_data: f64 = 1.0;
/// let meta = PointMeta::new(original_data); /// let meta = PointMeta::new(original_data);
/// let my_data: f64 = meta.get().unwrap(); /// let my_data: f64 = meta.get();
/// ``` /// ```
#[derive(Debug, Clone, Copy)] #[derive(Debug, Clone, Copy)]
pub struct PointMeta { pub struct PointMeta {
@ -92,51 +103,93 @@ impl PointMeta {
pub fn new<T: FromBytes>(value: T) -> Self { pub fn new<T: FromBytes>(value: T) -> Self {
let raw_bytes = T::bytes(&value); let raw_bytes = T::bytes(&value);
let mut bytes = [0; std::mem::size_of::<f64>()]; let mut bytes = [0; std::mem::size_of::<f64>()];
for (idx, byte) in raw_bytes.iter().enumerate() { for (byte, save_byte) in raw_bytes.into_iter().zip(bytes.iter_mut()) {
bytes[idx] = *byte; *save_byte = byte;
} }
Self { Self {
bytes, bytes,
datatype: T::field_datatype(), datatype: T::field_datatype(),
} }
} }
fn new_from_buffer( fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self {
data: &[u8], debug_assert!(data.len() >= offset + datatype.size());
offset: usize,
datatype: &FieldDatatype, let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) };
) -> Result<Self, ConversionError> {
let size = datatype_size(datatype);
let bytes = data
.get(offset..offset + size)
.ok_or(ConversionError::DataLengthMismatch)?;
let mut bytes_array = [0; std::mem::size_of::<f64>()]; // 8 bytes as f64 is the largest type let mut bytes_array = [0; std::mem::size_of::<f64>()]; // 8 bytes as f64 is the largest type
for (idx, byte) in bytes.iter().enumerate() { for (byte, save_byte) in bytes.into_iter().zip(bytes_array.iter_mut()) {
bytes_array[idx] = *byte; *save_byte = *byte;
} }
Ok(Self {
Self {
bytes: bytes_array, bytes: bytes_array,
datatype: *datatype, datatype: *datatype,
}) }
} }
/// Get the value from the PointMeta. It will return None if the datatype does not match. /// Get the numeric value from the PointMeta description.
/// ///
/// # Example /// # Example
/// ``` /// ```
/// let original_data: f64 = 1.0; /// let original_data: f64 = 1.0;
/// let meta = ros_pointcloud2::PointMeta::new(original_data); /// let meta = ros_pointcloud2::PointMeta::new(original_data);
/// let my_data: f64 = meta.get().unwrap(); /// let my_data: f64 = meta.get();
/// ``` /// ```
pub fn get<T: FromBytes>(&self) -> Result<T, ConversionError> { pub fn get<T: FromBytes>(&self) -> T {
if self.datatype != T::field_datatype() { let size = T::field_datatype().size();
return Err(ConversionError::MetaDatatypeMismatch); let bytes = self
} .bytes
let size = datatype_size(&T::field_datatype()); .get(0..size)
if let Some(bytes) = self.bytes.get(0..size) { .expect("Exceeds bounds of f64, which is the largest type");
Ok(T::from_le_bytes(bytes)) T::from_le_bytes(bytes)
} else { }
Err(ConversionError::DataLengthMismatch) // self.bytes are not long enough, already handled while parsing }
}
impl From<f32> for PointMeta {
fn from(value: f32) -> Self {
Self::new(value)
}
}
impl From<f64> for PointMeta {
fn from(value: f64) -> Self {
Self::new(value)
}
}
impl From<i32> for PointMeta {
fn from(value: i32) -> Self {
Self::new(value)
}
}
impl From<u8> for PointMeta {
fn from(value: u8) -> Self {
Self::new(value)
}
}
impl From<u16> for PointMeta {
fn from(value: u16) -> Self {
Self::new(value)
}
}
impl From<u32> for PointMeta {
fn from(value: u32) -> Self {
Self::new(value)
}
}
impl From<i8> for PointMeta {
fn from(value: i8) -> Self {
Self::new(value)
}
}
impl From<i16> for PointMeta {
fn from(value: i16) -> Self {
Self::new(value)
} }
} }

358
src/pcl_utils.rs
View File

@ -1,11 +1,13 @@
use crate::{ConversionError, MetaNames, PointConvertible, PointMeta}; use crate::{MetaNames, Point, PointConvertible};
/// Pack an RGB color into a single f32 value as used in ROS with PCL for RViz usage.
#[inline] #[inline]
fn pack_rgb(r: u8, g: u8, b: u8) -> f32 { fn pack_rgb(r: u8, g: u8, b: u8) -> f32 {
let packed = ((r as u32) << 16) + ((g as u32) << 8) + (b as u32); let packed = ((r as u32) << 16) + ((g as u32) << 8) + (b as u32);
f32::from_bits(packed) f32::from_bits(packed)
} }
/// Unpack an RGB color from a single f32 value as used in ROS with PCL for RViz usage.
#[inline] #[inline]
fn unpack_rgb(rgb: f32) -> [u8; 3] { fn unpack_rgb(rgb: f32) -> [u8; 3] {
let packed: u32 = rgb.to_bits(); let packed: u32 = rgb.to_bits();
@ -24,23 +26,22 @@ pub struct PointXYZ {
pub z: f32, pub z: f32,
} }
impl TryInto<([f32; 3], [PointMeta; 0])> for PointXYZ { impl From<Point<f32, 3, 0>> for PointXYZ {
type Error = ConversionError; fn from(point: Point<f32, 3, 0>) -> Self {
Self {
fn try_into(self) -> Result<([f32; 3], [PointMeta; 0]), Self::Error> { x: point.coords[0],
Ok(([self.x, self.y, self.z], [])) y: point.coords[1],
z: point.coords[2],
}
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 0])> for PointXYZ { impl Into<Point<f32, 3, 0>> for PointXYZ {
type Error = ConversionError; fn into(self) -> Point<f32, 3, 0> {
Point {
fn try_from(data: ([f32; 3], [PointMeta; 0])) -> Result<Self, Self::Error> { coords: [self.x, self.y, self.z],
Ok(Self { meta: Default::default(),
x: data.0[0], }
y: data.0[1],
z: data.0[2],
})
} }
} }
@ -62,24 +63,23 @@ pub struct PointXYZI {
pub intensity: f32, pub intensity: f32,
} }
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZI { impl From<Point<f32, 3, 1>> for PointXYZI {
type Error = ConversionError; fn from(point: Point<f32, 3, 1>) -> Self {
Self {
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { x: point.coords[0],
Ok(([self.x, self.y, self.z], [PointMeta::new(self.intensity)])) y: point.coords[1],
z: point.coords[2],
intensity: point.meta[0].get(),
}
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZI { impl Into<Point<f32, 3, 1>> for PointXYZI {
type Error = ConversionError; fn into(self) -> Point<f32, 3, 1> {
Point {
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> { coords: [self.x, self.y, self.z],
Ok(Self { meta: [self.intensity.into()],
x: data.0[0], }
y: data.0[1],
z: data.0[2],
intensity: data.1[0].get()?,
})
} }
} }
@ -103,29 +103,28 @@ pub struct PointXYZRGB {
pub b: u8, pub b: u8,
} }
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB { impl From<Point<f32, 3, 1>> for PointXYZRGB {
type Error = ConversionError; fn from(point: Point<f32, 3, 1>) -> Self {
let rgb = point.meta[0].get::<f32>();
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok(([self.x, self.y, self.z], [PointMeta::new(rgb)]))
}
}
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
let rgb_unpacked = unpack_rgb(rgb); let rgb_unpacked = unpack_rgb(rgb);
Ok(Self { Self {
x: data.0[0], x: point.coords[0],
y: data.0[1], y: point.coords[1],
z: data.0[2], z: point.coords[2],
r: rgb_unpacked[0], r: rgb_unpacked[0],
g: rgb_unpacked[1], g: rgb_unpacked[1],
b: rgb_unpacked[2], b: rgb_unpacked[2],
}) }
}
}
impl Into<Point<f32, 3, 1>> for PointXYZRGB {
fn into(self) -> Point<f32, 3, 1> {
let rgb = pack_rgb(self.r, self.g, self.b);
Point {
coords: [self.x, self.y, self.z],
meta: [rgb.into()],
}
} }
} }
@ -151,33 +150,29 @@ pub struct PointXYZRGBA {
pub a: u8, pub a: u8,
} }
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA { impl From<Point<f32, 3, 2>> for PointXYZRGBA {
type Error = ConversionError; fn from(point: Point<f32, 3, 2>) -> Self {
let rgb = point.meta[0].get::<f32>();
fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok((
[self.x, self.y, self.z],
[PointMeta::new(rgb), PointMeta::new(self.a)],
))
}
}
impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
let rgb_unpacked = unpack_rgb(rgb); let rgb_unpacked = unpack_rgb(rgb);
Ok(Self { Self {
x: data.0[0], x: point.coords[0],
y: data.0[1], y: point.coords[1],
z: data.0[2], z: point.coords[2],
r: rgb_unpacked[0], r: rgb_unpacked[0],
g: rgb_unpacked[1], g: rgb_unpacked[1],
b: rgb_unpacked[2], b: rgb_unpacked[2],
a: data.1[1].get()?, a: point.meta[1].get(),
}) }
}
}
impl Into<Point<f32, 3, 2>> for PointXYZRGBA {
fn into(self) -> Point<f32, 3, 2> {
let rgb = pack_rgb(self.r, self.g, self.b);
Point {
coords: [self.x, self.y, self.z],
meta: [rgb.into(), self.a.into()],
}
} }
} }
@ -204,40 +199,36 @@ pub struct PointXYZRGBNormal {
pub normal_z: f32, pub normal_z: f32,
} }
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal { impl From<Point<f32, 3, 4>> for PointXYZRGBNormal {
type Error = ConversionError; fn from(point: Point<f32, 3, 4>) -> Self {
let rgb = point.meta[0].get::<f32>();
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok((
[self.x, self.y, self.z],
[
PointMeta::new(rgb),
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
],
))
}
}
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
let rgb_unpacked = unpack_rgb(rgb); let rgb_unpacked = unpack_rgb(rgb);
Ok(Self { Self {
x: data.0[0], x: point.coords[0],
y: data.0[1], y: point.coords[1],
z: data.0[2], z: point.coords[2],
r: rgb_unpacked[0], r: rgb_unpacked[0],
g: rgb_unpacked[1], g: rgb_unpacked[1],
b: rgb_unpacked[2], b: rgb_unpacked[2],
normal_x: data.1[1].get()?, normal_x: point.meta[1].get(),
normal_y: data.1[2].get()?, normal_y: point.meta[2].get(),
normal_z: data.1[3].get()?, normal_z: point.meta[3].get(),
}) }
}
}
impl Into<Point<f32, 3, 4>> for PointXYZRGBNormal {
fn into(self) -> Point<f32, 3, 4> {
let rgb = pack_rgb(self.r, self.g, self.b);
Point {
coords: [self.x, self.y, self.z],
meta: [
rgb.into(),
self.normal_x.into(),
self.normal_y.into(),
self.normal_z.into(),
],
}
} }
} }
@ -262,35 +253,31 @@ pub struct PointXYZINormal {
pub normal_z: f32, pub normal_z: f32,
} }
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZINormal { impl From<Point<f32, 3, 4>> for PointXYZINormal {
type Error = ConversionError; fn from(point: Point<f32, 3, 4>) -> Self {
Self {
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { x: point.coords[0],
Ok(( y: point.coords[1],
[self.x, self.y, self.z], z: point.coords[2],
[ intensity: point.meta[0].get(),
PointMeta::new(self.intensity), normal_x: point.meta[1].get(),
PointMeta::new(self.normal_x), normal_y: point.meta[2].get(),
PointMeta::new(self.normal_y), normal_z: point.meta[3].get(),
PointMeta::new(self.normal_z), }
],
))
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal { impl Into<Point<f32, 3, 4>> for PointXYZINormal {
type Error = ConversionError; fn into(self) -> Point<f32, 3, 4> {
Point {
fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result<Self, Self::Error> { coords: [self.x, self.y, self.z],
Ok(Self { meta: [
x: data.0[0], self.intensity.into(),
y: data.0[1], self.normal_x.into(),
z: data.0[2], self.normal_y.into(),
intensity: data.1[0].get()?, self.normal_z.into(),
normal_x: data.1[1].get()?, ],
normal_y: data.1[2].get()?, }
normal_z: data.1[3].get()?,
})
} }
} }
@ -312,24 +299,23 @@ pub struct PointXYZL {
pub label: u32, pub label: u32,
} }
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZL { impl From<Point<f32, 3, 1>> for PointXYZL {
type Error = ConversionError; fn from(point: Point<f32, 3, 1>) -> Self {
Self {
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { x: point.coords[0],
Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)])) y: point.coords[1],
z: point.coords[2],
label: point.meta[0].get(),
}
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL { impl Into<Point<f32, 3, 1>> for PointXYZL {
type Error = ConversionError; fn into(self) -> Point<f32, 3, 1> {
Point {
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> { coords: [self.x, self.y, self.z],
Ok(Self { meta: [self.label.into()],
x: data.0[0], }
y: data.0[1],
z: data.0[2],
label: data.1[0].get()?,
})
} }
} }
@ -354,33 +340,29 @@ pub struct PointXYZRGBL {
pub label: u32, pub label: u32,
} }
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL { impl From<Point<f32, 3, 2>> for PointXYZRGBL {
type Error = ConversionError; fn from(point: Point<f32, 3, 2>) -> Self {
let rgb = point.meta[0].get::<f32>();
fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok((
[self.x, self.y, self.z],
[PointMeta::new(rgb), PointMeta::new(self.label)],
))
}
}
impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
let rgb_unpacked = unpack_rgb(rgb); let rgb_unpacked = unpack_rgb(rgb);
Ok(Self { Self {
x: data.0[0], x: point.coords[0],
y: data.0[1], y: point.coords[1],
z: data.0[2], z: point.coords[2],
r: rgb_unpacked[0], r: rgb_unpacked[0],
g: rgb_unpacked[1], g: rgb_unpacked[1],
b: rgb_unpacked[2], b: rgb_unpacked[2],
label: data.1[1].get()?, label: point.meta[1].get(),
}) }
}
}
impl Into<Point<f32, 3, 2>> for PointXYZRGBL {
fn into(self) -> Point<f32, 3, 2> {
let rgb = pack_rgb(self.r, self.g, self.b);
Point {
coords: [self.x, self.y, self.z],
meta: [rgb.into(), self.label.into()],
}
} }
} }
@ -404,33 +386,29 @@ pub struct PointXYZNormal {
pub normal_z: f32, pub normal_z: f32,
} }
impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZNormal { impl From<Point<f32, 3, 3>> for PointXYZNormal {
type Error = ConversionError; fn from(point: Point<f32, 3, 3>) -> Self {
Self {
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> { x: point.coords[0],
Ok(( y: point.coords[1],
[self.x, self.y, self.z], z: point.coords[2],
[ normal_x: point.meta[0].get(),
PointMeta::new(self.normal_x), normal_y: point.meta[1].get(),
PointMeta::new(self.normal_y), normal_z: point.meta[2].get(),
PointMeta::new(self.normal_z), }
],
))
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZNormal { impl Into<Point<f32, 3, 3>> for PointXYZNormal {
type Error = ConversionError; fn into(self) -> Point<f32, 3, 3> {
Point {
fn try_from(data: ([f32; 3], [PointMeta; 3])) -> Result<Self, Self::Error> { coords: [self.x, self.y, self.z],
Ok(Self { meta: [
x: data.0[0], self.normal_x.into(),
y: data.0[1], self.normal_y.into(),
z: data.0[2], self.normal_z.into(),
normal_x: data.1[0].get()?, ],
normal_y: data.1[1].get()?, }
normal_z: data.1[2].get()?,
})
} }
} }

256
src/reader.rs
View File

@ -1,50 +1,85 @@
use crate::*; use crate::*;
use convert::*;
/// 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<const DIM: usize, const METADIM: usize, C> =
Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderF64<const DIM: usize, const METADIM: usize, C> =
Reader<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderI8<const DIM: usize, const METADIM: usize, C> =
Reader<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderI16<const DIM: usize, const METADIM: usize, C> =
Reader<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderI32<const DIM: usize, const METADIM: usize, C> =
Reader<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderU8<const DIM: usize, const METADIM: usize, C> =
Reader<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderU16<const DIM: usize, const METADIM: usize, C> =
Reader<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderU32<const DIM: usize, const METADIM: usize, C> =
Reader<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
/// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`). /// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`).
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZ = Reader<f32, { size_of!(f32) }, 3, 0, PointXYZ>; pub type ReaderXYZ = ReaderF32<3, 0, PointXYZ>;
/// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`). /// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`).
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZI = Reader<f32, { size_of!(f32) }, 3, 1, PointXYZI>; pub type ReaderXYZI = ReaderF32<3, 1, PointXYZI>;
/// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`). /// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`).
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZNormal = Reader<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>; pub type ReaderXYZNormal = ReaderF32<3, 3, PointXYZNormal>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb (see `PointXYZRGB`). /// Provides the message as an Iterator over xyz coordinates and unpacked rgb (see `PointXYZRGB`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. /// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZRGB = Reader<f32, { size_of!(f32) }, 3, 1, PointXYZRGB>; pub type ReaderXYZRGB = ReaderF32<3, 1, PointXYZRGB>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb and intensity (see `PointXYZRGBL`). /// Provides the message as an Iterator over xyz coordinates and unpacked rgb and intensity (see `PointXYZRGBL`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. /// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZRGBL = Reader<f32, { size_of!(f32) }, 3, 2, PointXYZRGBL>; pub type ReaderXYZRGBL = ReaderF32<3, 2, PointXYZRGBL>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb with additional alpha channel (see `PointXYZRGBA`). /// Provides the message as an Iterator over xyz coordinates and unpacked rgb with additional alpha channel (see `PointXYZRGBA`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. /// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZRGBA = Reader<f32, { size_of!(f32) }, 3, 2, PointXYZRGBA>; pub type ReaderXYZRGBA = ReaderF32<3, 2, PointXYZRGBA>;
/// Provides the message as an Iterator over xyz coordinates and normal and unpacked rgb (see `PointXYZRGBNormal`). /// Provides the message as an Iterator over xyz coordinates and normal and unpacked rgb (see `PointXYZRGBNormal`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard. /// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZRGBNormal = Reader<f32, { size_of!(f32) }, 3, 4, PointXYZRGBNormal>; pub type ReaderXYZRGBNormal = ReaderF32<3, 4, PointXYZRGBNormal>;
/// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`). /// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
pub type ReaderXYZINormal = Reader<f32, { size_of!(f32) }, 3, 4, PointXYZINormal>; pub type ReaderXYZINormal = ReaderF32<3, 4, PointXYZINormal>;
/// Provides the message as an Iterator over xyz coordinates and intensity with additional label (see `PointXYZL`). /// Provides the message as an Iterator over xyz coordinates and intensity with additional label (see `PointXYZL`).
/// Every additional meta data is ignored. /// Every additional meta data is ignored.
/// The label is typically used for semantic segmentation. /// The label is typically used for semantic segmentation.
pub type ReaderXYZL = Reader<f32, { size_of!(f32) }, 3, 1, PointXYZL>; pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>;
/// The Reader provides an abstraction around PointCloud2Msg by implementing a fallible iterator around the data buffer. /// The Reader 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.
/// At runtime, the iterator catches read errors from the Msg type which typically only occur with corrupted data. ///
/// # Predefined Readers
/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
/// ///
/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first. /// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data. /// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
@ -59,29 +94,84 @@ pub type ReaderXYZL = Reader<f32, { size_of!(f32) }, 3, 1, PointXYZL>;
/// ///
/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags. /// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
/// ///
/// # Example /// ## Example
/// ``` /// ```
/// use ros_pointcloud2::{ /// use ros_pointcloud2::{
/// FallibleIterator, // needed for for_each and the other iterator methods /// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
/// reader::ReaderXYZ,
/// writer::WriterXYZ,
/// PointCloud2Msg,
/// pcl_utils::PointXYZ,
/// }; /// };
/// ///
///
/// let cloud_points: Vec<PointXYZ> = vec![ /// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ]; /// ];
/// ///
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap(); /// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// let convert = ReaderXYZ::try_from(msg).unwrap(); // parse message /// let convert = ReaderXYZ::try_from(msg).unwrap();
/// // ^^^^^^^^^ conversion from PointCloud2Msg to Reader that implements Iterator
/// ///
/// convert.for_each(|point: PointXYZ| { /// convert.for_each(|point: PointXYZ| {
/// // do something with the point /// // do something with the point
/// Ok(()) // return Ok to continue iteration /// });
/// }) /// ```
/// .unwrap(); // handle point conversion errors /// # Fully Custom Reader
/// When the predefined types are not enough (like sensor specific metadata), you can describe your Reader with the following template arguments:
/// - T: The coordinate type, e.g. f32
/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
/// Afterwards, implement the PointConvertible trait for your custom point type.
///
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta,
/// };
///
/// type Xyz = f32; // coordinate type
/// const XYZ_S: usize = size_of!(Xyz);
/// const DIM: usize = 3; // helper constant for dimensionality
/// const METADIM: usize = 1; // helper constant for meta data fields
///
/// #[derive(Debug, PartialEq, Clone)]
/// struct CustomPoint {
/// x: f32,
/// y: f32,
/// z: f32,
/// i: u8,
/// }
///
/// impl From<Point<f32, 3, 1>> for CustomPoint {
/// fn from(point: Point<f32, 3, 1>) -> Self {
/// Self {
/// x: point.coords[0],
/// y: point.coords[1],
/// z: point.coords[2],
/// i: point.meta[0].get(),
/// }
/// }
/// }
///
///impl Into<Point<f32, 3, 1>> for CustomPoint {
/// fn into(self) -> Point<f32, 3, 1> {
/// Point {
/// coords: [self.x, self.y, self.z],
/// meta: [self.i.into()],
/// }
/// }
///}
///
/// impl MetaNames<METADIM> for CustomPoint {
/// fn meta_names() -> [String; METADIM] {
/// [format!("intensity")]
/// }
/// }
///
/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
///
/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// // ^^^^^^^^ your custom Reader
/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// ``` /// ```
pub struct Reader<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C> pub struct Reader<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
where where
@ -103,14 +193,13 @@ where
/// The iterator is fallible because the data is read from a byte buffer inside the PointCloud2 message, which is inherently unsafe. /// The iterator is fallible because the data is read from a byte buffer inside the PointCloud2 message, which is inherently unsafe.
/// ///
/// See ConversionError for possible errors that can occur during iteration. /// See ConversionError for possible errors that can occur during iteration.
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> FallibleIterator impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> Iterator
for Reader<T, SIZE, DIM, METADIM, C> for Reader<T, SIZE, DIM, METADIM, C>
where where
T: FromBytes, T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>, C: PointConvertible<T, SIZE, DIM, METADIM>,
{ {
type Item = C; type Item = C;
type Error = ConversionError;
/// The size_hint is the length of the remaining elements and the maximum length of the iterator. /// The size_hint is the length of the remaining elements and the maximum length of the iterator.
/// ///
@ -124,41 +213,40 @@ where
/// It must keep track of the current iteration and the length of the cloud so it has to mutate self. /// It must keep track of the current iteration and the length of the cloud so it has to mutate self.
/// ///
/// The current point is then converted into the custom type. If the conversion fails, an error is returned. /// The current point is then converted into the custom type. If the conversion fails, an error is returned.
fn next(&mut self) -> Result<Option<Self::Item>, Self::Error> { fn next(&mut self) -> Option<Self::Item> {
if self.iteration >= self.cloud_length { if self.iteration >= self.cloud_length {
return Ok(None); // iteration finished return None; // iteration finished
} }
let mut xyz: [T; DIM] = [T::default(); DIM]; let mut xyz: [T; DIM] = [T::default(); DIM];
let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM]; xyz.iter_mut()
for (idx, in_point_offset) in self.offsets.iter().enumerate() { .zip(self.offsets.iter())
if idx < DIM { .for_each(|(p_xyz, in_point_offset)| {
match load_loadable::<T, SIZE>( *p_xyz = load_loadable::<T, SIZE>(
(self.iteration * self.point_step_size) + in_point_offset, (self.iteration * self.point_step_size) + in_point_offset,
&self.data, &self.data,
&self.endianness, &self.endianness,
) { );
Some(c) => xyz[idx] = c, });
None => return Err(ConversionError::EndOfBuffer),
} debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
} else { debug_assert!(
let meta_idx = idx - DIM; self.offsets.len() == DIM + METADIM,
let meta_type = self.meta[meta_idx].1; "Offset length mismatch"
);
let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM];
meta.iter_mut()
.zip(self.offsets.iter().skip(DIM))
.zip(self.meta.iter())
.for_each(|((p_meta, in_point_offset), (_, meta_type))| {
let start = (self.iteration * self.point_step_size) + in_point_offset; let start = (self.iteration * self.point_step_size) + in_point_offset;
if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) { *p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
meta[meta_idx] = m; });
} else {
return Err(ConversionError::MetaIndexLengthMismatch);
}
}
}
self.iteration += 1; self.iteration += 1;
match C::try_from((xyz, meta)) { Some(C::from(Point { coords: xyz, meta }))
Err(_) => Err(ConversionError::PointConversionError),
Ok(tuple) => Ok(Some(tuple)),
}
} }
} }
@ -179,11 +267,7 @@ where
/// # Example /// # Example
/// ``` /// ```
/// use ros_pointcloud2::{ /// use ros_pointcloud2::{
/// FallibleIterator, // needed for for_each and the other iterator methods /// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
/// reader::ReaderXYZ,
/// writer::WriterXYZ,
/// PointCloud2Msg,
/// pcl_utils::PointXYZ,
/// }; /// };
/// ///
/// let cloud_points: Vec<PointXYZ> = vec![ /// let cloud_points: Vec<PointXYZ> = vec![
@ -222,33 +306,36 @@ where
"z" => has_z = Some(idx), "z" => has_z = Some(idx),
_ => { _ => {
if lower_meta_names.contains(&lower_field_name) { if lower_meta_names.contains(&lower_field_name) {
println!("{} {} {}", idx, field.name, meta_with_offsets.len()); let meta_idx = idx - DIM;
match meta_with_offsets.get_mut(idx - DIM) { debug_assert!(
None => return Err(ConversionError::MetaIndexLengthMismatch), meta_idx < meta_with_offsets.len(),
Some(data) => { "Meta data length mismatch"
data.0 = field.name.clone(); );
data.1 = convert_msg_code_to_type(field.datatype)?; meta_with_offsets[meta_idx].0 = field.name.clone();
data.2 = field.offset as usize; meta_with_offsets[meta_idx].1 = field.datatype.try_into()?;
} meta_with_offsets[meta_idx].2 = field.offset as usize;
}
} }
} }
} }
} }
meta_with_offsets.sort_unstable_by(|a, b| a.2.cmp(&b.2)); meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
debug_assert!(
meta_with_offsets.len() == METADIM,
"Meta data length mismatch"
);
let mut meta_offsets = [usize::default(); METADIM]; let mut meta_offsets = [usize::default(); METADIM];
let mut meta: Vec<(String, FieldDatatype)> = let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM];
vec![(String::default(), FieldDatatype::U8); METADIM];
meta_with_offsets meta_with_offsets
.into_iter() .into_iter()
.enumerate() .zip(meta.iter_mut())
.for_each(|(idx, (name, datatype, offset))| { .zip(meta_offsets.iter_mut())
unsafe { .for_each(|(((name, datatype, offset), meta), meta_offset)| {
// all arrays have size METADIM *meta = (name, datatype);
*meta.get_unchecked_mut(idx) = (name, datatype); *meta_offset = offset;
*meta_offsets.get_unchecked_mut(idx) = offset;
};
}); });
let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?; let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
@ -288,26 +375,31 @@ where
} }
let point_step_size = cloud.point_step as usize; let point_step_size = cloud.point_step as usize;
if point_step_size * cloud.width as usize * cloud.height as usize != cloud.data.len() { let cloud_length = cloud.width as usize * cloud.height as usize;
if point_step_size * cloud_length != cloud.data.len() {
return Err(ConversionError::DataLengthMismatch); return Err(ConversionError::DataLengthMismatch);
} }
if let Some(max_offset) = offsets.last() { let last_offset = offsets.last().expect("Dimensionality is 0.");
if let Some(last_meta) = meta.last() {
let size_with_last_meta = max_offset + datatype_size(&last_meta.1); if let Some(last_meta) = meta.last() {
if size_with_last_meta > point_step_size { let size_with_last_meta = last_offset + last_meta.1.size();
return Err(ConversionError::DataLengthMismatch); if size_with_last_meta > point_step_size {
} return Err(ConversionError::DataLengthMismatch);
}
} else {
if last_offset + xyz_field_type.size() > point_step_size {
return Err(ConversionError::DataLengthMismatch);
} }
} }
Ok(Self { Ok(Self {
iteration: 0, iteration: 0,
data: cloud.data, data: cloud.data,
point_step_size: point_step_size, point_step_size,
cloud_length: cloud.width as usize * cloud.height as usize, cloud_length: cloud.width as usize * cloud.height as usize,
offsets: offsets, offsets,
meta: meta, meta,
endianness: endian, endianness: endian,
phantom_t: std::marker::PhantomData, phantom_t: std::marker::PhantomData,
phantom_c: std::marker::PhantomData, phantom_c: std::marker::PhantomData,

231
src/writer.rs
View File

@ -1,40 +1,163 @@
use crate::*; use crate::*;
/// 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<const DIM: usize, const METADIM: usize, C> =
Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterF64<const DIM: usize, const METADIM: usize, C> =
Writer<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterI8<const DIM: usize, const METADIM: usize, C> =
Writer<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterI16<const DIM: usize, const METADIM: usize, C> =
Writer<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterI32<const DIM: usize, const METADIM: usize, C> =
Writer<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterU8<const DIM: usize, const METADIM: usize, C> =
Writer<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterU16<const DIM: usize, const METADIM: usize, C> =
Writer<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterU32<const DIM: usize, const METADIM: usize, C> =
Writer<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
/// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`). /// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`).
pub type WriterXYZ = Writer<f32, { size_of!(f32) }, 3, 0, PointXYZ>; pub type WriterXYZ = WriterF32<3, 0, PointXYZ>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`). /// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`).
pub type WriterXYZI = Writer<f32, { size_of!(f32) }, 3, 1, PointXYZI>; pub type WriterXYZI = WriterF32<3, 1, PointXYZI>;
/// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`). /// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`).
pub type WriterXYZNormal = Writer<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>; pub type WriterXYZNormal = WriterF32<3, 3, PointXYZNormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`). /// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`).
pub type WriterXYZRGB = Writer<f32, { size_of!(f32) }, 3, 1, PointXYZRGB>; pub type WriterXYZRGB = WriterF32<3, 1, PointXYZRGB>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`). /// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`).
pub type WriterXYZRGBL = Writer<f32, { size_of!(f32) }, 3, 2, PointXYZRGBL>; pub type WriterXYZRGBL = WriterF32<3, 2, PointXYZRGBL>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels and alpha channel (see `PointXYZRGBA`). /// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels and alpha channel (see `PointXYZRGBA`).
pub type WriterXYZRGBA = Writer<f32, { size_of!(f32) }, 3, 2, PointXYZRGBA>; pub type WriterXYZRGBA = WriterF32<3, 2, PointXYZRGBA>;
/// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`). /// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`).
pub type WriterXYZRGBNormal = Writer<f32, { size_of!(f32) }, 3, 4, PointXYZRGBNormal>; pub type WriterXYZRGBNormal = WriterF32<3, 4, PointXYZRGBNormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`). /// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
pub type WriterXYZINormal = Writer<f32, { size_of!(f32) }, 3, 4, PointXYZINormal>; pub type WriterXYZINormal = WriterF32<3, 4, PointXYZINormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`). /// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`).
pub type WriterXYZL = Writer<f32, { size_of!(f32) }, 3, 1, PointXYZL>; pub type WriterXYZL = WriterF32<3, 1, PointXYZL>;
// eats an iterator, can only write from iterators to messages, so must not implement iterator pattern! /// The Write creates a PointCloud2Msg out of your point iterator.
///
/// The iterator is defined at compile time, so the Point has to be described via template arguments.
///
/// # Predefined Readers
/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
///
/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
///
/// ROS1 with rosrust:
/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ROS2 with r2r:
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
///
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
///
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// // ^^^^^^^^^ creating PointCloud2Msg from an iterator
/// ```
/// # Fully Custom Writer
/// When the predefined types are not enough (like sensor specific metadata), you can describe your Writer with the following template arguments:
/// - T: The coordinate type, e.g. f32
/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
/// Afterwards, implement the PointConvertible trait for your custom point type.
///
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, convert::MetaNames, PointMeta,
/// };
///
/// type Xyz = f32; // coordinate type
/// const XYZ_S: usize = size_of!(Xyz);
/// const DIM: usize = 3; // helper constant for dimensionality
/// const METADIM: usize = 1; // helper constant for meta data fields
///
/// #[derive(Debug, PartialEq, Clone)]
/// struct CustomPoint {
/// x: f32,
/// y: f32,
/// z: f32,
/// i: u8,
/// }
///
/// impl From<Point<f32, 3, 1>> for CustomPoint {
/// fn from(point: Point<f32, 3, 1>) -> Self {
/// Self {
/// x: point.coords[0],
/// y: point.coords[1],
/// z: point.coords[2],
/// i: point.meta[0].get(),
/// }
/// }
/// }
///
///impl Into<Point<f32, 3, 1>> for CustomPoint {
/// fn into(self) -> Point<f32, 3, 1> {
/// Point {
/// coords: [self.x, self.y, self.z],
/// meta: [self.i.into()],
/// }
/// }
///}
///
/// impl MetaNames<METADIM> for CustomPoint {
/// fn meta_names() -> [String; METADIM] {
/// [format!("intensity")]
/// }
/// }
/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
///
/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// // ^^^^^^^^ your custom Writer
/// ```
pub struct Writer<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C> pub struct Writer<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
where where
T: FromBytes, T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>, C: PointConvertible<T, SIZE, DIM, METADIM>,
{ {
coordinates: Box<dyn Iterator<Item = C>>, // internal iterator coordinates: Box<dyn Iterator<Item = C>>,
xyz_field_type: FieldDatatype,
phantom_t: std::marker::PhantomData<T>, phantom_t: std::marker::PhantomData<T>,
} }
@ -46,41 +169,45 @@ where
{ {
type Error = ConversionError; type Error = ConversionError;
/// Convert the point cloud to a ROS message. /// Writes the points to a PointCloud2Msg.
/// First use the `try_from` method for initializing the conversion and parsing meta data. ///
/// First use the `from` method for initializing the Writer.
/// Then use the `try_into` method to do the actual conversion. /// Then use the `try_into` method to do the actual conversion.
/// ///
/// The operation is O(n) in time complexity where n is the number of points in the point cloud.
///
/// # Example /// # Example
/// ``` /// ```
/// use ros_pointcloud2::ros_types::PointCloud2Msg; /// use ros_pointcloud2::{
/// use ros_pointcloud2::ConvertXYZ; /// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
/// use ros_pointcloud2::pcl_utils::PointXYZ; /// };
/// ///
/// let cloud_points: Vec<PointXYZ> = vec![ /// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ]; /// ];
/// let msg_out: PointCloud2Msg = WriterXYZ::try_from(cloud_points).unwrap().try_into().unwrap(); /// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// // ^^^^^^^^ ROS message conversion
/// ``` /// ```
fn try_into(mut self) -> Result<PointCloud2Msg, Self::Error> { fn try_into(mut self) -> Result<PointCloud2Msg, Self::Error> {
if DIM > 3 { if DIM > 3 {
return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time? return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time?
} }
let datatype: u8 = convert_to_msg_code(&self.xyz_field_type);
let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack
if DIM > 1 { if DIM > 1 {
fields.push(PointFieldMsg { fields.push(PointFieldMsg {
name: "x".to_string(), name: "x".to_string(),
offset: 0, offset: 0,
datatype, datatype: T::field_datatype().into(),
count: 1, count: 1,
}); });
fields.push(PointFieldMsg { fields.push(PointFieldMsg {
name: "y".to_string(), name: "y".to_string(),
offset: SIZE as u32, offset: SIZE as u32,
datatype, datatype: T::field_datatype().into(),
count: 1, count: 1,
}); });
} }
@ -89,48 +216,37 @@ where
fields.push(PointFieldMsg { fields.push(PointFieldMsg {
name: "z".to_string(), name: "z".to_string(),
offset: 2 * SIZE as u32, offset: 2 * SIZE as u32,
datatype, datatype: T::field_datatype().into(),
count: 1, count: 1,
}); });
} }
let mut cloud = PointCloud2Msg::default();
// self was created using a point iterator
let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?; let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
let meta: [PointMeta; METADIM] = match first_point.clone().try_into() { let point: Point<T, DIM, METADIM> = first_point.clone().into();
Ok(meta) => meta.1,
Err(_) => return Err(ConversionError::PointConversionError),
};
let meta_names = C::meta_names(); let meta_names = C::meta_names();
let sized_dim = DIM as u32 * SIZE as u32; let mut meta_offsets_acc = DIM as u32 * SIZE as u32;
for(meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
for (idx, value) in meta.iter().enumerate() { let datatype_code = meta_value.datatype.into();
let datatype: u8 = convert_to_msg_code(&value.datatype); if FieldDatatype::try_from(datatype_code).is_err() {
let mut offset = sized_dim; return Err(ConversionError::UnsupportedFieldType);
for i in 0..idx {
let datatype: u8 = convert_to_msg_code(&meta[i].datatype);
let field_type = convert_msg_code_to_type(datatype)?;
let field_size = datatype_size(&field_type);
offset += field_size as u32;
} }
fields.push(PointFieldMsg { fields.push(PointFieldMsg {
name: meta_names name: meta_name,
.get(idx) offset: meta_offsets_acc,
.ok_or(ConversionError::MetaIndexLengthMismatch)? datatype: datatype_code,
.clone(),
offset,
datatype,
count: 1, count: 1,
}); });
} meta_offsets_acc += meta_value.datatype.size() as u32
};
// calculate point size inside byte buffer let mut cloud = PointCloud2Msg::default();
cloud.point_step = fields.iter().fold(0, |acc, field| { cloud.point_step = fields.iter().fold(0, |acc, field| {
let field_type = convert_msg_code_to_type(field.datatype).unwrap(); let field_type: FieldDatatype = field.datatype.try_into().expect("Unsupported field but checked before.");
let field_size = datatype_size(&field_type); let field_size = field_type.size();
acc + field.count * field_size as u32 acc + field.count * field_size as u32
}); });
@ -156,15 +272,28 @@ where
T: FromBytes, T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>, C: PointConvertible<T, SIZE, DIM, METADIM>,
{ {
/// Create a new Writer struct from an iterator. /// Create a Writer from any iterator that iterates over a template-defined point to a ROS message type.
/// First use the `from` method for initializing the Writer.
/// Then use the `try_into` method to do the actual conversion.
///
/// The operation is O(n) in time complexity where n is the number of points in the point cloud.
/// ///
/// # Example /// # Example
/// ``` /// ```
/// use ros_pointcloud2::{
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// // ^^^^ Writer creation
/// ``` /// ```
pub fn from(coordinates: impl IntoIterator<Item = C> + 'static) -> Self { pub fn from(coordinates: impl IntoIterator<Item = C> + 'static) -> Self {
Self { Self {
coordinates: Box::new(coordinates.into_iter()), coordinates: Box::new(coordinates.into_iter()),
xyz_field_type: FieldDatatype::F32,
phantom_t: std::marker::PhantomData, phantom_t: std::marker::PhantomData,
} }
} }

302
tests/e2e_test.rs
View File

@ -1,4 +1,3 @@
use fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::*; use ros_pointcloud2::pcl_utils::*;
use ros_pointcloud2::reader::*; use ros_pointcloud2::reader::*;
use ros_pointcloud2::ros_types::PointCloud2Msg; use ros_pointcloud2::ros_types::PointCloud2Msg;
@ -8,14 +7,27 @@ use std::fmt::Debug;
macro_rules! convert_from_into { macro_rules! convert_from_into {
($reader:ty, $writer:ty, $point:ty, $cloud:expr) => { ($reader:ty, $writer:ty, $point:ty, $cloud:expr) => {
let copy = $cloud.clone(); convert_from_into_in_out_cloud!(
let msg: Result<PointCloud2Msg, _> = <$writer>::from($cloud).try_into(); $reader,
$writer,
$point,
$cloud.clone(),
$point,
$cloud,
$point
);
};
}
macro_rules! convert_from_into_in_out_cloud {
($reader:ty, $writer:ty, $point:ty, $in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
let msg: Result<PointCloud2Msg, _> = <$writer>::from($in_cloud).try_into();
assert!(msg.is_ok()); assert!(msg.is_ok());
let to_p_type = <$reader>::try_from(msg.unwrap()); let to_p_type = <$reader>::try_from(msg.unwrap());
assert!(to_p_type.is_ok()); assert!(to_p_type.is_ok());
let to_p_type = to_p_type.unwrap(); let to_p_type = to_p_type.unwrap();
let back_to_type = to_p_type.map(|point| Ok(point)).collect::<Vec<$point>>(); let back_to_type = to_p_type.collect::<Vec<$out_point>>();
assert_eq!(copy, back_to_type.unwrap()); assert_eq!($out_cloud, back_to_type);
}; };
} }
@ -30,29 +42,35 @@ fn custom_xyz_f32() {
y: f32, y: f32,
z: f32, z: f32,
} }
type MyReader = Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyWriter = Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>; type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) { type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
fn from(point: CustomPoint) -> Self {
([point.x, point.y, point.z], []) impl From<Point<f32, 3, 0>> for CustomPoint {
fn from(point: Point<f32, 3, 0>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
}
} }
} }
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError; impl Into<Point<f32, 3, 0>> for CustomPoint {
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, ConversionError> { fn into(self) -> Point<f32, 3, 0> {
Ok(Self { Point {
x: data.0[0], coords: [self.x, self.y, self.z],
y: data.0[1], meta: [],
z: data.0[2], }
})
} }
} }
impl convert::MetaNames<METADIM> for CustomPoint { impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] { fn meta_names() -> [String; METADIM] {
[] []
} }
} }
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {} impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
convert_from_into!( convert_from_into!(
MyReader, MyReader,
@ -80,6 +98,8 @@ fn custom_xyz_f32() {
#[test] #[test]
fn custom_xyzi_f32() { fn custom_xyzi_f32() {
type Xyz = f32;
const XYZ_S: usize = std::mem::size_of::<Xyz>();
const DIM: usize = 3; const DIM: usize = 3;
const METADIM: usize = 1; const METADIM: usize = 1;
#[derive(Debug, PartialEq, Clone)] #[derive(Debug, PartialEq, Clone)]
@ -89,32 +109,37 @@ fn custom_xyzi_f32() {
z: f32, z: f32,
i: u8, i: u8,
} }
type MyReader = reader::Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) { impl From<Point<f32, 3, 1>> for CustomPoint {
fn from(point: CustomPoint) -> Self { fn from(point: Point<f32, 3, 1>) -> Self {
([point.x, point.y, point.z], [PointMeta::new(point.i)]) Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
i: point.meta[0].get(),
}
} }
} }
type MyWriter = writer::Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>; impl Into<Point<f32, 3, 1>> for CustomPoint {
fn into(self) -> Point<f32, 3, 1> {
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { Point {
type Error = ConversionError; coords: [self.x, self.y, self.z],
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, ConversionError> { meta: [self.i.into()],
Ok(Self { }
x: data.0[0],
y: data.0[1],
z: data.0[2],
i: data.1.first().unwrap().get().unwrap(),
})
} }
} }
impl convert::MetaNames<METADIM> for CustomPoint { impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] { fn meta_names() -> [String; METADIM] {
[format!("intensity")] [format!("intensity")]
} }
} }
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
type MyReader = reader::Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
type MyWriter = writer::Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
let cloud = vec![ let cloud = vec![
CustomPoint { CustomPoint {
@ -159,33 +184,29 @@ fn custom_rgba_f32() {
b: u8, b: u8,
a: u8, a: u8,
} }
type MyReader = reader::Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>; type MyReader = reader::Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
type MyWriter = writer::Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>; type MyWriter = writer::Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self { impl From<Point<f32, 3, 4>> for CustomPoint {
( fn from(point: Point<f32, 3, 4>) -> Self {
[point.x, point.y, point.z], Self {
[ x: point.coords[0],
PointMeta::new(point.r), y: point.coords[1],
PointMeta::new(point.g), z: point.coords[2],
PointMeta::new(point.b), r: point.meta[0].get(),
PointMeta::new(point.a), g: point.meta[1].get(),
], b: point.meta[2].get(),
) a: point.meta[3].get(),
}
} }
} }
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError; impl Into<Point<f32, 3, 4>> for CustomPoint {
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, Self::Error> { fn into(self) -> Point<f32, 3, 4> {
Ok(Self { Point {
x: data.0[0], coords: [self.x, self.y, self.z],
y: data.0[1], meta: [self.r.into(), self.g.into(), self.b.into(), self.a.into()],
z: data.0[2], }
r: data.1[0].get()?,
g: data.1[1].get()?,
b: data.1[2].get()?,
a: data.1[3].get()?,
})
} }
} }
impl convert::MetaNames<METADIM> for CustomPoint { impl convert::MetaNames<METADIM> for CustomPoint {
@ -193,7 +214,7 @@ fn custom_rgba_f32() {
["r", "g", "b", "a"].map(|s| s.to_string()) ["r", "g", "b", "a"].map(|s| s.to_string())
} }
} }
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {} impl PointConvertible<f32, { std::mem::size_of::<f32>() }, DIM, METADIM> for CustomPoint {}
let cloud = vec![ let cloud = vec![
CustomPoint { CustomPoint {
x: 0.0, x: 0.0,
@ -614,3 +635,162 @@ fn converterxyzi() {
] ]
); );
} }
#[test]
fn write_xyzi_read_xyz() {
let write_cloud = vec![
PointXYZI {
x: 0.0,
y: 1.0,
z: 5.0,
intensity: 0.0,
},
PointXYZI {
x: 1.0,
y: 1.5,
z: 5.0,
intensity: 1.0,
},
PointXYZI {
x: 1.3,
y: 1.6,
z: 5.7,
intensity: 2.0,
},
PointXYZI {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
intensity: f32::MAX,
},
];
let read_cloud = vec![
PointXYZ {
x: 0.0,
y: 1.0,
z: 5.0,
},
PointXYZ {
x: 1.0,
y: 1.5,
z: 5.0,
},
PointXYZ {
x: 1.3,
y: 1.6,
z: 5.7,
},
PointXYZ {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
},
];
convert_from_into_in_out_cloud!(
ReaderXYZ,
WriterXYZI,
PointXYZI,
write_cloud,
PointXYZI,
read_cloud,
PointXYZ
);
}
#[test]
fn write_less_than_available() {
const DIM: usize = 3;
const METADIM: usize = 0;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32,
y: f32,
z: f32,
dummy: f32,
}
type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
impl From<Point<f32, 3, 0>> for CustomPoint {
fn from(point: Point<f32, 3, 0>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
dummy: 0.0,
}
}
}
impl Into<Point<f32, 3, 0>> for CustomPoint {
fn into(self) -> Point<f32, 3, 0> {
Point {
coords: [self.x, self.y, self.z],
meta: [],
}
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
[]
}
}
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
let write_cloud = vec![
CustomPoint {
x: 1.0,
y: 2.0,
z: 3.0,
dummy: -10.0,
},
CustomPoint {
x: 4.0,
y: 5.0,
z: 6.0,
dummy: -10.0,
},
CustomPoint {
x: 7.0,
y: 8.0,
z: 9.0,
dummy: -10.0,
},
];
let read_cloud = vec![
CustomPoint {
x: 1.0,
y: 2.0,
z: 3.0,
dummy: 0.0,
},
CustomPoint {
x: 4.0,
y: 5.0,
z: 6.0,
dummy: 0.0,
},
CustomPoint {
x: 7.0,
y: 8.0,
z: 9.0,
dummy: 0.0,
},
];
convert_from_into_in_out_cloud!(
MyReader,
MyWriter,
CustomPoint,
write_cloud,
CustomPoint,
read_cloud,
CustomPoint
);
}

12
tests/r2r_msg_test.rs
View File

@ -1,11 +1,11 @@
#[cfg(feature = "r2r_msg")] #[cfg(feature = "r2r_msg")]
#[test] #[test]
fn convertxyz_r2r_msg() { fn convertxyz_r2r_msg() {
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
use r2r::sensor_msgs::msg::PointCloud2; use r2r::sensor_msgs::msg::PointCloud2;
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::ConvertXYZ;
let cloud = vec![ let cloud = vec![
PointXYZ { PointXYZ {
@ -25,10 +25,10 @@ fn convertxyz_r2r_msg() {
}, },
]; ];
let copy = cloud.clone(); let copy = cloud.clone();
let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
let r2r_msg_cloud: PointCloud2 = internal_cloud.into(); let r2r_msg_cloud: PointCloud2 = internal_cloud.into();
let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into(); let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into();
let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap(); let to_convert: ConvertXYZ = ReaderXYZ::try_from(convert_back_internal).unwrap();
let back_to_type = to_convert.map(|point| Ok(point)).collect::<Vec<PointXYZ>>(); let back_to_type = to_convert.map(|point| Ok(point)).collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type.unwrap()); assert_eq!(copy, back_to_type.unwrap());
} }

11
tests/rosrust_msg_test.rs
View File

@ -1,10 +1,9 @@
#[cfg(feature = "rosrust_msg")] #[cfg(feature = "rosrust_msg")]
#[test] #[test]
fn convertxyz_rosrust_msg() { fn convertxyz_rosrust_msg() {
use ros_pointcloud2::fallible_iterator::FallibleIterator; use ros_pointcloud2::{
use ros_pointcloud2::pcl_utils::PointXYZ; pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
use ros_pointcloud2::ros_types::PointCloud2Msg; };
use ros_pointcloud2::ConvertXYZ;
let cloud = vec![ let cloud = vec![
PointXYZ { PointXYZ {
@ -24,10 +23,10 @@ fn convertxyz_rosrust_msg() {
}, },
]; ];
let copy = cloud.clone(); let copy = cloud.clone();
let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into(); let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into();
let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into(); let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into();
let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap(); let to_convert: ConvertXYZ = ReaderXYZ::try_from(convert_back_internal).unwrap();
let back_to_type = to_convert.map(|point| Ok(point)).collect::<Vec<PointXYZ>>(); let back_to_type = to_convert.map(|point| Ok(point)).collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type.unwrap()); assert_eq!(copy, back_to_type.unwrap());
} }