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/**"]
[dependencies]
fallible-iterator = "0.3.0"
rosrust_msg = { version = "0.1", optional = true }
rosrust = { version = "0.9.11", optional = true }
r2r = { version = "0.8.4", optional = true }

76
README.md
View File

@ -5,16 +5,14 @@
</p>
</p>
Providing a memory efficient way for message conversion while allowing user defined types without the cost of iterations.
Instead of converting the entire cloud into a `Vec`, you get an iterable type that converts each point from the message on the fly.
Providing a easy to use, generics defined, point-wise iterator abstraction of the byte buffer of `PointCloud2` to minimize iterations in your processing pipeline.
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
use ros_pointcloud2::{
fallible_iterator::FallibleIterator,
pcl_utils::PointXYZ,
ros_types::PointCloud2Msg,
ConvertXYZ,
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
// 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
let internal_msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points)
.unwrap()
.try_into()
// Some changes to demonstrate lazy iterator usage
let changed_it = cloud_points.iter().map(|p| {
p.x = 0.5;
});
// Vector -> Writer -> Message
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points)
.try_into() // iterating points here O(n)
.unwrap();
// Convert to your favorite ROS crate message type, we will use rosrust here.
// let msg: rosrust_msg::sensor_msgs::PointCloud2 = internal_msg.into();
// Anything that implements `IntoIterator` also works - like another iterator.
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();
// Message -> Converter -> Vector
let convert: ConvertXYZ = ConvertXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = convert
// Message -> Reader. The Reader implements the Iterator trait.
let reader = ReaderXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = reader
.map(|point: PointXYZ| {
// Insert your point business logic here
// or use other methods like .for_each().
// Some logic here
// We are using a fallible iterator so we need to return a Result.
Ok(point)
point
})
.collect::<Vec<PointXYZ>>()
.unwrap(); // Handle point conversion or byte errors here.
.collect::<Vec<PointXYZ>>();
assert_eq!(new_cloud_points, cloud_copy);
```
@ -226,10 +233,25 @@ let to_custom_type = MyConverter::try_from(custom_msg).unwrap();
## Future Work
- 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
- 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
[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.
pub trait GetFieldDatatype {
fn field_datatype() -> FieldDatatype;
@ -72,30 +87,36 @@ impl GetFieldDatatype for i16 {
}
}
pub(crate) fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 {
match geo_type {
FieldDatatype::I8 => 1,
FieldDatatype::U8 => 2,
FieldDatatype::I16 => 3,
FieldDatatype::U16 => 4,
FieldDatatype::I32 => 5,
FieldDatatype::U32 => 6,
FieldDatatype::F32 => 7,
FieldDatatype::F64 => 8,
impl TryFrom<u8> for FieldDatatype {
type Error = ConversionError;
fn try_from(value: u8) -> Result<Self, Self::Error> {
match value {
1 => Ok(FieldDatatype::I8),
2 => Ok(FieldDatatype::U8),
3 => Ok(FieldDatatype::I16),
4 => Ok(FieldDatatype::U16),
5 => Ok(FieldDatatype::I32),
6 => Ok(FieldDatatype::U32),
7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64),
_ => Err(ConversionError::UnsupportedFieldType),
}
}
}
pub(crate) fn convert_msg_code_to_type(code: u8) -> Result<FieldDatatype, ConversionError> {
match code {
7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64),
5 => Ok(FieldDatatype::I32),
2 => Ok(FieldDatatype::U8),
4 => Ok(FieldDatatype::U16),
6 => Ok(FieldDatatype::U32),
1 => Ok(FieldDatatype::I8),
3 => Ok(FieldDatatype::I16),
_ => Err(ConversionError::UnsupportedFieldType),
impl Into<u8> for FieldDatatype {
fn into(self) -> u8 {
match self {
FieldDatatype::I8 => 1,
FieldDatatype::U8 => 2,
FieldDatatype::I16 => 3,
FieldDatatype::U16 => 4,
FieldDatatype::I32 => 5,
FieldDatatype::U32 => 6,
FieldDatatype::F32 => 7,
FieldDatatype::F64 => 8,
}
}
}
@ -107,7 +128,7 @@ pub(crate) fn check_coord(
match coord {
Some(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);
}
Ok(field.clone())
@ -122,19 +143,6 @@ pub trait MetaNames<const METADIM: usize> {
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)]
pub(crate) fn add_point_to_byte_buffer<
T: FromBytes,
@ -146,19 +154,17 @@ pub(crate) fn add_point_to_byte_buffer<
coords: C,
cloud: &mut PointCloud2Msg,
) -> Result<bool, ConversionError> {
let (coords_data, coords_meta): ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() {
Ok(meta) => meta,
Err(_) => return Err(ConversionError::PointConversionError),
};
let point: Point<T, DIM, METADIM> = coords.into();
// (x, y, z...)
coords_data
point
.coords
.iter()
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
// meta data description
coords_meta.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)];
point.meta.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
cloud.data.extend_from_slice(truncated_bytes);
});
@ -300,35 +306,28 @@ pub(crate) fn load_loadable<T: FromBytes, const SIZE: usize>(
start_idx: usize,
data: &[u8],
endian: &Endianness,
) -> Option<T> {
) -> T {
match endian {
Endianness::Big => Some(T::from_be_bytes(
load_bytes::<SIZE>(start_idx, data)?.as_slice(),
)),
Endianness::Little => Some(T::from_le_bytes(
load_bytes::<SIZE>(start_idx, data)?.as_slice(),
)),
Endianness::Big => T::from_be_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
Endianness::Little => 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]> {
if start_idx + S > data.len() {
return None;
}
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;
}
}
}
/// Note: check if the data slice is long enough to load the bytes beforehand!
fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> [u8; S] {
let mut target = [u8::default(); S];
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)]

147
src/lib.rs
View File

@ -1,13 +1,18 @@
// TODO test reading writing different types
// 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")]
//#![doc = include_str!("../README.md")]
pub mod convert;
pub mod pcl_utils;
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_rules! size_of {
($t:ty) => {
@ -24,37 +29,43 @@ use crate::convert::*;
use crate::pcl_utils::*;
use crate::ros_types::PointFieldMsg;
pub use fallible_iterator::FallibleIterator;
/// Errors that can occur when converting between PointCloud2 and a custom type.
/// All errors that can occur for creating Reader and Writer.
#[derive(Debug)]
pub enum ConversionError {
/// The coordinate field does not match the expected datatype.
InvalidFieldFormat,
/// Not enough meta or dimensional fields in the PointCloud2 message.
NotEnoughFields,
/// The dimensionality of the point inside the message is too high.
TooManyDimensions,
/// The field type is not supported by the ROS message description.
UnsupportedFieldType,
/// There are no points in the point cloud.
NoPoints,
/// The length of the byte buffer in the message does not match the expected length computed from the fields.
DataLengthMismatch,
MetaIndexLengthMismatch,
EndOfBuffer,
PointConversionError,
MetaDatatypeMismatch,
}
pub struct Point<T, const DIM: usize, const METADIM: usize> {
pub coords: [T; DIM],
pub meta: [PointMeta; METADIM],
}
/// Trait to convert a point to a tuple of coordinates and meta data.
/// Implement this trait for your point type to use the conversion.
pub trait PointConvertible<T, const SIZE: usize, const DIM: usize, const METADIM: usize>:
TryInto<([T; DIM], [PointMeta; METADIM])>
+ TryFrom<([T; DIM], [PointMeta; METADIM])>
+ MetaNames<METADIM>
+ Clone
+ 'static
From<Point<T, DIM, METADIM>> + Into<Point<T, DIM, METADIM>> + MetaNames<METADIM> + Clone + 'static
where
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
/// datatype that is encoded so that it can be decoded later.
@ -65,7 +76,7 @@ where
///
/// let original_data: f64 = 1.0;
/// let meta = PointMeta::new(original_data);
/// let my_data: f64 = meta.get().unwrap();
/// let my_data: f64 = meta.get();
/// ```
#[derive(Debug, Clone, Copy)]
pub struct PointMeta {
@ -92,51 +103,93 @@ impl PointMeta {
pub fn new<T: FromBytes>(value: T) -> Self {
let raw_bytes = T::bytes(&value);
let mut bytes = [0; std::mem::size_of::<f64>()];
for (idx, byte) in raw_bytes.iter().enumerate() {
bytes[idx] = *byte;
for (byte, save_byte) in raw_bytes.into_iter().zip(bytes.iter_mut()) {
*save_byte = byte;
}
Self {
bytes,
datatype: T::field_datatype(),
}
}
fn new_from_buffer(
data: &[u8],
offset: usize,
datatype: &FieldDatatype,
) -> Result<Self, ConversionError> {
let size = datatype_size(datatype);
let bytes = data
.get(offset..offset + size)
.ok_or(ConversionError::DataLengthMismatch)?;
fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self {
debug_assert!(data.len() >= offset + datatype.size());
let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) };
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() {
bytes_array[idx] = *byte;
for (byte, save_byte) in bytes.into_iter().zip(bytes_array.iter_mut()) {
*save_byte = *byte;
}
Ok(Self {
Self {
bytes: bytes_array,
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
/// ```
/// let original_data: f64 = 1.0;
/// 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> {
if self.datatype != T::field_datatype() {
return Err(ConversionError::MetaDatatypeMismatch);
}
let size = datatype_size(&T::field_datatype());
if let Some(bytes) = self.bytes.get(0..size) {
Ok(T::from_le_bytes(bytes))
} else {
Err(ConversionError::DataLengthMismatch) // self.bytes are not long enough, already handled while parsing
}
pub fn get<T: FromBytes>(&self) -> T {
let size = T::field_datatype().size();
let bytes = self
.bytes
.get(0..size)
.expect("Exceeds bounds of f64, which is the largest type");
T::from_le_bytes(bytes)
}
}
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]
fn pack_rgb(r: u8, g: u8, b: u8) -> f32 {
let packed = ((r as u32) << 16) + ((g as u32) << 8) + (b as u32);
f32::from_bits(packed)
}
/// Unpack an RGB color from a single f32 value as used in ROS with PCL for RViz usage.
#[inline]
fn unpack_rgb(rgb: f32) -> [u8; 3] {
let packed: u32 = rgb.to_bits();
@ -24,23 +26,22 @@ pub struct PointXYZ {
pub z: f32,
}
impl TryInto<([f32; 3], [PointMeta; 0])> for PointXYZ {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 0]), Self::Error> {
Ok(([self.x, self.y, self.z], []))
impl From<Point<f32, 3, 0>> for PointXYZ {
fn from(point: Point<f32, 3, 0>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 0])> for PointXYZ {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 0])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
})
impl Into<Point<f32, 3, 0>> for PointXYZ {
fn into(self) -> Point<f32, 3, 0> {
Point {
coords: [self.x, self.y, self.z],
meta: Default::default(),
}
}
}
@ -62,24 +63,23 @@ pub struct PointXYZI {
pub intensity: f32,
}
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZI {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
Ok(([self.x, self.y, self.z], [PointMeta::new(self.intensity)]))
impl From<Point<f32, 3, 1>> for PointXYZI {
fn from(point: Point<f32, 3, 1>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
intensity: point.meta[0].get(),
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZI {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
intensity: data.1[0].get()?,
})
impl Into<Point<f32, 3, 1>> for PointXYZI {
fn into(self) -> Point<f32, 3, 1> {
Point {
coords: [self.x, self.y, self.z],
meta: [self.intensity.into()],
}
}
}
@ -103,29 +103,28 @@ pub struct PointXYZRGB {
pub b: u8,
}
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
type Error = ConversionError;
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>()?;
impl From<Point<f32, 3, 1>> for PointXYZRGB {
fn from(point: Point<f32, 3, 1>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
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,
}
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
type Error = ConversionError;
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>()?;
impl From<Point<f32, 3, 2>> for PointXYZRGBA {
fn from(point: Point<f32, 3, 2>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
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,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
type Error = ConversionError;
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>()?;
impl From<Point<f32, 3, 4>> for PointXYZRGBNormal {
fn from(point: Point<f32, 3, 4>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
normal_x: data.1[1].get()?,
normal_y: data.1[2].get()?,
normal_z: data.1[3].get()?,
})
normal_x: point.meta[1].get(),
normal_y: point.meta[2].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,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZINormal {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok((
[self.x, self.y, self.z],
[
PointMeta::new(self.intensity),
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
],
))
impl From<Point<f32, 3, 4>> for PointXYZINormal {
fn from(point: Point<f32, 3, 4>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
intensity: point.meta[0].get(),
normal_x: point.meta[1].get(),
normal_y: point.meta[2].get(),
normal_z: point.meta[3].get(),
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
intensity: data.1[0].get()?,
normal_x: data.1[1].get()?,
normal_y: data.1[2].get()?,
normal_z: data.1[3].get()?,
})
impl Into<Point<f32, 3, 4>> for PointXYZINormal {
fn into(self) -> Point<f32, 3, 4> {
Point {
coords: [self.x, self.y, self.z],
meta: [
self.intensity.into(),
self.normal_x.into(),
self.normal_y.into(),
self.normal_z.into(),
],
}
}
}
@ -312,24 +299,23 @@ pub struct PointXYZL {
pub label: u32,
}
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZL {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)]))
impl From<Point<f32, 3, 1>> for PointXYZL {
fn from(point: Point<f32, 3, 1>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
label: point.meta[0].get(),
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
label: data.1[0].get()?,
})
impl Into<Point<f32, 3, 1>> for PointXYZL {
fn into(self) -> Point<f32, 3, 1> {
Point {
coords: [self.x, self.y, self.z],
meta: [self.label.into()],
}
}
}
@ -354,33 +340,29 @@ pub struct PointXYZRGBL {
pub label: u32,
}
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
type Error = ConversionError;
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>()?;
impl From<Point<f32, 3, 2>> for PointXYZRGBL {
fn from(point: Point<f32, 3, 2>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
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,
}
impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZNormal {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> {
Ok((
[self.x, self.y, self.z],
[
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
],
))
impl From<Point<f32, 3, 3>> for PointXYZNormal {
fn from(point: Point<f32, 3, 3>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
normal_x: point.meta[0].get(),
normal_y: point.meta[1].get(),
normal_z: point.meta[2].get(),
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZNormal {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 3])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
normal_x: data.1[0].get()?,
normal_y: data.1[1].get()?,
normal_z: data.1[2].get()?,
})
impl Into<Point<f32, 3, 3>> for PointXYZNormal {
fn into(self) -> Point<f32, 3, 3> {
Point {
coords: [self.x, self.y, self.z],
meta: [
self.normal_x.into(),
self.normal_y.into(),
self.normal_z.into(),
],
}
}
}

256
src/reader.rs
View File

@ -1,50 +1,85 @@
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`).
/// 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`).
/// 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`).
/// 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`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// 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`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// 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`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// 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`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// 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`).
/// 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`).
/// Every additional meta data is ignored.
/// 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.
/// At runtime, the iterator catches read errors from the Msg type which typically only occur with corrupted data.
/// 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.
@ -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.
///
/// # Example
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// FallibleIterator, // needed for for_each and the other iterator methods
/// reader::ReaderXYZ,
/// writer::WriterXYZ,
/// PointCloud2Msg,
/// pcl_utils::PointXYZ,
/// pcl_utils::PointXYZ, reader::ReaderXYZ, 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();
/// 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| {
/// // 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>
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.
///
/// 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>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
type Item = C;
type Error = ConversionError;
/// 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.
///
/// 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 {
return Ok(None); // iteration finished
return None; // iteration finished
}
let mut xyz: [T; DIM] = [T::default(); DIM];
let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM];
for (idx, in_point_offset) in self.offsets.iter().enumerate() {
if idx < DIM {
match load_loadable::<T, SIZE>(
xyz.iter_mut()
.zip(self.offsets.iter())
.for_each(|(p_xyz, in_point_offset)| {
*p_xyz = load_loadable::<T, SIZE>(
(self.iteration * self.point_step_size) + in_point_offset,
&self.data,
&self.endianness,
) {
Some(c) => xyz[idx] = c,
None => return Err(ConversionError::EndOfBuffer),
}
} else {
let meta_idx = idx - DIM;
let meta_type = self.meta[meta_idx].1;
);
});
debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
debug_assert!(
self.offsets.len() == DIM + METADIM,
"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;
if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) {
meta[meta_idx] = m;
} else {
return Err(ConversionError::MetaIndexLengthMismatch);
}
}
}
*p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
});
self.iteration += 1;
match C::try_from((xyz, meta)) {
Err(_) => Err(ConversionError::PointConversionError),
Ok(tuple) => Ok(Some(tuple)),
}
Some(C::from(Point { coords: xyz, meta }))
}
}
@ -179,11 +267,7 @@ where
/// # Example
/// ```
/// use ros_pointcloud2::{
/// FallibleIterator, // needed for for_each and the other iterator methods
/// reader::ReaderXYZ,
/// writer::WriterXYZ,
/// PointCloud2Msg,
/// pcl_utils::PointXYZ,
/// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
/// let cloud_points: Vec<PointXYZ> = vec![
@ -222,33 +306,36 @@ where
"z" => has_z = Some(idx),
_ => {
if lower_meta_names.contains(&lower_field_name) {
println!("{} {} {}", idx, field.name, meta_with_offsets.len());
match meta_with_offsets.get_mut(idx - DIM) {
None => return Err(ConversionError::MetaIndexLengthMismatch),
Some(data) => {
data.0 = field.name.clone();
data.1 = convert_msg_code_to_type(field.datatype)?;
data.2 = field.offset as usize;
}
}
let meta_idx = idx - DIM;
debug_assert!(
meta_idx < meta_with_offsets.len(),
"Meta data length mismatch"
);
meta_with_offsets[meta_idx].0 = field.name.clone();
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: Vec<(String, FieldDatatype)> =
vec![(String::default(), FieldDatatype::U8); METADIM];
let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM];
meta_with_offsets
.into_iter()
.enumerate()
.for_each(|(idx, (name, datatype, offset))| {
unsafe {
// all arrays have size METADIM
*meta.get_unchecked_mut(idx) = (name, datatype);
*meta_offsets.get_unchecked_mut(idx) = offset;
};
.zip(meta.iter_mut())
.zip(meta_offsets.iter_mut())
.for_each(|(((name, datatype, offset), meta), meta_offset)| {
*meta = (name, datatype);
*meta_offset = offset;
});
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;
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);
}
if let Some(max_offset) = offsets.last() {
if let Some(last_meta) = meta.last() {
let size_with_last_meta = max_offset + datatype_size(&last_meta.1);
if size_with_last_meta > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
let last_offset = offsets.last().expect("Dimensionality is 0.");
if let Some(last_meta) = meta.last() {
let size_with_last_meta = last_offset + last_meta.1.size();
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 {
iteration: 0,
data: cloud.data,
point_step_size: point_step_size,
point_step_size,
cloud_length: cloud.width as usize * cloud.height as usize,
offsets: offsets,
meta: meta,
offsets,
meta,
endianness: endian,
phantom_t: std::marker::PhantomData,
phantom_c: std::marker::PhantomData,

245
src/writer.rs
View File

@ -1,40 +1,163 @@
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`).
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`).
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`).
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`).
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`).
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`).
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`).
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`).
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`).
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>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
coordinates: Box<dyn Iterator<Item = C>>, // internal iterator
xyz_field_type: FieldDatatype,
coordinates: Box<dyn Iterator<Item = C>>,
phantom_t: std::marker::PhantomData<T>,
}
@ -46,41 +169,45 @@ where
{
type Error = ConversionError;
/// Convert the point cloud to a ROS message.
/// First use the `try_from` method for initializing the conversion and parsing meta data.
/// Writes the points to a PointCloud2Msg.
///
/// 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
/// ```
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// use ros_pointcloud2::ConvertXYZ;
/// use ros_pointcloud2::pcl_utils::PointXYZ;
/// 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::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> {
if DIM > 3 {
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
if DIM > 1 {
fields.push(PointFieldMsg {
name: "x".to_string(),
offset: 0,
datatype,
datatype: T::field_datatype().into(),
count: 1,
});
fields.push(PointFieldMsg {
name: "y".to_string(),
offset: SIZE as u32,
datatype,
datatype: T::field_datatype().into(),
count: 1,
});
}
@ -89,48 +216,37 @@ where
fields.push(PointFieldMsg {
name: "z".to_string(),
offset: 2 * SIZE as u32,
datatype,
datatype: T::field_datatype().into(),
count: 1,
});
}
let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
let point: Point<T, DIM, METADIM> = first_point.clone().into();
let meta_names = C::meta_names();
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()) {
let datatype_code = meta_value.datatype.into();
if FieldDatatype::try_from(datatype_code).is_err() {
return Err(ConversionError::UnsupportedFieldType);
}
fields.push(PointFieldMsg {
name: meta_name,
offset: meta_offsets_acc,
datatype: datatype_code,
count: 1,
});
meta_offsets_acc += meta_value.datatype.size() as u32
};
let mut cloud = PointCloud2Msg::default();
// self was created using a point iterator
let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
let meta: [PointMeta; METADIM] = match first_point.clone().try_into() {
Ok(meta) => meta.1,
Err(_) => return Err(ConversionError::PointConversionError),
};
let meta_names = C::meta_names();
let sized_dim = DIM as u32 * SIZE as u32;
for (idx, value) in meta.iter().enumerate() {
let datatype: u8 = convert_to_msg_code(&value.datatype);
let mut offset = sized_dim;
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 {
name: meta_names
.get(idx)
.ok_or(ConversionError::MetaIndexLengthMismatch)?
.clone(),
offset,
datatype,
count: 1,
});
}
// calculate point size inside byte buffer
cloud.point_step = fields.iter().fold(0, |acc, field| {
let field_type = convert_msg_code_to_type(field.datatype).unwrap();
let field_size = datatype_size(&field_type);
let field_type: FieldDatatype = field.datatype.try_into().expect("Unsupported field but checked before.");
let field_size = field_type.size();
acc + field.count * field_size as u32
});
@ -156,15 +272,28 @@ where
T: FromBytes,
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
/// ```
/// 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 {
Self {
coordinates: Box::new(coordinates.into_iter()),
xyz_field_type: FieldDatatype::F32,
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::reader::*;
use ros_pointcloud2::ros_types::PointCloud2Msg;
@ -8,14 +7,27 @@ use std::fmt::Debug;
macro_rules! convert_from_into {
($reader:ty, $writer:ty, $point:ty, $cloud:expr) => {
let copy = $cloud.clone();
let msg: Result<PointCloud2Msg, _> = <$writer>::from($cloud).try_into();
convert_from_into_in_out_cloud!(
$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());
let to_p_type = <$reader>::try_from(msg.unwrap());
assert!(to_p_type.is_ok());
let to_p_type = to_p_type.unwrap();
let back_to_type = to_p_type.map(|point| Ok(point)).collect::<Vec<$point>>();
assert_eq!(copy, back_to_type.unwrap());
let back_to_type = to_p_type.collect::<Vec<$out_point>>();
assert_eq!($out_cloud, back_to_type);
};
}
@ -30,29 +42,35 @@ fn custom_xyz_f32() {
y: f32,
z: f32,
}
type MyReader = Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyWriter = Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self {
([point.x, point.y, point.z], [])
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],
}
}
}
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, ConversionError> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
})
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, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
convert_from_into!(
MyReader,
@ -80,6 +98,8 @@ fn custom_xyz_f32() {
#[test]
fn custom_xyzi_f32() {
type Xyz = f32;
const XYZ_S: usize = std::mem::size_of::<Xyz>();
const DIM: usize = 3;
const METADIM: usize = 1;
#[derive(Debug, PartialEq, Clone)]
@ -89,32 +109,37 @@ fn custom_xyzi_f32() {
z: f32,
i: u8,
}
type MyReader = reader::Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self {
([point.x, point.y, point.z], [PointMeta::new(point.i)])
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(),
}
}
}
type MyWriter = writer::Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, ConversionError> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
i: data.1.first().unwrap().get().unwrap(),
})
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 convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
[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![
CustomPoint {
@ -159,33 +184,29 @@ fn custom_rgba_f32() {
b: u8,
a: u8,
}
type MyReader = reader::Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyWriter = writer::Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self {
(
[point.x, point.y, point.z],
[
PointMeta::new(point.r),
PointMeta::new(point.g),
PointMeta::new(point.b),
PointMeta::new(point.a),
],
)
type MyReader = reader::Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
type MyWriter = writer::Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
impl From<Point<f32, 3, 4>> for CustomPoint {
fn from(point: Point<f32, 3, 4>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: point.meta[0].get(),
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;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
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 Into<Point<f32, 3, 4>> for CustomPoint {
fn into(self) -> Point<f32, 3, 4> {
Point {
coords: [self.x, self.y, self.z],
meta: [self.r.into(), self.g.into(), self.b.into(), self.a.into()],
}
}
}
impl convert::MetaNames<METADIM> for CustomPoint {
@ -193,7 +214,7 @@ fn custom_rgba_f32() {
["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![
CustomPoint {
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")]
#[test]
fn convertxyz_r2r_msg() {
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
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![
PointXYZ {
@ -25,10 +25,10 @@ fn convertxyz_r2r_msg() {
},
];
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 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>>();
assert_eq!(copy, back_to_type.unwrap());
}

11
tests/rosrust_msg_test.rs
View File

@ -1,10 +1,9 @@
#[cfg(feature = "rosrust_msg")]
#[test]
fn convertxyz_rosrust_msg() {
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::ConvertXYZ;
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
let cloud = vec![
PointXYZ {
@ -24,10 +23,10 @@ fn convertxyz_rosrust_msg() {
},
];
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 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>>();
assert_eq!(copy, back_to_type.unwrap());
}