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

add iterator impl

This commit is contained in:
stelzo 2024-04-26 21:50:23 +02:00
parent 0105a76f53
commit 6f55407a20
No known key found for this signature in database
GPG Key ID: FC4EF89052319374
8 changed files with 1247 additions and 1078 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Cargo.toml
View File

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

349
src/convert.rs Normal file
View File

@ -0,0 +1,349 @@
use crate::*;
/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html).
#[derive(Clone, Debug, PartialEq, Copy)]
pub enum FieldDatatype {
F32,
F64,
I32,
U8,
U16,
U32,
I8,
I16,
}
impl Default for FieldDatatype {
fn default() -> Self {
FieldDatatype::F32
}
}
/// Getter trait for the datatype of a field value.
pub trait GetFieldDatatype {
fn field_datatype() -> FieldDatatype;
}
impl GetFieldDatatype for f32 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::F32
}
}
impl GetFieldDatatype for f64 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::F64
}
}
impl GetFieldDatatype for i32 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::I32
}
}
impl GetFieldDatatype for u8 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::U8
}
}
impl GetFieldDatatype for u16 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::U16
}
}
impl GetFieldDatatype for u32 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::U32
}
}
impl GetFieldDatatype for i8 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::I8
}
}
impl GetFieldDatatype for i16 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::I16
}
}
pub(crate) fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 {
match geo_type {
FieldDatatype::I8 => 1,
FieldDatatype::U8 => 2,
FieldDatatype::I16 => 3,
FieldDatatype::U16 => 4,
FieldDatatype::I32 => 5,
FieldDatatype::U32 => 6,
FieldDatatype::F32 => 7,
FieldDatatype::F64 => 8,
}
}
pub(crate) fn convert_msg_code_to_type(code: u8) -> Result<FieldDatatype, ConversionError> {
match code {
7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64),
5 => Ok(FieldDatatype::I32),
2 => Ok(FieldDatatype::U8),
4 => Ok(FieldDatatype::U16),
6 => Ok(FieldDatatype::U32),
1 => Ok(FieldDatatype::I8),
3 => Ok(FieldDatatype::I16),
_ => Err(ConversionError::UnsupportedFieldType),
}
}
pub(crate) fn check_coord(
coord: Option<usize>,
fields: &[PointFieldMsg],
xyz_field_type: &FieldDatatype,
) -> Result<PointFieldMsg, ConversionError> {
match coord {
Some(y_idx) => {
let field = &fields[y_idx];
if field.datatype != convert_to_msg_code(xyz_field_type) {
return Err(ConversionError::InvalidFieldFormat);
}
Ok(field.clone())
}
None => Err(ConversionError::NotEnoughFields),
}
}
/// Matching field names from each meta data per point to the PointField name.
/// Always make sure to use the same order as in your Into<> implementation to have a correct mapping.
pub trait MetaNames<const METADIM: usize> {
fn meta_names() -> [String; METADIM];
}
pub(crate) fn datatype_size(datatype: &FieldDatatype) -> usize {
match datatype {
FieldDatatype::U8 => 1,
FieldDatatype::U16 => 2,
FieldDatatype::U32 => 4,
FieldDatatype::I8 => 1,
FieldDatatype::I16 => 2,
FieldDatatype::I32 => 4,
FieldDatatype::F32 => 4,
FieldDatatype::F64 => 8,
}
}
#[inline(always)]
pub(crate) fn add_point_to_byte_buffer<
T: FromBytes,
const SIZE: usize,
const DIM: usize,
const METADIM: usize,
C: PointConvertible<T, SIZE, DIM, METADIM>,
>(
coords: C,
cloud: &mut PointCloud2Msg,
) -> Result<bool, ConversionError> {
let (coords_data, coords_meta): ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() {
Ok(meta) => meta,
Err(_) => return Err(ConversionError::PointConversionError),
};
// (x, y, z...)
coords_data
.iter()
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
// meta data description
coords_meta.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)];
cloud.data.extend_from_slice(truncated_bytes);
});
cloud.width += 1;
Ok(true)
}
/// This trait is used to convert a byte slice to a primitive type.
/// All PointField types are supported.
pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype {
fn from_be_bytes(bytes: &[u8]) -> Self;
fn from_le_bytes(bytes: &[u8]) -> Self;
fn bytes(_: &Self) -> Vec<u8>;
}
impl FromBytes for i8 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
fn bytes(x: &i8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i16 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
fn bytes(x: &i16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u16 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
fn bytes(x: &u16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u32 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn bytes(x: &u32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for f32 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn bytes(x: &f32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i32 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn bytes(x: &i32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for f64 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
fn bytes(x: &f64) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u8 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
fn bytes(x: &u8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
#[derive(Clone, Debug, PartialEq)]
pub(crate) enum Endianness {
Big,
Little,
}
impl Default for Endianness {
fn default() -> Self {
Endianness::Little
}
}
pub(crate) fn load_loadable<T: FromBytes, const SIZE: usize>(
start_idx: usize,
data: &[u8],
endian: &Endianness,
) -> Option<T> {
match endian {
Endianness::Big => Some(T::from_be_bytes(
load_bytes::<SIZE>(start_idx, data)?.as_slice(),
)),
Endianness::Little => Some(T::from_le_bytes(
load_bytes::<SIZE>(start_idx, data)?.as_slice(),
)),
}
}
fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> Option<[u8; S]> {
if start_idx + S > data.len() {
return None;
}
let mut buff: [u8; S] = [0; S];
for (byte, buff_val) in buff.iter_mut().enumerate().take(S) {
let raw_byte = data.get(start_idx + byte);
match raw_byte {
None => {
return None;
}
Some(some_byte) => {
*buff_val = *some_byte;
}
}
}
Some(buff)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn from_bytes() {
i8::bytes(&1);
u8::bytes(&1);
i16::bytes(&1);
u16::bytes(&1);
i32::bytes(&1);
u32::bytes(&1);
f32::bytes(&1.0);
f64::bytes(&1.0);
}
}

761
src/lib.rs
View File

@ -1,15 +1,30 @@
// TODO test reading writing different types
// TODO read less info from the point cloud than is available
// TODO write less info to the point cloud than is available
// TODO explain templating arguments more for reader and writer
#![doc = include_str!("../README.md")]
pub mod convert;
pub mod pcl_utils;
pub mod ros_types;
#[macro_export]
macro_rules! size_of {
($t:ty) => {
std::mem::size_of::<$t>()
};
}
pub mod reader;
pub mod writer;
pub use ros_types::PointCloud2Msg;
use crate::convert::*;
use crate::pcl_utils::*;
use crate::ros_types::{PointCloud2Msg, PointFieldMsg};
use num_traits::Zero;
use crate::ros_types::PointFieldMsg;
#[macro_use]
pub extern crate mem_macros;
pub extern crate fallible_iterator;
pub use fallible_iterator::FallibleIterator;
/// Errors that can occur when converting between PointCloud2 and a custom type.
#[derive(Debug)]
@ -33,228 +48,12 @@ pub trait PointConvertible<T, const SIZE: usize, const DIM: usize, const METADIM
+ TryFrom<([T; DIM], [PointMeta; METADIM])>
+ MetaNames<METADIM>
+ Clone
+ 'static
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)]
pub enum FieldDatatype {
F32,
F64,
I32,
U8,
U16,
U32,
I8,
I16,
}
/// Getter trait for the datatype of a field value.
pub trait GetFieldDatatype {
fn field_datatype() -> FieldDatatype;
}
impl GetFieldDatatype for f32 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::F32
}
}
impl GetFieldDatatype for f64 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::F64
}
}
impl GetFieldDatatype for i32 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::I32
}
}
impl GetFieldDatatype for u8 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::U8
}
}
impl GetFieldDatatype for u16 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::U16
}
}
impl GetFieldDatatype for u32 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::U32
}
}
impl GetFieldDatatype for i8 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::I8
}
}
impl GetFieldDatatype for i16 {
fn field_datatype() -> FieldDatatype {
FieldDatatype::I16
}
}
fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 {
match geo_type {
FieldDatatype::I8 => 1,
FieldDatatype::U8 => 2,
FieldDatatype::I16 => 3,
FieldDatatype::U16 => 4,
FieldDatatype::I32 => 5,
FieldDatatype::U32 => 6,
FieldDatatype::F32 => 7,
FieldDatatype::F64 => 8,
}
}
fn convert_msg_code_to_type(code: u8) -> Result<FieldDatatype, ConversionError> {
match code {
7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64),
5 => Ok(FieldDatatype::I32),
2 => Ok(FieldDatatype::U8),
4 => Ok(FieldDatatype::U16),
6 => Ok(FieldDatatype::U32),
1 => Ok(FieldDatatype::I8),
3 => Ok(FieldDatatype::I16),
_ => Err(ConversionError::UnsupportedFieldType),
}
}
fn check_coord(
coord: Option<usize>,
fields: &[PointFieldMsg],
xyz_field_type: &FieldDatatype,
) -> Result<PointFieldMsg, ConversionError> {
match coord {
Some(y_idx) => {
let field = &fields[y_idx];
if field.datatype != convert_to_msg_code(xyz_field_type) {
return Err(ConversionError::InvalidFieldFormat);
}
Ok(field.clone())
}
None => Err(ConversionError::NotEnoughFields),
}
}
/// Matching field names from each meta data per point to the PointField name.
/// Always make sure to use the same order as in your Into<> implementation to have a correct mapping.
pub trait MetaNames<const METADIM: usize> {
fn meta_names() -> [String; METADIM];
}
/// The Convert struct is used to convert between a PointCloud2 message and a custom type.
/// A custom type must implement the FromBytes trait and the Into trait.
/// The Into trait is used to convert the custom type into a tuple of the coordinates and the meta data.
/// The FromBytes trait is used to convert the bytes from the PointCloud2 message into the custom type.
///
/// # Example
/// ```
/// use ros_pointcloud2::mem_macros::size_of;
/// use ros_pointcloud2::{Convert, PointMeta};
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// const DIM : usize = 3; // how many dimensions your point has (e.g. x, y, z)
/// 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>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
iteration: usize,
coordinates: Vec<C>,
phantom_t: std::marker::PhantomData<T>,
data: Vec<u8>,
point_step_size: usize,
cloud_length: usize,
offsets: Vec<usize>,
big_endian: Endianness,
xyz_field_type: FieldDatatype,
meta: Vec<(String, FieldDatatype)>,
}
pub type ConvertXYZ = Convert<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
pub type ConvertXYZI = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZI>;
pub type ConvertXYZNormal = Convert<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>;
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>;
pub type ConvertXYZL = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZL>;
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;
/// Converts a vector of custom types into a Convert struct that implements the Iterator trait.
///
/// # Example
/// ```
/// use ros_pointcloud2::{ConvertXYZ, ConversionError};
/// use ros_pointcloud2::pcl_utils::PointXYZ;
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
/// let convert: Result<ConvertXYZ, ConversionError> = ConvertXYZ::try_from(cloud_points);
/// ```
fn try_from(cloud: Vec<C>) -> Result<Self, Self::Error> {
let length = cloud.len();
let mut meta: Vec<(String, FieldDatatype)> = vec![];
let first_point = cloud.first().ok_or(ConversionError::NoPoints)?;
let point_meta: ([T; DIM], [PointMeta; METADIM]) = match first_point.clone().try_into() {
Ok(point_meta) => point_meta,
Err(_) => {
return Err(ConversionError::PointConversionError);
}
};
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,
));
point_step_size += datatype_size(&value.datatype);
}
Ok(Self {
phantom_t: std::marker::PhantomData,
iteration: usize::zero(),
coordinates: cloud,
data: Vec::new(),
point_step_size,
cloud_length: length,
offsets: Vec::new(),
big_endian: Endianness::Little,
xyz_field_type: T::field_datatype(),
meta,
})
}
}
/// Meta data representation for a point
///
/// This struct is used to store meta data in a fixed size byte buffer along the with the
@ -270,14 +69,14 @@ where
/// ```
#[derive(Debug, Clone, Copy)]
pub struct PointMeta {
bytes: [u8; size_of!(f64)],
bytes: [u8; std::mem::size_of::<f64>()],
datatype: FieldDatatype,
}
impl Default for PointMeta {
fn default() -> Self {
Self {
bytes: [0; size_of!(f64)],
bytes: [0; std::mem::size_of::<f64>()],
datatype: FieldDatatype::F32,
}
}
@ -292,7 +91,7 @@ impl PointMeta {
/// ```
pub fn new<T: FromBytes>(value: T) -> Self {
let raw_bytes = T::bytes(&value);
let mut bytes = [0; size_of!(f64)];
let mut bytes = [0; std::mem::size_of::<f64>()];
for (idx, byte) in raw_bytes.iter().enumerate() {
bytes[idx] = *byte;
}
@ -311,7 +110,7 @@ impl PointMeta {
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; std::mem::size_of::<f64>()]; // 8 bytes as f64 is the largest type
for (idx, byte) in bytes.iter().enumerate() {
bytes_array[idx] = *byte;
}
@ -341,511 +140,3 @@ impl PointMeta {
}
}
}
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;
/// Converts a ROS PointCloud2 message into a Convert struct that implements the Iterator trait.
/// Iterate over the struct to get or use the points.
///
/// # Example
/// Since we do not have a ROS node here, we first create a PointCloud2 message and then convert back to a Convert struct.
/// ```
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// use ros_pointcloud2::ConvertXYZ;
/// use ros_pointcloud2::pcl_utils::PointXYZ;
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
/// let msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
///
/// let convert: ConvertXYZ = ConvertXYZ::try_from(msg).unwrap(); // parse message
/// ```
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
if cloud.fields.len() < DIM {
return Err(ConversionError::NotEnoughFields);
}
let xyz_field_type = T::field_datatype();
let mut has_x: Option<usize> = None;
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 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() {
"x" => has_x = Some(idx),
"y" => has_y = Some(idx),
"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.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))
.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 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 {
ConversionError::NotEnoughFields => {
if DIM == 3 {
return Err(ConversionError::NotEnoughFields);
}
}
_ => return Err(err),
},
}
let endian = if cloud.is_bigendian {
Endianness::Big
} else {
Endianness::Little
};
if offsets.len() != DIM {
return Err(ConversionError::NotEnoughFields);
}
offsets.extend(meta_offsets);
if offsets.len() != DIM + METADIM {
return Err(ConversionError::NotEnoughFields);
}
let point_step_size = cloud.point_step as usize;
if point_step_size * cloud.width as usize * cloud.height as usize != cloud.data.len() {
return Err(ConversionError::DataLengthMismatch);
}
if let Some(max_offset) = offsets.last() {
if let Some(last_meta) = meta.last() {
let size_with_last_meta = max_offset + datatype_size(&last_meta.1);
if size_with_last_meta > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
}
}
Ok(Self {
phantom_t: std::marker::PhantomData,
iteration: usize::zero(),
coordinates: Vec::new(),
data: cloud.data,
point_step_size,
cloud_length: cloud.width as usize * cloud.height as usize,
offsets,
big_endian: endian,
xyz_field_type: T::field_datatype(),
meta,
})
}
}
fn datatype_size(datatype: &FieldDatatype) -> usize {
match datatype {
FieldDatatype::U8 => 1,
FieldDatatype::U16 => 2,
FieldDatatype::U32 => 4,
FieldDatatype::I8 => 1,
FieldDatatype::I16 => 2,
FieldDatatype::I32 => 4,
FieldDatatype::F32 => 4,
FieldDatatype::F64 => 8,
}
}
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.
/// First use the `try_from` method for initializing the conversion and parsing meta data.
/// Then use the `try_into` method to do the actual conversion.
///
/// # Example
/// ```
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// use ros_pointcloud2::ConvertXYZ;
/// use ros_pointcloud2::pcl_utils::PointXYZ;
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
/// let msg_out: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
/// ```
fn try_into(self) -> Result<PointCloud2Msg, Self::Error> {
let mut cloud = PointCloud2Msg::default();
// Define the message fields
let mut fields = Vec::new();
if DIM > 3 {
return Err(ConversionError::TooManyDimensions);
}
let datatype: u8 = convert_to_msg_code(&self.xyz_field_type);
if DIM > 1 {
fields.push(PointFieldMsg {
name: "x".to_string(),
offset: 0,
datatype,
count: 1,
});
fields.push(PointFieldMsg {
name: "y".to_string(),
offset: SIZE as u32,
datatype,
count: 1,
});
}
if DIM == 3 {
fields.push(PointFieldMsg {
name: "z".to_string(),
offset: 2 * SIZE as u32,
datatype,
count: 1,
});
}
// meta data fields
let first_point = self.coordinates.first().ok_or(ConversionError::NoPoints)?;
let meta: ([T; DIM], [PointMeta; METADIM]) = match first_point.clone().try_into() {
Ok(meta) => meta,
Err(_) => return Err(ConversionError::PointConversionError),
};
let meta_names = C::meta_names();
let sized_dim = DIM as u32 * SIZE as u32;
for (idx, value) in meta.1.iter().enumerate() {
let datatype: u8 = convert_to_msg_code(&value.datatype);
let mut offset = sized_dim;
for i in 0..idx {
let datatype: u8 = convert_to_msg_code(&meta.1[i].datatype);
let field_type = convert_msg_code_to_type(datatype)?;
let field_size = datatype_size(&field_type);
offset += field_size as u32;
}
fields.push(PointFieldMsg {
name: meta_names[idx].to_string(),
offset,
datatype,
count: 1,
});
}
// calc all meta data points step size
let mut step_size = 0;
for field in fields.iter() {
let field_type = convert_msg_code_to_type(field.datatype)?;
let field_size = datatype_size(&field_type);
step_size += field.count * field_size as u32;
}
cloud.fields = fields;
cloud.point_step = step_size;
cloud.row_step = self.coordinates.len() as u32 * cloud.point_step;
cloud.height = 1;
cloud.width = self.coordinates.len() as u32;
cloud.is_bigendian = false;
cloud.is_dense = true;
for coords in self.coordinates {
let coords_data: ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() {
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.1.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)];
cloud.data.extend_from_slice(truncated_bytes);
});
}
Ok(cloud)
}
}
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 {
self.cloud_length
}
pub fn is_empty(&self) -> bool {
self.cloud_length == 0
}
}
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;
/// Iterate over the data buffer and interpret the data as a point.
fn next(&mut self) -> Result<Option<Self::Item>, Self::Error> {
if self.iteration >= self.cloud_length {
return Ok(None); // iteration finished
}
let mut xyz: [T; DIM] = [T::zero(); DIM];
let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM];
for (idx, in_point_offset) in self.offsets.iter().enumerate() {
if idx < DIM {
match load_loadable::<T, SIZE>(
(self.iteration * self.point_step_size) + in_point_offset,
&self.data,
self.big_endian.clone(),
) {
Some(c) => xyz[idx] = c,
None => return Err(ConversionError::EndOfBuffer),
}
} else {
let meta_idx = idx - DIM;
let meta_type = self.meta[meta_idx].1;
let start = (self.iteration * self.point_step_size) + in_point_offset;
if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) {
meta[meta_idx] = m;
} else {
return Err(ConversionError::MetaIndexLengthMismatch); // since we can not return an error in the iterator, we finish the iteration here early since the last point is not valid. This case is not expected since we catch it while parsing the file.
}
}
}
self.iteration += 1;
let conv = C::try_from((xyz, meta)); // try convert the point
match conv {
Err(_) => Err(ConversionError::PointConversionError),
Ok(tuple) => Ok(Some(tuple)),
}
}
}
/// This trait is used to convert a byte slice to a primitive type.
/// All PointField types are supported.
pub trait FromBytes: Zero + Sized + Copy + GetFieldDatatype {
fn from_be_bytes(bytes: &[u8]) -> Self;
fn from_le_bytes(bytes: &[u8]) -> Self;
fn bytes(_: &Self) -> Vec<u8>;
}
impl FromBytes for i8 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
fn bytes(x: &i8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i16 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
fn bytes(x: &i16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u16 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
fn bytes(x: &u16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u32 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn bytes(x: &u32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for f32 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn bytes(x: &f32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i32 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
fn bytes(x: &i32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for f64 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
fn bytes(x: &f64) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u8 {
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
fn bytes(x: &u8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
#[derive(Clone, Debug, PartialEq)]
enum Endianness {
Big,
Little,
}
fn load_loadable<T: FromBytes, const SIZE: usize>(
start_idx: usize,
data: &[u8],
endian: Endianness,
) -> Option<T> {
match endian {
Endianness::Big => Some(T::from_be_bytes(
load_bytes::<SIZE>(start_idx, data)?.as_slice(),
)),
Endianness::Little => Some(T::from_le_bytes(
load_bytes::<SIZE>(start_idx, data)?.as_slice(),
)),
}
}
fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> Option<[u8; S]> {
if start_idx + S > data.len() {
return None;
}
let mut buff: [u8; S] = [u8::zero(); S];
for (byte, buff_val) in buff.iter_mut().enumerate().take(S) {
let raw_byte = data.get(start_idx + byte);
match raw_byte {
None => {
return None;
}
Some(some_byte) => {
*buff_val = *some_byte;
}
}
}
Some(buff)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn from_bytes() {
i8::bytes(&1);
u8::bytes(&1);
i16::bytes(&1);
u16::bytes(&1);
i32::bytes(&1);
u32::bytes(&1);
f32::bytes(&1.0);
f64::bytes(&1.0);
}
}

18
src/pcl_utils.rs
View File

@ -50,7 +50,7 @@ impl MetaNames<0> for PointXYZ {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 0> for PointXYZ {}
impl PointConvertible<f32, { std::mem::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.
@ -89,7 +89,7 @@ impl MetaNames<1> for PointXYZI {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for PointXYZI {}
impl PointConvertible<f32, { std::mem::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.
@ -135,7 +135,7 @@ impl MetaNames<1> for PointXYZRGB {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for PointXYZRGB {}
impl PointConvertible<f32, { std::mem::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.
@ -187,7 +187,7 @@ impl MetaNames<2> for PointXYZRGBA {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 2> for PointXYZRGBA {}
impl PointConvertible<f32, { std::mem::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.
@ -247,7 +247,7 @@ impl MetaNames<4> for PointXYZRGBNormal {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 4> for PointXYZRGBNormal {}
impl PointConvertible<f32, { std::mem::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.
@ -300,7 +300,7 @@ impl MetaNames<4> for PointXYZINormal {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 4> for PointXYZINormal {}
impl PointConvertible<f32, { std::mem::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.
@ -339,7 +339,7 @@ impl MetaNames<1> for PointXYZL {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 1> for PointXYZL {}
impl PointConvertible<f32, { std::mem::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.
@ -390,7 +390,7 @@ impl MetaNames<2> for PointXYZRGBL {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 2> for PointXYZRGBL {}
impl PointConvertible<f32, { std::mem::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.
@ -440,4 +440,4 @@ impl MetaNames<3> for PointXYZNormal {
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 3> for PointXYZNormal {}
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 3> for PointXYZNormal {}

316
src/reader.rs Normal file
View File

@ -0,0 +1,316 @@
use crate::*;
/// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`).
/// Every additional meta data is ignored.
pub type ReaderXYZ = Reader<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
/// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`).
/// Every additional meta data is ignored.
pub type ReaderXYZI = Reader<f32, { size_of!(f32) }, 3, 1, PointXYZI>;
/// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`).
/// Every additional meta data is ignored.
pub type ReaderXYZNormal = Reader<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb (see `PointXYZRGB`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGB = Reader<f32, { size_of!(f32) }, 3, 1, PointXYZRGB>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb and intensity (see `PointXYZRGBL`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGBL = Reader<f32, { size_of!(f32) }, 3, 2, PointXYZRGBL>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb with additional alpha channel (see `PointXYZRGBA`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGBA = Reader<f32, { size_of!(f32) }, 3, 2, PointXYZRGBA>;
/// Provides the message as an Iterator over xyz coordinates and normal and unpacked rgb (see `PointXYZRGBNormal`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGBNormal = Reader<f32, { size_of!(f32) }, 3, 4, PointXYZRGBNormal>;
/// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
/// Every additional meta data is ignored.
pub type ReaderXYZINormal = Reader<f32, { size_of!(f32) }, 3, 4, PointXYZINormal>;
/// Provides the message as an Iterator over xyz coordinates and intensity with additional label (see `PointXYZL`).
/// Every additional meta data is ignored.
/// The label is typically used for semantic segmentation.
pub type ReaderXYZL = Reader<f32, { size_of!(f32) }, 3, 1, PointXYZL>;
/// The Reader provides an abstraction around PointCloud2Msg by implementing a fallible iterator around the data buffer.
///
/// The iterator is defined at compile time, so the Point has to be described via template arguments.
/// At runtime, the iterator catches read errors from the Msg type which typically only occur with corrupted data.
///
/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
///
/// ROS1 with rosrust:
/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ROS2 with r2r:
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
///
/// # Example
/// ```
/// use ros_pointcloud2::{
/// FallibleIterator, // needed for for_each and the other iterator methods
/// reader::ReaderXYZ,
/// writer::WriterXYZ,
/// PointCloud2Msg,
/// pcl_utils::PointXYZ,
/// };
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
///
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// let convert = ReaderXYZ::try_from(msg).unwrap(); // parse message
///
/// convert.for_each(|point: PointXYZ| {
/// // do something with the point
/// Ok(()) // return Ok to continue iteration
/// })
/// .unwrap(); // handle point conversion errors
/// ```
pub struct Reader<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
where
T: FromBytes,
C: MetaNames<METADIM>,
{
iteration: usize,
data: Vec<u8>,
point_step_size: usize,
cloud_length: usize,
offsets: Vec<usize>,
meta: Vec<(String, FieldDatatype)>,
endianness: Endianness,
phantom_t: std::marker::PhantomData<T>, // internally used for byte and datatype conversions
phantom_c: std::marker::PhantomData<C>, // internally used for meta names array
}
/// The iterator implementation for the Reader struct.
/// The iterator is fallible because the data is read from a byte buffer inside the PointCloud2 message, which is inherently unsafe.
///
/// See ConversionError for possible errors that can occur during iteration.
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> FallibleIterator
for Reader<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
type Item = C;
type Error = ConversionError;
/// The size_hint is the length of the remaining elements and the maximum length of the iterator.
///
/// PointCloud2 messages contain the length of the cloud, so we can prepare coming operations.
/// This hint is used inside common iterator functions like `collect<Vec<_>>`, for example.
fn size_hint(&self) -> (usize, Option<usize>) {
(self.cloud_length - self.iteration, Some(self.cloud_length))
}
/// Get the data from the byte buffer and convert it to the predefined point.
/// It must keep track of the current iteration and the length of the cloud so it has to mutate self.
///
/// The current point is then converted into the custom type. If the conversion fails, an error is returned.
fn next(&mut self) -> Result<Option<Self::Item>, Self::Error> {
if self.iteration >= self.cloud_length {
return Ok(None); // iteration finished
}
let mut xyz: [T; DIM] = [T::default(); DIM];
let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM];
for (idx, in_point_offset) in self.offsets.iter().enumerate() {
if idx < DIM {
match load_loadable::<T, SIZE>(
(self.iteration * self.point_step_size) + in_point_offset,
&self.data,
&self.endianness,
) {
Some(c) => xyz[idx] = c,
None => return Err(ConversionError::EndOfBuffer),
}
} else {
let meta_idx = idx - DIM;
let meta_type = self.meta[meta_idx].1;
let start = (self.iteration * self.point_step_size) + in_point_offset;
if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) {
meta[meta_idx] = m;
} else {
return Err(ConversionError::MetaIndexLengthMismatch);
}
}
}
self.iteration += 1;
match C::try_from((xyz, meta)) {
Err(_) => Err(ConversionError::PointConversionError),
Ok(tuple) => Ok(Some(tuple)),
}
}
}
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryFrom<PointCloud2Msg>
for Reader<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: MetaNames<METADIM>,
{
type Error = ConversionError;
/// Convert a PointCloud2Msg into a Reader.
/// Converting a PointCloud2Msg into a Reader is a fallible operation since the message can contain only a subset of the required fields.
///
/// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
/// It therefore has effectively a constant time complexity O(1) for practical purposes.
///
/// # Example
/// ```
/// use ros_pointcloud2::{
/// FallibleIterator, // needed for for_each and the other iterator methods
/// reader::ReaderXYZ,
/// writer::WriterXYZ,
/// PointCloud2Msg,
/// pcl_utils::PointXYZ,
/// };
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
///
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
///
/// let convert = ReaderXYZ::try_from(msg).unwrap();
/// // ^^^^^^^^ conversion from PointCloud2Msg to Reader
/// ```
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
if cloud.fields.len() < DIM {
return Err(ConversionError::NotEnoughFields);
}
let xyz_field_type = T::field_datatype();
let mut has_x: Option<usize> = None;
let mut has_y: Option<usize> = None;
let mut has_z: Option<usize> = None;
let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); METADIM];
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() {
"x" => has_x = Some(idx),
"y" => has_y = Some(idx),
"z" => has_z = Some(idx),
_ => {
if lower_meta_names.contains(&lower_field_name) {
println!("{} {} {}", idx, field.name, meta_with_offsets.len());
match meta_with_offsets.get_mut(idx - DIM) {
None => return Err(ConversionError::MetaIndexLengthMismatch),
Some(data) => {
data.0 = field.name.clone();
data.1 = convert_msg_code_to_type(field.datatype)?;
data.2 = field.offset as usize;
}
}
}
}
}
}
meta_with_offsets.sort_unstable_by(|a, b| a.2.cmp(&b.2));
let mut meta_offsets = [usize::default(); METADIM];
let mut meta: Vec<(String, FieldDatatype)> =
vec![(String::default(), FieldDatatype::U8); METADIM];
meta_with_offsets
.into_iter()
.enumerate()
.for_each(|(idx, (name, datatype, offset))| {
unsafe {
// all arrays have size METADIM
*meta.get_unchecked_mut(idx) = (name, datatype);
*meta_offsets.get_unchecked_mut(idx) = offset;
};
});
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 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 {
ConversionError::NotEnoughFields => {
if DIM == 3 {
return Err(ConversionError::NotEnoughFields);
}
}
_ => return Err(err),
},
}
let endian = if cloud.is_bigendian {
Endianness::Big
} else {
Endianness::Little
};
if offsets.len() != DIM {
return Err(ConversionError::NotEnoughFields);
}
offsets.extend(meta_offsets);
if offsets.len() != DIM + METADIM {
return Err(ConversionError::NotEnoughFields);
}
let point_step_size = cloud.point_step as usize;
if point_step_size * cloud.width as usize * cloud.height as usize != cloud.data.len() {
return Err(ConversionError::DataLengthMismatch);
}
if let Some(max_offset) = offsets.last() {
if let Some(last_meta) = meta.last() {
let size_with_last_meta = max_offset + datatype_size(&last_meta.1);
if size_with_last_meta > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
}
}
Ok(Self {
iteration: 0,
data: cloud.data,
point_step_size: point_step_size,
cloud_length: cloud.width as usize * cloud.height as usize,
offsets: offsets,
meta: meta,
endianness: endian,
phantom_t: std::marker::PhantomData,
phantom_c: std::marker::PhantomData,
})
}
}

171
src/writer.rs Normal file
View File

@ -0,0 +1,171 @@
use crate::*;
/// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`).
pub type WriterXYZ = Writer<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`).
pub type WriterXYZI = Writer<f32, { size_of!(f32) }, 3, 1, PointXYZI>;
/// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`).
pub type WriterXYZNormal = Writer<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`).
pub type WriterXYZRGB = Writer<f32, { size_of!(f32) }, 3, 1, PointXYZRGB>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`).
pub type WriterXYZRGBL = Writer<f32, { size_of!(f32) }, 3, 2, PointXYZRGBL>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels and alpha channel (see `PointXYZRGBA`).
pub type WriterXYZRGBA = Writer<f32, { size_of!(f32) }, 3, 2, PointXYZRGBA>;
/// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`).
pub type WriterXYZRGBNormal = Writer<f32, { size_of!(f32) }, 3, 4, PointXYZRGBNormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
pub type WriterXYZINormal = Writer<f32, { size_of!(f32) }, 3, 4, PointXYZINormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`).
pub type WriterXYZL = Writer<f32, { size_of!(f32) }, 3, 1, PointXYZL>;
// eats an iterator, can only write from iterators to messages, so must not implement iterator pattern!
pub struct Writer<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
coordinates: Box<dyn Iterator<Item = C>>, // internal iterator
xyz_field_type: FieldDatatype,
phantom_t: std::marker::PhantomData<T>,
}
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryInto<PointCloud2Msg>
for Writer<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.
/// First use the `try_from` method for initializing the conversion and parsing meta data.
/// Then use the `try_into` method to do the actual conversion.
///
/// # Example
/// ```
/// use ros_pointcloud2::ros_types::PointCloud2Msg;
/// use ros_pointcloud2::ConvertXYZ;
/// use ros_pointcloud2::pcl_utils::PointXYZ;
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
/// let msg_out: PointCloud2Msg = WriterXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
/// ```
fn try_into(mut self) -> Result<PointCloud2Msg, Self::Error> {
if DIM > 3 {
return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time?
}
let datatype: u8 = convert_to_msg_code(&self.xyz_field_type);
let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack
if DIM > 1 {
fields.push(PointFieldMsg {
name: "x".to_string(),
offset: 0,
datatype,
count: 1,
});
fields.push(PointFieldMsg {
name: "y".to_string(),
offset: SIZE as u32,
datatype,
count: 1,
});
}
if DIM == 3 {
fields.push(PointFieldMsg {
name: "z".to_string(),
offset: 2 * SIZE as u32,
datatype,
count: 1,
});
}
let mut cloud = PointCloud2Msg::default();
// self was created using a point iterator
let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
let meta: [PointMeta; METADIM] = match first_point.clone().try_into() {
Ok(meta) => meta.1,
Err(_) => return Err(ConversionError::PointConversionError),
};
let meta_names = C::meta_names();
let sized_dim = DIM as u32 * SIZE as u32;
for (idx, value) in meta.iter().enumerate() {
let datatype: u8 = convert_to_msg_code(&value.datatype);
let mut offset = sized_dim;
for i in 0..idx {
let datatype: u8 = convert_to_msg_code(&meta[i].datatype);
let field_type = convert_msg_code_to_type(datatype)?;
let field_size = datatype_size(&field_type);
offset += field_size as u32;
}
fields.push(PointFieldMsg {
name: meta_names
.get(idx)
.ok_or(ConversionError::MetaIndexLengthMismatch)?
.clone(),
offset,
datatype,
count: 1,
});
}
// calculate point size inside byte buffer
cloud.point_step = fields.iter().fold(0, |acc, field| {
let field_type = convert_msg_code_to_type(field.datatype).unwrap();
let field_size = datatype_size(&field_type);
acc + field.count * field_size as u32
});
// actual point -> byte conversion -- O(n)
add_point_to_byte_buffer(first_point, &mut cloud)?;
for coords in self.coordinates {
add_point_to_byte_buffer(coords, &mut cloud)?;
}
cloud.fields = fields;
cloud.height = 1; // everything is in one row (unstructured)
cloud.is_bigendian = false; // ROS default
cloud.is_dense = true; // ROS default
cloud.row_step = cloud.width * cloud.point_step; // Note: redundant but defined in PointCloud2 message
Ok(cloud)
}
}
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
Writer<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
/// Create a new Writer struct from an iterator.
///
/// # Example
/// ```
/// ```
pub fn from(coordinates: impl IntoIterator<Item = C> + 'static) -> Self {
Self {
coordinates: Box::new(coordinates.into_iter()),
xyz_field_type: FieldDatatype::F32,
phantom_t: std::marker::PhantomData,
}
}
}

706
tests/e2e_test.rs
View File

@ -1,31 +1,22 @@
use fallible_iterator::FallibleIterator;
use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::pcl_utils::*;
use ros_pointcloud2::reader::*;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::writer::*;
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());
macro_rules! convert_from_into {
($reader:ty, $writer:ty, $point:ty, $cloud:expr) => {
let copy = $cloud.clone();
let msg: Result<PointCloud2Msg, _> = <$writer>::from($cloud).try_into();
assert!(msg.is_ok());
let to_p_type = <$reader>::try_from(msg.unwrap());
assert!(to_p_type.is_ok());
let to_p_type = to_p_type.unwrap();
let back_to_type = to_p_type.map(|point| Ok(point)).collect::<Vec<$point>>();
assert_eq!(copy, back_to_type.unwrap());
};
}
#[test]
@ -39,7 +30,8 @@ fn custom_xyz_f32() {
y: f32,
z: f32,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyReader = Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyWriter = Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self {
([point.x, point.y, point.z], [])
@ -55,30 +47,35 @@ fn custom_xyz_f32() {
})
}
}
impl MetaNames<METADIM> for CustomPoint {
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
[]
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
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,
},
]);
convert_from_into!(
MyReader,
MyWriter,
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]
@ -92,12 +89,15 @@ fn custom_xyzi_f32() {
z: f32,
i: u8,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyReader = reader::Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self {
([point.x, point.y, point.z], [PointMeta::new(point.i)])
}
}
type MyWriter = writer::Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, ConversionError> {
@ -109,13 +109,14 @@ fn custom_xyzi_f32() {
})
}
}
impl MetaNames<METADIM> for CustomPoint {
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
["intensity"].map(|s| s.to_string())
[format!("intensity")]
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
convert_from_into::<MyConverter, CustomPoint>(vec![
let cloud = vec![
CustomPoint {
x: 0.0,
y: 1.0,
@ -140,7 +141,8 @@ fn custom_xyzi_f32() {
z: f32::MAX,
i: u8::MAX,
},
]);
];
convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
}
#[test]
@ -157,7 +159,8 @@ fn custom_rgba_f32() {
b: u8,
a: u8,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyReader = reader::Reader<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
type MyWriter = writer::Writer<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self {
(
@ -185,13 +188,13 @@ fn custom_rgba_f32() {
})
}
}
impl MetaNames<METADIM> for CustomPoint {
impl convert::MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
["r", "g", "b", "a"].map(|s| s.to_string())
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
convert_from_into::<MyConverter, CustomPoint>(vec![
let cloud = vec![
CustomPoint {
x: 0.0,
y: 1.0,
@ -228,7 +231,8 @@ fn custom_rgba_f32() {
b: u8::MAX,
a: u8::MAX,
},
]);
];
convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
}
#[test]
@ -256,317 +260,357 @@ fn converterxyz() {
},
];
convert_from_into::<ConvertXYZ, PointXYZ>(cloud);
convert_from_into!(ReaderXYZ, WriterXYZ, PointXYZ, cloud);
}
#[test]
fn converterxyzrgba() {
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,
},
]);
convert_from_into!(
ReaderXYZRGBA,
WriterXYZRGBA,
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() {
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,
},
]);
convert_from_into!(
ReaderXYZINormal,
WriterXYZINormal,
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() {
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,
},
]);
convert_from_into!(
ReaderXYZRGBNormal,
WriterXYZRGBNormal,
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,
},
]);
convert_from_into!(
ReaderXYZNormal,
WriterXYZNormal,
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,
},
]);
convert_from_into!(
ReaderXYZRGBL,
WriterXYZRGBL,
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,
},
]);
convert_from_into!(
ReaderXYZRGB,
WriterXYZRGB,
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,
},
]);
convert_from_into!(
ReaderXYZL,
WriterXYZL,
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,
},
]);
convert_from_into!(
ReaderXYZI,
WriterXYZI,
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,
},
]
);
}

2
tests/r2r_msg_test.rs
View File

@ -1,11 +1,11 @@
#[cfg(feature = "r2r_msg")]
#[test]
fn convertxyz_r2r_msg() {
use r2r::sensor_msgs::msg::PointCloud2;
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::ConvertXYZ;
use r2r::sensor_msgs::msg::PointCloud2;
let cloud = vec![
PointXYZ {