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

Fix RGB not rendering in rviz (#2) 

fix rviz rgb rendering #1
This commit is contained in:
Christopher Sieh 2023-04-28 07:57:11 +02:00 committed by GitHub
parent 8288417145
commit 9ca323a5df
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 694 additions and 314 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Cargo.toml
View File

@ -1,6 +1,6 @@
[package] [package]
name = "ros_pointcloud2" name = "ros_pointcloud2"
version = "0.2.0" version = "0.2.1"
edition = "2021" edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"] authors = ["Christopher Sieh <stelzo@steado.de>"]
description = "Customizable conversions for working with sensor_msgs/PointCloud2." description = "Customizable conversions for working with sensor_msgs/PointCloud2."

12
README.md
View File

@ -1,13 +1,13 @@
# ROS PointCloud2 # ROS PointCloud2
A lightweight Rust implementation for fast, safe and customizable conversions to and from the `sensor_msgs/PointCloud2` ROS message. Customizable conversions to and from the `sensor_msgs/PointCloud2` ROS message.
```toml ```toml
[dependencies] [dependencies]
ros_pointcloud2 = "0.2.0" ros_pointcloud2 = "0.2.1"
``` ```
Providing a fast and memory efficient way for message conversion while allowing user defined types without the cost of iterations. 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 `Iterator` that converts each point from the message on the fly. Instead of converting the entire cloud into a `Vec`, you get an `Iterator` that converts each point from the message on the fly.
An example for using this crate is [this filter node](https://github.com/stelzo/cloudfilter). It is also a good starting point for An example for using this crate is [this filter node](https://github.com/stelzo/cloudfilter). It is also a good starting point for
@ -52,7 +52,7 @@ assert_eq!(new_cloud_points, cloud_copy);
To use `ros_pointcloud2` in your favorite ROS crate, you can either use this crate's features (see Integration section below) or implement the `Into` and `From` traits for `PointCloud2Msg`. To use `ros_pointcloud2` in your favorite ROS crate, you can either use this crate's features (see Integration section below) or implement the `Into` and `From` traits for `PointCloud2Msg`.
Please avoid cloning the `data: Vec<u8>` of the message. Try to avoid cloning the `data: Vec<u8>` field.
```rust ```rust
use ros_pointcloud2::ros_types::PointCloud2Msg; use ros_pointcloud2::ros_types::PointCloud2Msg;
@ -73,13 +73,13 @@ impl From<YourROSPointCloud2> for PointCloud2Msg {
## Integrations ## Integrations
Currently, we only implement a conversion for the following ROS crate: Currently, there is only 1 integration for the following ROS crate:
- [rosrust](https://github.com/adnanademovic/rosrust) - [rosrust](https://github.com/adnanademovic/rosrust)
You can use one by enabling the corresponding feature. You can use one by enabling the corresponding feature.
```toml ```toml
[dependencies] [dependencies]
ros_pointcloud2 = { version = "0.2.0", features = ["rosrust_msg"]} ros_pointcloud2 = { version = "0.2.1", features = ["rosrust_msg"]}
``` ```
Please open an issue or PR if you want to see support for other crates. Please open an issue or PR if you want to see support for other crates.

152
src/lib.rs
View File

@ -2,9 +2,9 @@
pub mod pcl_utils; pub mod pcl_utils;
pub mod ros_types; pub mod ros_types;
use num_traits::Zero;
use crate::pcl_utils::*; use crate::pcl_utils::*;
use crate::ros_types::{PointCloud2Msg, PointFieldMsg}; use crate::ros_types::{PointCloud2Msg, PointFieldMsg};
use num_traits::Zero;
#[macro_use] #[macro_use]
pub extern crate mem_macros; pub extern crate mem_macros;
@ -28,8 +28,15 @@ pub enum ConversionError {
/// 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: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize>: TryInto<([T; DIM], [PointMeta; METADIM])> + TryFrom<([T; DIM], [PointMeta; METADIM])> + MetaNames<METADIM> + Clone {} 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
where
T: FromBytes,
{
}
/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html). /// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html).
#[derive(Clone, Debug, PartialEq, Copy)] #[derive(Clone, Debug, PartialEq, Copy)]
@ -56,7 +63,9 @@ impl GetFieldDatatype for f32 {
} }
impl GetFieldDatatype for f64 { impl GetFieldDatatype for f64 {
fn field_datatype() -> FieldDatatype { FieldDatatype::F64 } fn field_datatype() -> FieldDatatype {
FieldDatatype::F64
}
} }
impl GetFieldDatatype for i32 { impl GetFieldDatatype for i32 {
@ -122,7 +131,11 @@ fn convert_msg_code_to_type(code: u8) -> Result<FieldDatatype, ConversionError>
} }
} }
fn check_coord(coord: Option<usize>, fields: &Vec<PointFieldMsg>, xyz_field_type: &FieldDatatype) -> Result<PointFieldMsg, ConversionError> { fn check_coord(
coord: Option<usize>,
fields: &Vec<PointFieldMsg>,
xyz_field_type: &FieldDatatype,
) -> Result<PointFieldMsg, ConversionError> {
match coord { match coord {
Some(y_idx) => { Some(y_idx) => {
let field = &fields[y_idx]; let field = &fields[y_idx];
@ -130,7 +143,7 @@ fn check_coord(coord: Option<usize>, fields: &Vec<PointFieldMsg>, xyz_field_type
return Err(ConversionError::InvalidFieldFormat); return Err(ConversionError::InvalidFieldFormat);
} }
Ok(field.clone()) Ok(field.clone())
}, }
None => { None => {
return Err(ConversionError::NotEnoughFields); return Err(ConversionError::NotEnoughFields);
} }
@ -157,7 +170,11 @@ pub trait MetaNames<const METADIM: usize> {
/// const METADIM : usize = 4; // how many meta fields you have (e.g. r, g, b, a) /// const METADIM : usize = 4; // how many meta fields you have (e.g. r, g, b, a)
/// type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, ([f32; DIM], [PointMeta; METADIM])>; /// type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, ([f32; DIM], [PointMeta; METADIM])>;
/// ``` /// ```
pub struct Convert<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> { pub struct Convert<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
iteration: usize, iteration: usize,
coordinates: Vec<C>, coordinates: Vec<C>,
phantom_t: std::marker::PhantomData<T>, phantom_t: std::marker::PhantomData<T>,
@ -170,31 +187,21 @@ pub struct Convert<T: FromBytes, const SIZE: usize, const DIM: usize, const META
meta: Vec<(String, FieldDatatype)>, meta: Vec<(String, FieldDatatype)>,
} }
/// X, Y, Z in f32 (float) without meta data.
pub type ConvertXYZ = Convert<f32, { size_of!(f32) }, 3, 0, PointXYZ>; pub type ConvertXYZ = Convert<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
/// X, Y, Z in f32 (float), meta: intensity in f32.
pub type ConvertXYZI = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZI>; pub type ConvertXYZI = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZI>;
/// X, Y, Z in f32 (float), meta: normal_x, normal_y, normal_z in f32.
pub type ConvertXYZNormal = Convert<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>; pub type ConvertXYZNormal = Convert<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>;
pub type ConvertXYZRGB = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZRGB>;
/// X, Y, Z in f32 (float), meta: r, g, b in u8. pub type ConvertXYZRGBL = Convert<f32, { size_of!(f32) }, 3, 2, PointXYZRGBL>;
pub type ConvertXYZRGB = Convert<f32, { size_of!(f32) }, 3, 3, PointXYZRGB>; pub type ConvertXYZRGBA = Convert<f32, { size_of!(f32) }, 3, 2, PointXYZRGBA>;
pub type ConvertXYZRGBNormal = Convert<f32, { size_of!(f32) }, 3, 4, PointXYZRGBNormal>;
/// X, Y, Z in f32 (float), meta: r, g, b, a in u8.
pub type ConvertXYZRGBA = Convert<f32, { size_of!(f32) }, 3, 4, PointXYZRGBA>;
/// X, Y, Z in f32 (float), meta: r, g, b in u8, normal_x, normal_y, normal_z in f32.
pub type ConvertXYZRGBNormal = Convert<f32, { size_of!(f32) }, 3, 6, PointXYZRGBNormal>;
/// X, Y, Z in f32 (float), meta: intensity in f32, normal_x, normal_y, normal_z in f32.
pub type ConvertXYZINormal = Convert<f32, { size_of!(f32) }, 3, 4, PointXYZINormal>; pub type ConvertXYZINormal = Convert<f32, { size_of!(f32) }, 3, 4, PointXYZINormal>;
/// X, Y, Z in f32 (float), meta: label in u32.
pub type ConvertXYZL = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZL>; pub type ConvertXYZL = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZL>;
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryFrom<Vec<C>> for Convert<T, SIZE, DIM, METADIM, C> impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryFrom<Vec<C>>
for Convert<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{ {
type Error = ConversionError; type Error = ConversionError;
@ -225,7 +232,13 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
let meta_names = C::meta_names(); let meta_names = C::meta_names();
let mut point_step_size = DIM * SIZE; let mut point_step_size = DIM * SIZE;
for (idx, value) in point_meta.1.iter().enumerate() { for (idx, value) in point_meta.1.iter().enumerate() {
meta.push((meta_names.get(idx).ok_or(ConversionError::MetaIndexLengthMismatch)?.clone(), value.datatype.clone())); meta.push((
meta_names
.get(idx)
.ok_or(ConversionError::MetaIndexLengthMismatch)?
.clone(),
value.datatype.clone(),
));
point_step_size += datatype_size(&value.datatype); point_step_size += datatype_size(&value.datatype);
} }
@ -291,9 +304,15 @@ impl PointMeta {
} }
} }
fn new_from_buffer(data: &Vec<u8>, offset: usize, datatype: &FieldDatatype) -> Result<Self, ConversionError> { fn new_from_buffer(
data: &Vec<u8>,
offset: usize,
datatype: &FieldDatatype,
) -> Result<Self, ConversionError> {
let size = datatype_size(datatype); let size = datatype_size(datatype);
let bytes = data.get(offset..offset + size).ok_or(ConversionError::DataLengthMismatch)?; let bytes = data
.get(offset..offset + size)
.ok_or(ConversionError::DataLengthMismatch)?;
let mut bytes_array = [0; size_of!(f64)]; // 8 bytes as f64 is the largest type let mut bytes_array = [0; size_of!(f64)]; // 8 bytes as f64 is the largest type
for (idx, byte) in bytes.iter().enumerate() { for (idx, byte) in bytes.iter().enumerate() {
bytes_array[idx] = *byte; bytes_array[idx] = *byte;
@ -325,7 +344,11 @@ impl PointMeta {
} }
} }
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryFrom<PointCloud2Msg> for Convert<T, SIZE, DIM, METADIM, C> impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryFrom<PointCloud2Msg>
for Convert<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{ {
type Error = ConversionError; type Error = ConversionError;
@ -359,9 +382,13 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
let mut has_y: Option<usize> = None; let mut has_y: Option<usize> = None;
let mut has_z: Option<usize> = None; let mut has_z: Option<usize> = None;
let mut meta_with_offsets: Vec<(String, FieldDatatype, usize)> = Vec::with_capacity(METADIM); let mut meta_with_offsets: Vec<(String, FieldDatatype, usize)> =
Vec::with_capacity(METADIM);
let lower_meta_names = C::meta_names().iter().map(|x| x.to_lowercase()).collect::<Vec<String>>(); let lower_meta_names = C::meta_names()
.iter()
.map(|x| x.to_lowercase())
.collect::<Vec<String>>();
for (idx, field) in cloud.fields.iter().enumerate() { for (idx, field) in cloud.fields.iter().enumerate() {
let lower_field_name = field.name.to_lowercase(); let lower_field_name = field.name.to_lowercase();
match lower_field_name.as_str() { match lower_field_name.as_str() {
@ -370,7 +397,11 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
"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) {
meta_with_offsets.push((field.name.clone(), convert_msg_code_to_type(field.datatype)?, field.offset as usize)); meta_with_offsets.push((
field.name.clone(),
convert_msg_code_to_type(field.datatype)?,
field.offset as usize,
));
} }
} }
} }
@ -378,31 +409,29 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
meta_with_offsets.sort_by(|a, b| a.2.cmp(&b.2)); meta_with_offsets.sort_by(|a, b| a.2.cmp(&b.2));
let meta_offsets: Vec<usize> = meta_with_offsets.iter().map(|x| x.2).collect(); let meta_offsets: Vec<usize> = meta_with_offsets.iter().map(|x| x.2).collect();
let meta: Vec<(String, FieldDatatype)> = meta_with_offsets.iter().map(|x| (x.0.clone(), x.1.clone())).collect(); let meta: Vec<(String, FieldDatatype)> = meta_with_offsets
.iter()
.map(|x| (x.0.clone(), x.1.clone()))
.collect();
let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?; let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?; let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?;
let mut offsets = vec![ let mut offsets = vec![x_field.offset as usize, y_field.offset as usize];
x_field.offset as usize,
y_field.offset as usize
];
let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type); let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type);
match z_field { match z_field {
Ok(z_field) => { Ok(z_field) => {
offsets.push(z_field.offset as usize); offsets.push(z_field.offset as usize);
},
Err(err) => {
match err {
ConversionError::NotEnoughFields => {
if DIM == 3 {
return Err(ConversionError::NotEnoughFields);
}
},
_ => return Err(err),
}
} }
Err(err) => match err {
ConversionError::NotEnoughFields => {
if DIM == 3 {
return Err(ConversionError::NotEnoughFields);
}
}
_ => return Err(err),
},
} }
let endian = if cloud.is_bigendian { let endian = if cloud.is_bigendian {
@ -463,7 +492,12 @@ fn datatype_size(datatype: &FieldDatatype) -> usize {
} }
} }
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryInto<PointCloud2Msg> for Convert<T, SIZE, DIM, METADIM, C> { impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryInto<PointCloud2Msg>
for Convert<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
type Error = ConversionError; type Error = ConversionError;
/// Convert the point cloud to a ROS message. /// Convert the point cloud to a ROS message.
@ -566,7 +600,10 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
Ok(meta) => meta, Ok(meta) => meta,
Err(_) => return Err(ConversionError::PointConversionError), Err(_) => return Err(ConversionError::PointConversionError),
}; };
coords_data.0.iter().for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); coords_data
.0
.iter()
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
coords_data.1.iter().for_each(|meta| { coords_data.1.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)]; let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)];
cloud.data.extend_from_slice(truncated_bytes); cloud.data.extend_from_slice(truncated_bytes);
@ -577,7 +614,11 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
} }
} }
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> Convert<T, SIZE, DIM, METADIM, C> impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
Convert<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{ {
/// Convenience getter for the number of points in the cloud. /// Convenience getter for the number of points in the cloud.
pub fn len(&self) -> usize { pub fn len(&self) -> usize {
@ -585,7 +626,11 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
} }
} }
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> fallible_iterator::FallibleIterator for Convert<T, SIZE, DIM, METADIM, C> impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
fallible_iterator::FallibleIterator for Convert<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{ {
type Item = C; type Item = C;
type Error = ConversionError; type Error = ConversionError;
@ -651,7 +696,6 @@ impl FromBytes for i8 {
} }
} }
impl FromBytes for i16 { impl FromBytes for i16 {
fn from_be_bytes(bytes: &[u8]) -> Self { fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]]) Self::from_be_bytes([bytes[0], bytes[1]])
@ -788,7 +832,6 @@ fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> Option<[u8; S]>
Some(buff) Some(buff)
} }
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::*; use super::*;
@ -804,5 +847,4 @@ mod tests {
f32::bytes(&1.0); f32::bytes(&1.0);
f64::bytes(&1.0); f64::bytes(&1.0);
} }
} }

257
src/pcl_utils.rs
View File

@ -1,5 +1,18 @@
use crate::{ConversionError, MetaNames, PointConvertible, PointMeta}; use crate::{ConversionError, MetaNames, PointConvertible, PointMeta};
#[inline]
fn pack_rgb(r: u8, g: u8, b: u8) -> f32 {
((r as u32) << 16) as f32 + ((g as u32) << 8) as f32 + (b as u32) as f32
}
#[inline]
fn unpack_rgb(rgb: f32) -> [u8; 3] {
let r: u8 = ((rgb as u32) >> 16) as u8;
let g: u8 = ((rgb as u32) >> 8) as u8;
let b: u8 = (rgb as u32) as u8;
[r, g, b]
}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates. /// This is a 3D point with x, y, z coordinates.
#[derive(Clone, Debug, PartialEq, Copy)] #[derive(Clone, Debug, PartialEq, Copy)]
@ -35,8 +48,7 @@ impl MetaNames<0> for PointXYZ {
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 0> for PointXYZ {} impl PointConvertible<f32, { size_of!(f32) }, 3, 0> for PointXYZ {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an intensity value. /// This is a 3D point with x, y, z coordinates and an intensity value.
@ -64,7 +76,7 @@ impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZI {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
intensity: data.1[0].get().unwrap(), intensity: data.1[0].get()?,
}) })
} }
} }
@ -75,7 +87,7 @@ impl MetaNames<1> for PointXYZI {
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for PointXYZI {} impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for PointXYZI {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an RGB color value. /// This is a 3D point with x, y, z coordinates and an RGB color value.
@ -89,40 +101,39 @@ pub struct PointXYZRGB {
pub b: u8, pub b: u8,
} }
impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZRGB { impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
type Error = ConversionError; type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> { fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
Ok(([self.x, self.y, self.z], [ let rgb = pack_rgb(self.r, self.g, self.b);
PointMeta::new(self.r), Ok(([self.x, self.y, self.z], [PointMeta::new(rgb)]))
PointMeta::new(self.g),
PointMeta::new(self.b),
]))
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZRGB { impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
type Error = ConversionError; type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 3])) -> Result<Self, Self::Error> { 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);
Ok(Self { Ok(Self {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
r: data.1[0].get().unwrap(), r: rgb_unpacked[0],
g: data.1[1].get().unwrap(), g: rgb_unpacked[1],
b: data.1[2].get().unwrap(), b: rgb_unpacked[2],
}) })
} }
} }
impl MetaNames<3> for PointXYZRGB { impl MetaNames<1> for PointXYZRGB {
fn meta_names() -> [String; 3] { fn meta_names() -> [String; 1] {
["r", "g", "b"].map(|s| s.to_string()) ["rgb"].map(|s| s.to_string())
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 3> for PointXYZRGB {} impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for PointXYZRGB {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an RGBA color value. /// This is a 3D point with x, y, z coordinates and an RGBA color value.
@ -138,47 +149,43 @@ pub struct PointXYZRGBA {
pub a: u8, pub a: u8,
} }
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBA { impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
type Error = ConversionError; type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> {
Ok(([self.x, self.y, self.z], [ let rgb = pack_rgb(self.r, self.g, self.b);
PointMeta::new(self.r), Ok((
PointMeta::new(self.g), [self.x, self.y, self.z],
PointMeta::new(self.b), [PointMeta::new(rgb), PointMeta::new(self.a)],
PointMeta::new(self.a), ))
]))
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBA { impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
type Error = ConversionError; type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result<Self, Self::Error> { 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);
Ok(Self { Ok(Self {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
r: data.1[0].get().unwrap(), r: rgb_unpacked[0],
g: data.1[1].get().unwrap(), g: rgb_unpacked[1],
b: data.1[2].get().unwrap(), b: rgb_unpacked[2],
a: data.1[3].get().unwrap(), a: data.1[1].get()?,
}) })
} }
} }
impl MetaNames<4> for PointXYZRGBA { impl MetaNames<2> for PointXYZRGBA {
fn meta_names() -> [String; 4] { fn meta_names() -> [String; 2] {
[ ["rgb", "a"].map(|s| s.to_string())
"r",
"g",
"b",
"a",
].map(|s| s.to_string())
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 4> for PointXYZRGBA {} impl PointConvertible<f32, { size_of!(f32) }, 3, 2> for PointXYZRGBA {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates, an RGB color value and a normal vector. /// This is a 3D point with x, y, z coordinates, an RGB color value and a normal vector.
@ -195,53 +202,50 @@ pub struct PointXYZRGBNormal {
pub normal_z: f32, pub normal_z: f32,
} }
impl TryInto<([f32; 3], [PointMeta; 6])> for PointXYZRGBNormal { impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
type Error = ConversionError; type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 6]), Self::Error> { fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok(([self.x, self.y, self.z], [ let rgb = pack_rgb(self.r, self.g, self.b);
PointMeta::new(self.r), Ok((
PointMeta::new(self.g), [self.x, self.y, self.z],
PointMeta::new(self.b), [
PointMeta::new(self.normal_x), PointMeta::new(rgb),
PointMeta::new(self.normal_y), PointMeta::new(self.normal_x),
PointMeta::new(self.normal_z), PointMeta::new(self.normal_y),
])) PointMeta::new(self.normal_z),
],
))
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 6])> for PointXYZRGBNormal { impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
type Error = ConversionError; type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 6])) -> Result<Self, Self::Error> { 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);
Ok(Self { Ok(Self {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
r: data.1[0].get().unwrap(), r: rgb_unpacked[0],
g: data.1[1].get().unwrap(), g: rgb_unpacked[1],
b: data.1[2].get().unwrap(), b: rgb_unpacked[2],
normal_x: data.1[3].get().unwrap(), normal_x: data.1[1].get()?,
normal_y: data.1[4].get().unwrap(), normal_y: data.1[2].get()?,
normal_z: data.1[5].get().unwrap(), normal_z: data.1[3].get()?,
}) })
} }
} }
impl MetaNames<6> for PointXYZRGBNormal { impl MetaNames<4> for PointXYZRGBNormal {
fn meta_names() -> [String; 6] { fn meta_names() -> [String; 4] {
[ ["rgb", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
"r",
"g",
"b",
"normal_x",
"normal_y",
"normal_z",
].map(|s| s.to_string())
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 6> for PointXYZRGBNormal {} impl PointConvertible<f32, { size_of!(f32) }, 3, 4> for PointXYZRGBNormal {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates, an intensity value and a normal vector. /// This is a 3D point with x, y, z coordinates, an intensity value and a normal vector.
@ -260,12 +264,15 @@ impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZINormal {
type Error = ConversionError; type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok(([self.x, self.y, self.z], [ Ok((
PointMeta::new(self.intensity), [self.x, self.y, self.z],
PointMeta::new(self.normal_x), [
PointMeta::new(self.normal_y), PointMeta::new(self.intensity),
PointMeta::new(self.normal_z), PointMeta::new(self.normal_x),
])) PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
],
))
} }
} }
@ -277,26 +284,21 @@ impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
intensity: data.1[0].get().unwrap(), intensity: data.1[0].get()?,
normal_x: data.1[1].get().unwrap(), normal_x: data.1[1].get()?,
normal_y: data.1[2].get().unwrap(), normal_y: data.1[2].get()?,
normal_z: data.1[3].get().unwrap(), normal_z: data.1[3].get()?,
}) })
} }
} }
impl MetaNames<4> for PointXYZINormal { impl MetaNames<4> for PointXYZINormal {
fn meta_names() -> [String; 4] { fn meta_names() -> [String; 4] {
[ ["intensity", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
"intensity",
"normal_x",
"normal_y",
"normal_z",
].map(|s| s.to_string())
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 4> for PointXYZINormal {} impl PointConvertible<f32, { size_of!(f32) }, 3, 4> for PointXYZINormal {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a label. /// This is a 3D point with x, y, z coordinates and a label.
@ -312,9 +314,7 @@ impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZL {
type Error = ConversionError; type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
Ok(([self.x, self.y, self.z], [ Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)]))
PointMeta::new(self.label),
]))
} }
} }
@ -326,7 +326,7 @@ impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
label: data.1[0].get().unwrap(), label: data.1[0].get()?,
}) })
} }
} }
@ -337,7 +337,7 @@ impl MetaNames<1> for PointXYZL {
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for PointXYZL {} impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for PointXYZL {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a label. /// This is a 3D point with x, y, z coordinates and a label.
@ -352,47 +352,43 @@ pub struct PointXYZRGBL {
pub label: u32, pub label: u32,
} }
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBL { impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
type Error = ConversionError; type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> {
Ok(([self.x, self.y, self.z], [ let rgb = pack_rgb(self.r, self.g, self.b);
PointMeta::new(self.r), Ok((
PointMeta::new(self.g), [self.x, self.y, self.z],
PointMeta::new(self.b), [PointMeta::new(rgb), PointMeta::new(self.label)],
PointMeta::new(self.label), ))
]))
} }
} }
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBL { impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
type Error = ConversionError; type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result<Self, Self::Error> { 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);
Ok(Self { Ok(Self {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
r: data.1[0].get().unwrap(), r: rgb_unpacked[0],
g: data.1[1].get().unwrap(), g: rgb_unpacked[1],
b: data.1[2].get().unwrap(), b: rgb_unpacked[2],
label: data.1[3].get().unwrap(), label: data.1[1].get()?,
}) })
} }
} }
impl MetaNames<4> for PointXYZRGBL { impl MetaNames<2> for PointXYZRGBL {
fn meta_names() -> [String; 4] { fn meta_names() -> [String; 2] {
[ ["rgb", "label"].map(|s| s.to_string())
"r",
"g",
"b",
"label",
].map(|s| s.to_string())
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 4> for PointXYZRGBL {} impl PointConvertible<f32, { size_of!(f32) }, 3, 2> for PointXYZRGBL {}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a normal vector. /// This is a 3D point with x, y, z coordinates and a normal vector.
@ -410,11 +406,14 @@ impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZNormal {
type Error = ConversionError; type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> { fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> {
Ok(([self.x, self.y, self.z], [ Ok((
PointMeta::new(self.normal_x), [self.x, self.y, self.z],
PointMeta::new(self.normal_y), [
PointMeta::new(self.normal_z), PointMeta::new(self.normal_x),
])) PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
],
))
} }
} }
@ -426,21 +425,17 @@ impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZNormal {
x: data.0[0], x: data.0[0],
y: data.0[1], y: data.0[1],
z: data.0[2], z: data.0[2],
normal_x: data.1[0].get().unwrap(), normal_x: data.1[0].get()?,
normal_y: data.1[1].get().unwrap(), normal_y: data.1[1].get()?,
normal_z: data.1[2].get().unwrap(), normal_z: data.1[2].get()?,
}) })
} }
} }
impl MetaNames<3> for PointXYZNormal { impl MetaNames<3> for PointXYZNormal {
fn meta_names() -> [String; 3] { fn meta_names() -> [String; 3] {
[ ["normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
"normal_x",
"normal_y",
"normal_z",
].map(|s| s.to_string())
} }
} }
impl PointConvertible<f32, {size_of!(f32)}, 3, 3> for PointXYZNormal {} impl PointConvertible<f32, { size_of!(f32) }, 3, 3> for PointXYZNormal {}

6
src/ros_types.rs
View File

@ -6,10 +6,7 @@ pub struct TimeMsg {
impl Default for TimeMsg { impl Default for TimeMsg {
fn default() -> Self { fn default() -> Self {
Self { Self { sec: 0, nsec: 0 }
sec: 0,
nsec: 0,
}
} }
} }
@ -143,4 +140,3 @@ impl Into<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
} }
} }
} }

553
tests/e2e_test.rs
View File

@ -1,13 +1,37 @@
use ros_pointcloud2::{ConversionError, Convert, ConvertXYZ, ConvertXYZINormal, ConvertXYZRGBA, ConvertXYZRGBNormal, MetaNames, PointConvertible, PointMeta};
use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::pcl_utils::{PointXYZ, PointXYZINormal, PointXYZRGBA, PointXYZRGBNormal};
use fallible_iterator::FallibleIterator; use fallible_iterator::FallibleIterator;
use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::pcl_utils::*;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::*;
use std::fmt::Debug;
use std::{cmp, fmt};
fn convert_from_into<C, P>(cloud: Vec<P>)
where
C: FallibleIterator<Item = P>
+ TryFrom<PointCloud2Msg>
+ TryFrom<Vec<P>>
+ TryInto<PointCloud2Msg>,
<C as FallibleIterator>::Error: Debug,
<C as TryFrom<PointCloud2Msg>>::Error: Debug,
<C as TryInto<PointCloud2Msg>>::Error: Debug,
<C as TryFrom<Vec<P>>>::Error: Debug,
P: Clone + fmt::Debug + cmp::PartialEq,
{
let copy = cloud.clone();
let msg: Result<PointCloud2Msg, _> = C::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_p_type = C::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<P>>();
assert_eq!(copy, back_to_type.unwrap());
}
#[test] #[test]
fn custom_xyz_f32() { fn custom_xyz_f32() {
const DIM : usize = 3; const DIM: usize = 3;
const METADIM : usize = 0; const METADIM: usize = 0;
#[derive(Debug, PartialEq, Clone)] #[derive(Debug, PartialEq, Clone)]
struct CustomPoint { struct CustomPoint {
@ -38,25 +62,29 @@ fn custom_xyz_f32() {
} }
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {} impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![ convert_from_into::<MyConverter, CustomPoint>(vec![
CustomPoint { x: 1.0, y: 2.0, z: 3.0 }, CustomPoint {
CustomPoint { x: 4.0, y: 5.0, z: 6.0 }, x: 1.0,
CustomPoint { x: 7.0, y: 8.0, z: 9.0 }, y: 2.0,
]; z: 3.0,
let copy = custom_cloud.clone(); },
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into(); CustomPoint {
assert!(custom_msg.is_ok()); x: 4.0,
let to_custom_type = MyConverter::try_from(custom_msg.unwrap()); y: 5.0,
assert!(to_custom_type.is_ok()); z: 6.0,
let to_custom_type = to_custom_type.unwrap().map(|point| Ok(point)).collect::<Vec<CustomPoint>>(); },
assert_eq!(copy, to_custom_type.unwrap()); CustomPoint {
x: 7.0,
y: 8.0,
z: 9.0,
},
]);
} }
#[test] #[test]
fn custom_xyzi_f32() { fn custom_xyzi_f32() {
const DIM : usize = 3; const DIM: usize = 3;
const METADIM : usize = 1; const METADIM: usize = 1;
#[derive(Debug, PartialEq, Clone)] #[derive(Debug, PartialEq, Clone)]
struct CustomPoint { struct CustomPoint {
x: f32, x: f32,
@ -87,26 +115,38 @@ fn custom_xyzi_f32() {
} }
} }
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {} impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![ convert_from_into::<MyConverter, CustomPoint>(vec![
CustomPoint { x: 0.0, y: 1.0, z: 5.0, i: 0 }, CustomPoint {
CustomPoint { x: 1.0, y: 1.5, z: 5.0, i: 1 }, x: 0.0,
CustomPoint { x: 1.3, y: 1.6, z: 5.7, i: 2 }, y: 1.0,
CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, i: u8::MAX }, z: 5.0,
]; i: 0,
let copy = custom_cloud.clone(); },
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into(); CustomPoint {
assert!(custom_msg.is_ok()); x: 1.0,
let to_custom_type = MyConverter::try_from(custom_msg.unwrap()); y: 1.5,
assert!(to_custom_type.is_ok()); z: 5.0,
let to_custom_type = to_custom_type.unwrap(); i: 1,
let back_to_type = to_custom_type.map(|point| Ok(point)).collect::<Vec<CustomPoint>>(); },
assert_eq!(copy, back_to_type.unwrap()); CustomPoint {
x: 1.3,
y: 1.6,
z: 5.7,
i: 2,
},
CustomPoint {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
i: u8::MAX,
},
]);
} }
#[test] #[test]
fn custom_rgba_f32() { fn custom_rgba_f32() {
const DIM : usize = 3; const DIM: usize = 3;
const METADIM : usize = 4; const METADIM: usize = 4;
#[derive(Debug, PartialEq, Clone)] #[derive(Debug, PartialEq, Clone)]
struct CustomPoint { struct CustomPoint {
x: f32, x: f32,
@ -120,7 +160,15 @@ fn custom_rgba_f32() {
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>; type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) { fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) {
([self.x, self.y, self.z], [PointMeta::new(self.r), PointMeta::new(self.g), PointMeta::new(self.b), PointMeta::new(self.a)]) (
[self.x, self.y, self.z],
[
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
PointMeta::new(self.a),
],
)
} }
} }
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
@ -143,95 +191,382 @@ fn custom_rgba_f32() {
} }
} }
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {} impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![ convert_from_into::<MyConverter, CustomPoint>(vec![
CustomPoint { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 }, CustomPoint {
CustomPoint { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, a: 1 }, x: 0.0,
CustomPoint { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, a: 2 }, y: 1.0,
CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX }, z: 5.0,
]; r: 0,
let copy = custom_cloud.clone(); g: 0,
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into(); b: 0,
assert!(custom_msg.is_ok()); a: 0,
let to_custom_type = MyConverter::try_from(custom_msg.unwrap()); },
assert!(to_custom_type.is_ok()); CustomPoint {
let to_custom_type = to_custom_type.unwrap(); x: 1.0,
let back_to_type = to_custom_type.map(|point| Ok(point)).collect::<Vec<CustomPoint>>(); y: 1.5,
assert_eq!(copy, back_to_type.unwrap()); z: 5.0,
r: 1,
g: 1,
b: 1,
a: 1,
},
CustomPoint {
x: 1.3,
y: 1.6,
z: 5.7,
r: 2,
g: 2,
b: 2,
a: 2,
},
CustomPoint {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
r: u8::MAX,
g: u8::MAX,
b: u8::MAX,
a: u8::MAX,
},
]);
} }
#[test] #[test]
fn converterxyz() { fn converterxyz() {
let cloud = vec![ let cloud = vec![
PointXYZ { x: 0.0, y: 1.0, z: 5.0 }, PointXYZ {
PointXYZ { x: 1.0, y: 1.5, z: 5.0 }, x: 0.0,
PointXYZ { x: 1.3, y: 1.6, z: 5.7 }, y: 1.0,
PointXYZ { x: f32::MAX, y: f32::MIN, z: f32::MAX }, 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,
},
]; ];
let copy = cloud.clone(); convert_from_into::<ConvertXYZ, PointXYZ>(cloud);
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZ::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyz_type = ConvertXYZ::try_from(msg.unwrap());
assert!(to_xyz_type.is_ok());
let to_xyz_type: ConvertXYZ = to_xyz_type.unwrap();
let back_to_type = to_xyz_type.map(|point: PointXYZ| Ok(point)).collect::<Vec<PointXYZ>>();
assert!(back_to_type.is_ok());
assert_eq!(copy, back_to_type.unwrap());
} }
#[test] #[test]
fn converterxyzrgba() { fn converterxyzrgba() {
let cloud = vec![ convert_from_into::<ConvertXYZRGBA, PointXYZRGBA>(vec![
PointXYZRGBA { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 }, PointXYZRGBA {
PointXYZRGBA { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, a: 1 }, x: 0.0,
PointXYZRGBA { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, a: 2 }, y: 1.0,
PointXYZRGBA { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX }, z: 5.0,
]; r: 0,
g: 0,
let copy = cloud.clone(); b: 0,
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZRGBA::try_from(cloud).unwrap().try_into(); a: 0,
assert!(msg.is_ok()); },
let to_xyzrgba_type = ConvertXYZRGBA::try_from(msg.unwrap()); PointXYZRGBA {
assert!(to_xyzrgba_type.is_ok()); x: 1.0,
let to_xyzrgba_type = to_xyzrgba_type.unwrap(); y: 1.5,
let back_to_type = to_xyzrgba_type.map(|point| Ok(point)).collect::<Vec<PointXYZRGBA>>(); z: 5.0,
assert_eq!(copy, back_to_type.unwrap()); r: 1,
g: 1,
b: 1,
a: 1,
},
PointXYZRGBA {
x: 1.3,
y: 1.6,
z: 5.7,
r: 2,
g: 2,
b: 2,
a: 2,
},
PointXYZRGBA {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
r: u8::MAX,
g: u8::MAX,
b: u8::MAX,
a: u8::MAX,
},
]);
} }
#[test] #[test]
fn converterxyzinormal() { fn converterxyzinormal() {
let cloud = vec![ convert_from_into::<ConvertXYZINormal, PointXYZINormal>(vec![
PointXYZINormal { x: 0.0, y: 1.0, z: 5.0, intensity: 0.0, normal_x: 0.0, normal_y: 0.0, normal_z: 0.0 }, PointXYZINormal {
PointXYZINormal { x: 1.0, y: 1.5, z: 5.0, intensity: 1.0, normal_x: 1.0, normal_y: 1.0, normal_z: 1.0 }, x: 0.0,
PointXYZINormal { x: 1.3, y: 1.6, z: 5.7, intensity: 2.0, normal_x: 2.0, normal_y: 2.0, normal_z: 2.0 }, y: 1.0,
PointXYZINormal { x: f32::MAX, y: f32::MIN, z: f32::MAX, intensity: f32::MAX, normal_x: f32::MAX, normal_y: f32::MAX, normal_z: f32::MAX }, z: 5.0,
]; intensity: 0.0,
normal_x: 0.0,
let copy = cloud.clone(); normal_y: 0.0,
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZINormal::try_from(cloud).unwrap().try_into(); normal_z: 0.0,
assert!(msg.is_ok()); },
let to_xyzinormal_type = ConvertXYZINormal::try_from(msg.unwrap()); PointXYZINormal {
assert!(to_xyzinormal_type.is_ok()); x: 1.0,
let to_xyzinormal_type = to_xyzinormal_type.unwrap(); y: 1.5,
let back_to_type = to_xyzinormal_type.map(|point| Ok(point)).collect::<Vec<PointXYZINormal>>(); z: 5.0,
assert_eq!(copy, back_to_type.unwrap()); intensity: 1.0,
normal_x: 1.0,
normal_y: 1.0,
normal_z: 1.0,
},
PointXYZINormal {
x: 1.3,
y: 1.6,
z: 5.7,
intensity: 2.0,
normal_x: 2.0,
normal_y: 2.0,
normal_z: 2.0,
},
PointXYZINormal {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
intensity: f32::MAX,
normal_x: f32::MAX,
normal_y: f32::MAX,
normal_z: f32::MAX,
},
]);
} }
#[test] #[test]
fn converterxyzrgbnormal() { fn converterxyzrgbnormal() {
let cloud = vec![ convert_from_into::<ConvertXYZRGBNormal, PointXYZRGBNormal>(vec![
PointXYZRGBNormal { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, normal_x: 0.0, normal_y: 0.0, normal_z: 0.0 }, PointXYZRGBNormal {
PointXYZRGBNormal { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, normal_x: 1.0, normal_y: 1.0, normal_z: 1.0 }, x: 0.0,
PointXYZRGBNormal { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, normal_x: 2.0, normal_y: 2.0, normal_z: 2.0 }, y: 1.0,
PointXYZRGBNormal { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, normal_x: f32::MAX, normal_y: f32::MAX, normal_z: f32::MAX }, z: 5.0,
]; r: 0,
g: 0,
let copy = cloud.clone(); b: 0,
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZRGBNormal::try_from(cloud).unwrap().try_into(); normal_x: 0.0,
assert!(msg.is_ok()); normal_y: 0.0,
let to_xyzrgbnormal_type = ConvertXYZRGBNormal::try_from(msg.unwrap()); normal_z: 0.0,
assert!(to_xyzrgbnormal_type.is_ok()); },
let to_xyzrgbnormal_type = to_xyzrgbnormal_type.unwrap(); PointXYZRGBNormal {
let back_to_type = to_xyzrgbnormal_type.map(|point| Ok(point)).collect::<Vec<PointXYZRGBNormal>>(); x: 1.0,
assert_eq!(copy, back_to_type.unwrap()); y: 1.5,
z: 5.0,
r: 1,
g: 1,
b: 1,
normal_x: 1.0,
normal_y: 1.0,
normal_z: 1.0,
},
PointXYZRGBNormal {
x: 1.3,
y: 1.6,
z: 5.7,
r: 2,
g: 2,
b: 2,
normal_x: 2.0,
normal_y: 2.0,
normal_z: 2.0,
},
PointXYZRGBNormal {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
r: u8::MAX,
g: u8::MAX,
b: u8::MAX,
normal_x: f32::MAX,
normal_y: f32::MAX,
normal_z: f32::MAX,
},
]);
}
#[test]
fn converterxyznormal() {
convert_from_into::<ConvertXYZNormal, PointXYZNormal>(vec![
PointXYZNormal {
x: 0.0,
y: 1.0,
z: 5.0,
normal_x: 0.0,
normal_y: 0.0,
normal_z: 0.0,
},
PointXYZNormal {
x: 1.0,
y: 1.5,
z: 5.0,
normal_x: 1.0,
normal_y: 1.0,
normal_z: 1.0,
},
PointXYZNormal {
x: 1.3,
y: 1.6,
z: 5.7,
normal_x: 2.0,
normal_y: 2.0,
normal_z: 2.0,
},
PointXYZNormal {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
normal_x: f32::MAX,
normal_y: f32::MAX,
normal_z: f32::MAX,
},
]);
}
#[test]
fn converterxyzrgbl() {
convert_from_into::<ConvertXYZRGBL, PointXYZRGBL>(vec![
PointXYZRGBL {
x: 0.0,
y: 1.0,
z: 5.0,
r: 0,
g: 0,
b: 0,
label: 0,
},
PointXYZRGBL {
x: 1.0,
y: 1.5,
z: 5.0,
r: 1,
g: 1,
b: 1,
label: 1,
},
PointXYZRGBL {
x: 1.3,
y: 1.6,
z: 5.7,
r: 2,
g: 2,
b: 2,
label: 2,
},
PointXYZRGBL {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
r: u8::MAX,
g: u8::MAX,
b: u8::MAX,
label: u32::MAX,
},
]);
}
#[test]
fn converterxyzrgb() {
convert_from_into::<ConvertXYZRGB, PointXYZRGB>(vec![
PointXYZRGB {
x: 0.0,
y: 1.0,
z: 5.0,
r: 0,
g: 0,
b: 0,
},
PointXYZRGB {
x: 1.0,
y: 1.5,
z: 5.0,
r: 1,
g: 1,
b: 1,
},
PointXYZRGB {
x: 1.3,
y: 1.6,
z: 5.7,
r: 2,
g: 2,
b: 2,
},
PointXYZRGB {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
r: u8::MAX,
g: u8::MAX,
b: u8::MAX,
},
]);
}
#[test]
fn converterxyzl() {
convert_from_into::<ConvertXYZL, PointXYZL>(vec![
PointXYZL {
x: 0.0,
y: 1.0,
z: 5.0,
label: 0,
},
PointXYZL {
x: 1.0,
y: 1.5,
z: 5.0,
label: 1,
},
PointXYZL {
x: 1.3,
y: 1.6,
z: 5.7,
label: 2,
},
PointXYZL {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
label: u32::MAX,
},
]);
}
#[test]
fn converterxyzi() {
convert_from_into::<ConvertXYZI, PointXYZI>(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,
},
]);
} }

24
tests/rosrust_msg_test.rs
View File

@ -1,20 +1,32 @@
#[cfg(feature = "rosrust_msg")] #[cfg(feature = "rosrust_msg")]
#[test] #[test]
fn convertxyz_rosrust_msg() { fn convertxyz_rosrust_msg() {
use ros_pointcloud2::{ConvertXYZ}; use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::PointXYZ; use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg; use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::fallible_iterator::FallibleIterator; use ros_pointcloud2::ConvertXYZ;
let cloud = vec![ let cloud = vec![
PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, PointXYZ {
PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, x: 1.0,
PointXYZ { x: 7.0, y: 8.0, z: 9.0 }, y: 2.0,
z: 3.0,
},
PointXYZ {
x: 4.0,
y: 5.0,
z: 6.0,
},
PointXYZ {
x: 7.0,
y: 8.0,
z: 9.0,
},
]; ];
let copy = cloud.clone(); let copy = cloud.clone();
let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().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 = ConvertXYZ::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());