461 lines
11 KiB
Rust
461 lines
11 KiB
Rust
use ros_pointcloud2::prelude::*;
|
|
|
|
#[cfg(feature = "derive")]
|
|
use std::fmt::Debug;
|
|
|
|
macro_rules! convert_from_into {
|
|
($point:ty, $cloud:expr) => {
|
|
convert_from_into_in_out_cloud!($cloud, $point, $cloud, $point);
|
|
};
|
|
}
|
|
|
|
#[cfg(feature = "derive")]
|
|
macro_rules! convert_from_into_vec {
|
|
($point:ty, $cloud:expr) => {
|
|
convert_from_into_in_out_cloud_vec!($cloud, $point, $cloud, $point);
|
|
};
|
|
}
|
|
|
|
macro_rules! convert_from_into_in_out_cloud {
|
|
($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
|
|
let msg = PointCloud2Msg::try_from_iter($in_cloud.clone().into_iter());
|
|
assert!(msg.is_ok(), "{:?}", msg);
|
|
let msg = msg.unwrap();
|
|
let to_p_type = msg.try_into_iter();
|
|
assert!(to_p_type.is_ok());
|
|
let to_p_type = to_p_type.unwrap();
|
|
let back_to_type = to_p_type.collect::<Vec<$out_point>>();
|
|
let orig_cloud: Vec<$out_point> = $out_cloud.iter().cloned().collect();
|
|
assert_eq!(orig_cloud, back_to_type);
|
|
};
|
|
}
|
|
|
|
#[cfg(feature = "derive")]
|
|
macro_rules! convert_from_into_in_out_cloud_vec {
|
|
($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
|
|
let msg = PointCloud2Msg::try_from_vec($in_cloud.clone());
|
|
assert!(msg.is_ok(), "{:?}", msg);
|
|
let msg = msg.unwrap();
|
|
let to_p_type = msg.try_into_iter();
|
|
assert!(to_p_type.is_ok());
|
|
let to_p_type = to_p_type.unwrap();
|
|
let back_to_type = to_p_type.collect::<Vec<$out_point>>();
|
|
let orig_cloud: Vec<$out_point> = $out_cloud.iter().cloned().collect();
|
|
assert_eq!(orig_cloud, back_to_type);
|
|
};
|
|
}
|
|
|
|
#[test]
|
|
fn write_cloud() {
|
|
let cloud = vec![
|
|
PointXYZ::new(0.0, 1.0, 5.0),
|
|
PointXYZ::new(1.0, 1.5, 5.0),
|
|
PointXYZ::new(1.3, 1.6, 5.7),
|
|
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
|
];
|
|
|
|
let msg = PointCloud2Msg::try_from_iter(cloud);
|
|
assert!(msg.is_ok());
|
|
}
|
|
|
|
#[test]
|
|
#[cfg(feature = "derive")]
|
|
fn write_cloud_from_vec() {
|
|
let cloud = vec![
|
|
PointXYZ::new(0.0, 1.0, 5.0),
|
|
PointXYZ::new(1.0, 1.5, 5.0),
|
|
PointXYZ::new(1.3, 1.6, 5.7),
|
|
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
|
];
|
|
|
|
let msg = PointCloud2Msg::try_from_vec(cloud);
|
|
assert!(msg.is_ok());
|
|
|
|
let msg = msg.unwrap();
|
|
println!("{:?}", msg);
|
|
}
|
|
|
|
#[test]
|
|
#[cfg(feature = "derive")]
|
|
fn custom_xyz_f32() {
|
|
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
|
|
#[repr(C)]
|
|
struct CustomPoint {
|
|
x: f32,
|
|
y: f32,
|
|
z: f32,
|
|
}
|
|
|
|
convert_from_into!(
|
|
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]
|
|
#[cfg(feature = "derive")]
|
|
fn custom_xyzi_f32() {
|
|
let cloud: Vec<CustomPointXYZI> = vec![
|
|
CustomPointXYZI {
|
|
x: 0.0,
|
|
y: 1.0,
|
|
z: 5.0,
|
|
i: 0,
|
|
},
|
|
CustomPointXYZI {
|
|
x: 1.0,
|
|
y: 1.5,
|
|
z: 5.0,
|
|
i: 1,
|
|
},
|
|
CustomPointXYZI {
|
|
x: 1.3,
|
|
y: 1.6,
|
|
z: 5.7,
|
|
i: 2,
|
|
},
|
|
CustomPointXYZI {
|
|
x: f32::MAX,
|
|
y: f32::MIN,
|
|
z: f32::MAX,
|
|
i: u8::MAX,
|
|
},
|
|
];
|
|
|
|
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
|
|
#[repr(C)]
|
|
struct CustomPointXYZI {
|
|
x: f32,
|
|
y: f32,
|
|
z: f32,
|
|
i: u8,
|
|
}
|
|
|
|
convert_from_into!(CustomPointXYZI, cloud);
|
|
}
|
|
|
|
#[test]
|
|
#[cfg(feature = "derive")]
|
|
fn custom_rgba_f32() {
|
|
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
|
|
#[repr(C)]
|
|
struct CustomPoint {
|
|
x: f32,
|
|
y: f32,
|
|
z: f32,
|
|
r: u8,
|
|
g: u8,
|
|
b: u8,
|
|
a: u8,
|
|
}
|
|
|
|
let 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,
|
|
},
|
|
];
|
|
convert_from_into!(CustomPoint, cloud);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyz() {
|
|
let cloud = vec![
|
|
PointXYZ::new(0.0, 1.0, 5.0),
|
|
PointXYZ::new(1.0, 1.5, 5.0),
|
|
PointXYZ::new(1.3, 1.6, 5.7),
|
|
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
|
];
|
|
|
|
convert_from_into!(PointXYZ, cloud);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyzrgba() {
|
|
convert_from_into!(
|
|
PointXYZRGBA,
|
|
vec![
|
|
PointXYZRGBA::new(0.0, 1.0, 5.0, 0, 0, 0, 0),
|
|
PointXYZRGBA::new(1.0, 1.5, 5.0, 1, 1, 1, 1),
|
|
PointXYZRGBA::new(1.3, 1.6, 5.7, 2, 2, 2, 2),
|
|
PointXYZRGBA::new(
|
|
f32::MAX,
|
|
f32::MIN,
|
|
f32::MAX,
|
|
u8::MAX,
|
|
u8::MAX,
|
|
u8::MAX,
|
|
u8::MAX
|
|
),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyzinormal() {
|
|
convert_from_into!(
|
|
PointXYZINormal,
|
|
vec![
|
|
PointXYZINormal::new(0.0, 1.0, 5.0, 0.0, 0.0, 0.0, 0.0),
|
|
PointXYZINormal::new(1.0, 1.5, 5.0, 1.0, 1.0, 1.0, 1.0),
|
|
PointXYZINormal::new(1.3, 1.6, 5.7, 2.0, 2.0, 2.0, 2.0),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyzrgbnormal() {
|
|
convert_from_into!(
|
|
PointXYZRGBNormal,
|
|
vec![
|
|
PointXYZRGBNormal::new(0.0, 1.0, 5.0, RGB::new(0, 0, 0), 0.0, 0.0, 0.0),
|
|
PointXYZRGBNormal::new(1.0, 1.5, 5.0, RGB::new(1, 1, 1), 1.0, 1.0, 1.0),
|
|
PointXYZRGBNormal::new(1.3, 1.6, 5.7, RGB::new(2, 2, 2), 2.0, 2.0, 2.0),
|
|
PointXYZRGBNormal::new(
|
|
f32::MAX,
|
|
f32::MIN,
|
|
f32::MAX,
|
|
RGB::new(u8::MAX, u8::MAX, u8::MAX),
|
|
f32::MAX,
|
|
f32::MAX,
|
|
f32::MAX,
|
|
),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyznormal() {
|
|
convert_from_into!(
|
|
PointXYZNormal,
|
|
vec![
|
|
PointXYZNormal::new(0.0, 1.0, 5.0, 0.0, 0.0, 0.0),
|
|
PointXYZNormal::new(1.0, 1.5, 5.0, 1.0, 1.0, 1.0),
|
|
PointXYZNormal::new(1.3, 1.6, 5.7, 2.0, 2.0, 2.0),
|
|
PointXYZNormal::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX, f32::MAX, f32::MAX),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyzrgbl() {
|
|
convert_from_into!(
|
|
PointXYZRGBL,
|
|
vec![
|
|
PointXYZRGBL::new(0.0, 1.0, 5.0, 0, 0, 0, 0),
|
|
PointXYZRGBL::new(1.0, 1.5, 5.0, 1, 1, 1, 1),
|
|
PointXYZRGBL::new(1.3, 1.6, 5.7, 2, 2, 2, 2),
|
|
PointXYZRGBL::new(
|
|
f32::MAX,
|
|
f32::MIN,
|
|
f32::MAX,
|
|
u8::MAX,
|
|
u8::MAX,
|
|
u8::MAX,
|
|
u32::MAX
|
|
),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyzrgb() {
|
|
convert_from_into!(
|
|
PointXYZRGB,
|
|
vec![
|
|
PointXYZRGB::new(0.0, 1.0, 5.0, 0, 0, 0),
|
|
PointXYZRGB::new(1.0, 1.5, 5.0, 1, 1, 1),
|
|
PointXYZRGB::new(1.3, 1.6, 5.7, 2, 2, 2),
|
|
PointXYZRGB::new(f32::MAX, f32::MIN, f32::MAX, u8::MAX, u8::MAX, u8::MAX),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
#[cfg(feature = "derive")]
|
|
fn converterxyzrgb_from_vec() {
|
|
convert_from_into_vec!(
|
|
PointXYZRGB,
|
|
vec![
|
|
PointXYZRGB::new(0.0, 1.0, 5.0, 0, 0, 0),
|
|
PointXYZRGB::new(1.3, 1.6, 5.7, 2, 2, 2),
|
|
PointXYZRGB::new(f32::MAX, f32::MIN, f32::MAX, u8::MAX, u8::MAX, u8::MAX),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyzl() {
|
|
convert_from_into!(
|
|
PointXYZL,
|
|
vec![
|
|
PointXYZL::new(0.0, 1.0, 5.0, 0),
|
|
PointXYZL::new(1.0, 1.5, 5.0, 1),
|
|
PointXYZL::new(1.3, 1.6, 5.7, 2),
|
|
PointXYZL::new(f32::MAX, f32::MIN, f32::MAX, u32::MAX),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn converterxyzi() {
|
|
convert_from_into!(
|
|
PointXYZI,
|
|
vec![
|
|
PointXYZI::new(0.0, 1.0, 5.0, 0.0),
|
|
PointXYZI::new(1.0, 1.5, 5.0, 1.0),
|
|
PointXYZI::new(1.3, 1.6, 5.7, 2.0),
|
|
PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
|
|
]
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn write_xyzi_read_xyz() {
|
|
let write_cloud = vec![
|
|
PointXYZI::new(0.0, 1.0, 5.0, 0.0),
|
|
PointXYZI::new(1.0, 1.5, 5.0, 1.0),
|
|
PointXYZI::new(1.3, 1.6, 5.7, 2.0),
|
|
PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
|
|
];
|
|
|
|
let read_cloud = [
|
|
PointXYZ::new(0.0, 1.0, 5.0),
|
|
PointXYZ::new(1.0, 1.5, 5.0),
|
|
PointXYZ::new(1.3, 1.6, 5.7),
|
|
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
|
];
|
|
|
|
convert_from_into_in_out_cloud!(write_cloud, PointXYZI, read_cloud, PointXYZ);
|
|
}
|
|
|
|
#[cfg(feature = "derive")]
|
|
#[test]
|
|
fn write_less_than_available() {
|
|
#[derive(Debug, PartialEq, Clone, Default, TypeLayout)]
|
|
#[repr(C)]
|
|
struct CustomPoint {
|
|
x: f32,
|
|
y: f32,
|
|
z: f32,
|
|
dummy: f32,
|
|
}
|
|
|
|
impl From<RPCL2Point<3>> for CustomPoint {
|
|
fn from(point: RPCL2Point<3>) -> Self {
|
|
Self {
|
|
x: point[0].get(),
|
|
y: point[1].get(),
|
|
z: point[2].get(),
|
|
dummy: 0.0,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl From<CustomPoint> for RPCL2Point<3> {
|
|
fn from(point: CustomPoint) -> Self {
|
|
[point.x.into(), point.y.into(), point.z.into()].into()
|
|
}
|
|
}
|
|
|
|
impl Fields<3> for CustomPoint {
|
|
fn field_names_ordered() -> [&'static str; 3] {
|
|
["x", "y", "z"]
|
|
}
|
|
}
|
|
|
|
impl PointConvertible<3> 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 = [
|
|
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!(write_cloud, CustomPoint, read_cloud, CustomPoint);
|
|
}
|