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]
name = "ros_pointcloud2"
version = "0.2.0"
version = "0.2.1"
edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"]
description = "Customizable conversions for working with sensor_msgs/PointCloud2."

12
README.md
View File

@ -1,13 +1,13 @@
# 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
[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.
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`.
Please avoid cloning the `data: Vec<u8>` of the message.
Try to avoid cloning the `data: Vec<u8>` field.
```rust
use ros_pointcloud2::ros_types::PointCloud2Msg;
@ -73,13 +73,13 @@ impl From<YourROSPointCloud2> for PointCloud2Msg {
## 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)
You can use one by enabling the corresponding feature.
```toml
[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.

144
src/lib.rs
View File

@ -2,9 +2,9 @@
pub mod pcl_utils;
pub mod ros_types;
use num_traits::Zero;
use crate::pcl_utils::*;
use crate::ros_types::{PointCloud2Msg, PointFieldMsg};
use num_traits::Zero;
#[macro_use]
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.
/// 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).
#[derive(Clone, Debug, PartialEq, Copy)]
@ -56,7 +63,9 @@ impl GetFieldDatatype for f32 {
}
impl GetFieldDatatype for f64 {
fn field_datatype() -> FieldDatatype { FieldDatatype::F64 }
fn field_datatype() -> FieldDatatype {
FieldDatatype::F64
}
}
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 {
Some(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);
}
Ok(field.clone())
},
}
None => {
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)
/// 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,
coordinates: Vec<C>,
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)>,
}
/// X, Y, Z in f32 (float) without meta data.
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>;
/// 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>;
/// X, Y, Z in f32 (float), meta: r, g, b in u8.
pub type ConvertXYZRGB = Convert<f32, { size_of!(f32) }, 3, 3, PointXYZRGB>;
/// 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 ConvertXYZRGB = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZRGB>;
pub type ConvertXYZRGBL = Convert<f32, { size_of!(f32) }, 3, 2, PointXYZRGBL>;
pub type ConvertXYZRGBA = Convert<f32, { size_of!(f32) }, 3, 2, PointXYZRGBA>;
pub type ConvertXYZRGBNormal = Convert<f32, { size_of!(f32) }, 3, 4, PointXYZRGBNormal>;
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>;
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;
@ -225,7 +232,13 @@ impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C:
let meta_names = C::meta_names();
let mut point_step_size = DIM * SIZE;
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);
}
@ -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 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
for (idx, byte) in bytes.iter().enumerate() {
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;
@ -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_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() {
let lower_field_name = field.name.to_lowercase();
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),
_ => {
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));
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 y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?;
let mut offsets = vec![
x_field.offset as usize,
y_field.offset as usize
];
let mut offsets = vec![x_field.offset as usize, y_field.offset as usize];
let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type);
match z_field {
Ok(z_field) => {
offsets.push(z_field.offset as usize);
},
Err(err) => {
match err {
}
Err(err) => match err {
ConversionError::NotEnoughFields => {
if DIM == 3 {
return Err(ConversionError::NotEnoughFields);
}
},
}
_ => return Err(err),
}
}
},
}
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;
/// 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,
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| {
let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)];
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.
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 Error = ConversionError;
@ -651,7 +696,6 @@ impl FromBytes for i8 {
}
}
impl FromBytes for i16 {
fn from_be_bytes(bytes: &[u8]) -> Self {
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)
}
#[cfg(test)]
mod tests {
use super::*;
@ -804,5 +847,4 @@ mod tests {
f32::bytes(&1.0);
f64::bytes(&1.0);
}
}

237
src/pcl_utils.rs
View File

@ -1,5 +1,18 @@
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.
/// This is a 3D point with x, y, z coordinates.
#[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.
/// 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],
y: data.0[1],
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.
/// 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,
}
impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZRGB {
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
]))
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; 3])> for PointXYZRGB {
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
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 {
x: data.0[0],
y: data.0[1],
z: data.0[2],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
})
}
}
impl MetaNames<3> for PointXYZRGB {
fn meta_names() -> [String; 3] {
["r", "g", "b"].map(|s| s.to_string())
impl MetaNames<1> for PointXYZRGB {
fn meta_names() -> [String; 1] {
["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.
/// 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,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBA {
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
PointMeta::new(self.a),
]))
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; 4])> for PointXYZRGBA {
impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
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 {
x: data.0[0],
y: data.0[1],
z: data.0[2],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
a: data.1[3].get().unwrap(),
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
a: data.1[1].get()?,
})
}
}
impl MetaNames<4> for PointXYZRGBA {
fn meta_names() -> [String; 4] {
[
"r",
"g",
"b",
"a",
].map(|s| s.to_string())
impl MetaNames<2> for PointXYZRGBA {
fn meta_names() -> [String; 2] {
["rgb", "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.
/// 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,
}
impl TryInto<([f32; 3], [PointMeta; 6])> for PointXYZRGBNormal {
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 6]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
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; 6])> for PointXYZRGBNormal {
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
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 {
x: data.0[0],
y: data.0[1],
z: data.0[2],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
normal_x: data.1[3].get().unwrap(),
normal_y: data.1[4].get().unwrap(),
normal_z: data.1[5].get().unwrap(),
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()?,
})
}
}
impl MetaNames<6> for PointXYZRGBNormal {
fn meta_names() -> [String; 6] {
[
"r",
"g",
"b",
"normal_x",
"normal_y",
"normal_z",
].map(|s| s.to_string())
impl MetaNames<4> for PointXYZRGBNormal {
fn meta_names() -> [String; 4] {
["rgb", "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.
/// 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;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok(([self.x, self.y, self.z], [
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),
]))
],
))
}
}
@ -277,26 +284,21 @@ impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal {
x: data.0[0],
y: data.0[1],
z: data.0[2],
intensity: data.1[0].get().unwrap(),
normal_x: data.1[1].get().unwrap(),
normal_y: data.1[2].get().unwrap(),
normal_z: data.1[3].get().unwrap(),
intensity: data.1[0].get()?,
normal_x: data.1[1].get()?,
normal_y: data.1[2].get()?,
normal_z: data.1[3].get()?,
})
}
}
impl MetaNames<4> for PointXYZINormal {
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.
/// 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;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.label),
]))
Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)]))
}
}
@ -326,7 +326,7 @@ impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL {
x: data.0[0],
y: data.0[1],
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.
/// This is a 3D point with x, y, z coordinates and a label.
@ -352,47 +352,43 @@ pub struct PointXYZRGBL {
pub label: u32,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBL {
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
PointMeta::new(self.label),
]))
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; 4])> for PointXYZRGBL {
impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
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 {
x: data.0[0],
y: data.0[1],
z: data.0[2],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
label: data.1[3].get().unwrap(),
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
label: data.1[1].get()?,
})
}
}
impl MetaNames<4> for PointXYZRGBL {
fn meta_names() -> [String; 4] {
[
"r",
"g",
"b",
"label",
].map(|s| s.to_string())
impl MetaNames<2> for PointXYZRGBL {
fn meta_names() -> [String; 2] {
["rgb", "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.
/// 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;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> {
Ok(([self.x, self.y, self.z], [
Ok((
[self.x, self.y, self.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],
y: data.0[1],
z: data.0[2],
normal_x: data.1[0].get().unwrap(),
normal_y: data.1[1].get().unwrap(),
normal_z: data.1[2].get().unwrap(),
normal_x: data.1[0].get()?,
normal_y: data.1[1].get()?,
normal_z: data.1[2].get()?,
})
}
}
impl MetaNames<3> for PointXYZNormal {
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 {
fn default() -> Self {
Self {
sec: 0,
nsec: 0,
}
Self { 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 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]
fn custom_xyz_f32() {
const DIM : usize = 3;
const METADIM : usize = 0;
const DIM: usize = 3;
const METADIM: usize = 0;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
@ -38,25 +62,29 @@ fn custom_xyz_f32() {
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![
CustomPoint { x: 1.0, y: 2.0, z: 3.0 },
CustomPoint { x: 4.0, y: 5.0, z: 6.0 },
CustomPoint { x: 7.0, y: 8.0, z: 9.0 },
];
let copy = custom_cloud.clone();
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
let to_custom_type = to_custom_type.unwrap().map(|point| Ok(point)).collect::<Vec<CustomPoint>>();
assert_eq!(copy, to_custom_type.unwrap());
convert_from_into::<MyConverter, CustomPoint>(vec![
CustomPoint {
x: 1.0,
y: 2.0,
z: 3.0,
},
CustomPoint {
x: 4.0,
y: 5.0,
z: 6.0,
},
CustomPoint {
x: 7.0,
y: 8.0,
z: 9.0,
},
]);
}
#[test]
fn custom_xyzi_f32() {
const DIM : usize = 3;
const METADIM : usize = 1;
const DIM: usize = 3;
const METADIM: usize = 1;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32,
@ -87,26 +115,38 @@ fn custom_xyzi_f32() {
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![
CustomPoint { x: 0.0, y: 1.0, z: 5.0, i: 0 },
CustomPoint { x: 1.0, y: 1.5, z: 5.0, i: 1 },
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 },
];
let copy = custom_cloud.clone();
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
let to_custom_type = to_custom_type.unwrap();
let back_to_type = to_custom_type.map(|point| Ok(point)).collect::<Vec<CustomPoint>>();
assert_eq!(copy, back_to_type.unwrap());
convert_from_into::<MyConverter, CustomPoint>(vec![
CustomPoint {
x: 0.0,
y: 1.0,
z: 5.0,
i: 0,
},
CustomPoint {
x: 1.0,
y: 1.5,
z: 5.0,
i: 1,
},
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]
fn custom_rgba_f32() {
const DIM : usize = 3;
const METADIM : usize = 4;
const DIM: usize = 3;
const METADIM: usize = 4;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32,
@ -120,7 +160,15 @@ fn custom_rgba_f32() {
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
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 {
@ -143,95 +191,382 @@ fn custom_rgba_f32() {
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![
CustomPoint { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 },
CustomPoint { x: 1.0, y: 1.5, 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 },
];
let copy = custom_cloud.clone();
let custom_msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
let to_custom_type = to_custom_type.unwrap();
let back_to_type = to_custom_type.map(|point| Ok(point)).collect::<Vec<CustomPoint>>();
assert_eq!(copy, back_to_type.unwrap());
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 {
x: 1.0,
y: 1.5,
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]
fn converterxyz() {
let 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 },
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,
},
];
let copy = cloud.clone();
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());
convert_from_into::<ConvertXYZ, PointXYZ>(cloud);
}
#[test]
fn converterxyzrgba() {
let cloud = vec![
PointXYZRGBA { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 },
PointXYZRGBA { x: 1.0, y: 1.5, z: 5.0, 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 },
];
let copy = cloud.clone();
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZRGBA::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzrgba_type = ConvertXYZRGBA::try_from(msg.unwrap());
assert!(to_xyzrgba_type.is_ok());
let to_xyzrgba_type = to_xyzrgba_type.unwrap();
let back_to_type = to_xyzrgba_type.map(|point| Ok(point)).collect::<Vec<PointXYZRGBA>>();
assert_eq!(copy, back_to_type.unwrap());
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 {
x: 1.0,
y: 1.5,
z: 5.0,
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]
fn converterxyzinormal() {
let cloud = 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 { x: 1.0, y: 1.5, z: 5.0, 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 },
];
let copy = cloud.clone();
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZINormal::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzinormal_type = ConvertXYZINormal::try_from(msg.unwrap());
assert!(to_xyzinormal_type.is_ok());
let to_xyzinormal_type = to_xyzinormal_type.unwrap();
let back_to_type = to_xyzinormal_type.map(|point| Ok(point)).collect::<Vec<PointXYZINormal>>();
assert_eq!(copy, back_to_type.unwrap());
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 {
x: 1.0,
y: 1.5,
z: 5.0,
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]
fn converterxyzrgbnormal() {
let cloud = 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 { 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 },
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 },
];
let copy = cloud.clone();
let msg: Result<ros_pointcloud2::ros_types::PointCloud2Msg, _> = ConvertXYZRGBNormal::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzrgbnormal_type = ConvertXYZRGBNormal::try_from(msg.unwrap());
assert!(to_xyzrgbnormal_type.is_ok());
let to_xyzrgbnormal_type = to_xyzrgbnormal_type.unwrap();
let back_to_type = to_xyzrgbnormal_type.map(|point| Ok(point)).collect::<Vec<PointXYZRGBNormal>>();
assert_eq!(copy, back_to_type.unwrap());
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 {
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,
},
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,
},
]);
}

22
tests/rosrust_msg_test.rs
View File

@ -1,15 +1,27 @@
#[cfg(feature = "rosrust_msg")]
#[test]
fn convertxyz_rosrust_msg() {
use ros_pointcloud2::{ConvertXYZ};
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::ConvertXYZ;
let cloud = vec![
PointXYZ { x: 1.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 },
PointXYZ {
x: 1.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 internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap();