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

Move to Iterator only (#13) 

* add iterator impl

* redo api

* docs and clippy

* fix rosrust time

* no convert

* no optional point collect

* tidy

* avoid bulk import
This commit is contained in:
stelzo 2024-04-29 23:04:50 +02:00 committed by GitHub
parent 0105a76f53
commit a0ca014e13
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
11 changed files with 2269 additions and 1539 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Cargo.toml
View File

@ -14,9 +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 }
r2r = { version = "0.8.4", optional = true }

190
README.md
View File

@ -5,58 +5,48 @@
</p>
</p>
Providing a memory efficient way for message conversion while allowing user defined types without the cost of iterations.
Instead of converting the entire cloud into a `Vec`, you get an iterable type that converts each point from the message on the fly.
Providing an easy to use, generics defined, point-wise iterator abstraction over the byte buffer in `PointCloud2` to minimize iterations in your processing pipeline.
To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `ros_types::PointCloud2Msg`.
To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `PointCloud2Msg`.
## Examples
```rust
use ros_pointcloud2::{
fallible_iterator::FallibleIterator,
pcl_utils::PointXYZ,
ros_types::PointCloud2Msg,
ConvertXYZ,
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
// Your points (here using the predefined type PointXYZ).
let cloud_points = vec![
PointXYZ {
x: 1.3,
y: 1.6,
z: 5.7,
},
PointXYZ {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
},
PointXYZ {x: 91.486, y: -4.1, z: 42.0001,},
PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,},
];
let cloud_copy = cloud_points.clone(); // For checking equality later.
// For equality test later
let cloud_copy = cloud_points.clone();
// Vector -> Converter -> Message
let internal_msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points)
.unwrap()
.try_into()
// Vector -> Writer -> Message.
// You can also just give the Vec or anything that implements `IntoIterator`.
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
.try_into() // iterating points here O(n)
.unwrap();
// Convert to your favorite ROS crate message type, we will use rosrust here.
// let msg: rosrust_msg::sensor_msgs::PointCloud2 = internal_msg.into();
// Convert to your ROS crate message type, we will use r2r here.
// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
// Back to this crate's message type.
// Publish ...
// ... now incoming from a topic.
// let internal_msg: PointCloud2Msg = msg.into();
// Message -> Converter -> Vector
let convert: ConvertXYZ = ConvertXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = convert
// Message -> Reader -> your pipeline. The Reader implements the Iterator trait.
let reader = ReaderXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = reader
.map(|point: PointXYZ| {
// Insert your point business logic here
// or use other methods like .for_each().
// Some logic here
// We are using a fallible iterator so we need to return a Result.
Ok(point)
point
})
.collect::<Vec<PointXYZ>>()
.unwrap(); // Handle point conversion or byte errors here.
.collect::<Vec<PointXYZ>>(); // iterating points here O(n)
assert_eq!(new_cloud_points, cloud_copy);
```
@ -95,141 +85,13 @@ Also, indicate the following dependencies to your linker inside the `package.xml
<depend>builtin_interfaces</depend>
```
### Others
To use `ros_pointcloud2` somewhere else, you can also implement the `Into` and `From` traits for `PointCloud2Msg`.
Try to avoid cloning the `data: Vec<u8>` field.
```rust
use ros_pointcloud2::ros_types::PointCloud2Msg;
struct YourROSPointCloud2 {} // Likely to be generated by your ROS crate.
impl Into<YourROSPointCloud2> for PointCloud2Msg {
fn into(self) -> YourROSPointCloud2 {
todo!()
}
}
impl From<YourROSPointCloud2> for PointCloud2Msg {
fn from(msg: YourROSPointCloud2) -> Self {
todo!()
}
}
```
Please open an issue or PR if you want to see support for other crates.
## Features
- Easy to integrate into your favorite ROS1 or ROS2 crate
- Custom point types
- Predefined types for common conversions (compared to PCL)
- PointXYZ
- PointXYZI
- PointXYZL
- PointXYZRGB
- PointXYZRGBA
- PointXYZRGBL
- PointXYZNormal
- PointXYZINormal
- PointXYZRGBNormal
- 2D and 3D
## Custom Types
You can freely convert to your own point types without reiterating the entire cloud.
You just need to implement some traits.
```rust
use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::{ConversionError, Convert, MetaNames, PointConvertible, PointMeta};
const DIM: usize = 3;
const METADIM: usize = 4;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32, // DIM 1
y: f32, // DIM 2
z: f32, // DIM 3
r: u8, // METADIM 1
g: u8, // METADIM 2
b: u8, // METADIM 3
a: u8, // METADIM 4
}
// Converting your custom point to the crate's internal representation
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
fn from(point: CustomPoint) -> Self {
(
[point.x, point.y, point.z],
[
PointMeta::new(point.r),
PointMeta::new(point.g),
PointMeta::new(point.b),
PointMeta::new(point.a),
],
)
}
}
// The mappings for index of meta idx to field names. Example: 0 -> "r", 1 -> "g", 2 -> "b", 3 -> "a"
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
["r", "g", "b", "a"].map(|s| s.to_string())
}
}
// Converting crate's internal representation to your custom point
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
r: data.1[0].get()?,
g: data.1[1].get()?,
b: data.1[2].get()?,
a: data.1[3].get()?,
})
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
// Your custom cloud (Vector)
let custom_cloud = vec![
CustomPoint {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
r: u8::MAX,
g: u8::MAX,
b: u8::MAX,
a: u8::MAX,
},
];
// Cloud -> ROS message
let custom_msg: PointCloud2Msg = MyConverter::try_from(custom_cloud)
.unwrap()
.try_into()
.unwrap();
// ROS message -> Cloud
let to_custom_type = MyConverter::try_from(custom_msg).unwrap();
```
## Future Work
- Benchmark vs PCL
- Proper error passing to the iterator result (currently merged into `PointConversionError`)
- remove allocations
- introduce no-panic for maximum stability
- Add more predefined types
- Optional derive macros for custom point implementations
## License
[MIT](https://choosealicense.com/licenses/mit/)

361
src/convert.rs Normal file
View File

@ -0,0 +1,361 @@
use crate::*;
/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html).
#[derive(Default, Clone, Debug, PartialEq, Copy)]
pub enum FieldDatatype {
F32,
F64,
I32,
U8,
U16,
#[default]
U32,
I8,
I16,
}
impl FieldDatatype {
pub fn size(&self) -> usize {
match self {
FieldDatatype::U8 => 1,
FieldDatatype::U16 => 2,
FieldDatatype::U32 => 4,
FieldDatatype::I8 => 1,
FieldDatatype::I16 => 2,
FieldDatatype::I32 => 4,
FieldDatatype::F32 => 4,
FieldDatatype::F64 => 8,
}
}
}
/// Getter trait for the datatype of a field value.
pub trait GetFieldDatatype {
fn field_datatype() -> FieldDatatype;
}
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
}
}
impl TryFrom<u8> for FieldDatatype {
type Error = ConversionError;
fn try_from(value: u8) -> Result<Self, Self::Error> {
match value {
1 => Ok(FieldDatatype::I8),
2 => Ok(FieldDatatype::U8),
3 => Ok(FieldDatatype::I16),
4 => Ok(FieldDatatype::U16),
5 => Ok(FieldDatatype::I32),
6 => Ok(FieldDatatype::U32),
7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64),
_ => Err(ConversionError::UnsupportedFieldType),
}
}
}
impl From<FieldDatatype> for u8 {
fn from(val: FieldDatatype) -> Self {
match val {
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 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 != u8::from(*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 conversion implementation to have a correct mapping.
///
/// This trait is needed to implement the `PointConvertible` trait.
///
/// # Example for full point conversion.
/// ```
/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
///
/// #[derive(Clone, Debug, PartialEq, Copy)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
///
/// impl MetaNames<1> for MyPointXYZI {
/// fn meta_names() -> [&'static str; 1] {
/// ["intensity"]
/// }
/// }
/// ```
pub trait MetaNames<const METADIM: usize> {
fn meta_names() -> [&'static str; METADIM];
}
/// 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 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
#[inline]
fn bytes(x: &i8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i16 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
#[inline]
fn bytes(x: &i16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u16 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
#[inline]
fn bytes(x: &u16) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u32 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn bytes(x: &u32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for f32 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
#[inline]
fn bytes(x: &f32) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for i32 {
#[inline]
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 {
#[inline]
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],
])
}
#[inline]
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],
])
}
#[inline]
fn bytes(x: &f64) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
impl FromBytes for u8 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
Self::from_be_bytes([bytes[0]])
}
#[inline]
fn from_le_bytes(bytes: &[u8]) -> Self {
Self::from_le_bytes([bytes[0]])
}
#[inline]
fn bytes(x: &u8) -> Vec<u8> {
Vec::from(x.to_le_bytes())
}
}
#[derive(Default, Clone, Debug, PartialEq)]
pub(crate) enum Endianness {
Big,
#[default]
Little,
}
#[inline(always)]
pub(crate) fn load_loadable<T: FromBytes, const SIZE: usize>(
start_idx: usize,
data: &[u8],
endian: &Endianness,
) -> T {
match endian {
Endianness::Big => T::from_be_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
Endianness::Little => T::from_le_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
}
}
/// Note: check if the data slice is long enough to load the bytes beforehand! Uses unsafe indexing.
#[inline(always)]
fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> [u8; S] {
let mut target = [u8::default(); S];
debug_assert!(target.len() == S);
debug_assert!(data.len() >= start_idx + S);
let source = unsafe { data.get_unchecked(start_idx..start_idx + S) };
target
.iter_mut()
.zip(source.iter())
.for_each(|(target, source)| {
*target = *source;
});
target
}
#[cfg(test)]
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);
}
}

993
src/lib.rs
View File

File diff suppressed because it is too large Load Diff

409
src/pcl_utils.rs
View File

@ -1,11 +1,13 @@
use crate::{ConversionError, MetaNames, PointConvertible, PointMeta};
use crate::{MetaNames, Point, PointConvertible};
/// Pack an RGB color into a single f32 value as used in ROS with PCL for RViz usage.
#[inline]
fn pack_rgb(r: u8, g: u8, b: u8) -> f32 {
let packed = ((r as u32) << 16) + ((g as u32) << 8) + (b as u32);
f32::from_bits(packed)
}
/// Unpack an RGB color from a single f32 value as used in ROS with PCL for RViz usage.
#[inline]
fn unpack_rgb(rgb: f32) -> [u8; 3] {
let packed: u32 = rgb.to_bits();
@ -24,33 +26,32 @@ pub struct PointXYZ {
pub z: f32,
}
impl TryInto<([f32; 3], [PointMeta; 0])> for PointXYZ {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 0]), Self::Error> {
Ok(([self.x, self.y, self.z], []))
impl From<Point<f32, 3, 0>> for PointXYZ {
fn from(point: Point<f32, 3, 0>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 0])> for PointXYZ {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 0])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
})
impl From<PointXYZ> for Point<f32, 3, 0> {
fn from(point: PointXYZ) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [],
}
}
}
impl MetaNames<0> for PointXYZ {
fn meta_names() -> [String; 0] {
fn meta_names() -> [&'static str; 0] {
[]
}
}
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.
@ -62,34 +63,33 @@ pub struct PointXYZI {
pub intensity: f32,
}
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZI {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
Ok(([self.x, self.y, self.z], [PointMeta::new(self.intensity)]))
impl From<PointXYZI> for Point<f32, 3, 1> {
fn from(point: PointXYZI) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [point.intensity.into()],
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZI {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
intensity: data.1[0].get()?,
})
impl From<Point<f32, 3, 1>> for PointXYZI {
fn from(point: Point<f32, 3, 1>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
intensity: point.meta[0].get(),
}
}
}
impl MetaNames<1> for PointXYZI {
fn meta_names() -> [String; 1] {
["intensity"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 1] {
["intensity"]
}
}
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.
@ -103,39 +103,37 @@ pub struct PointXYZRGB {
pub b: u8,
}
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok(([self.x, self.y, self.z], [PointMeta::new(rgb)]))
}
}
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZRGB {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
impl From<Point<f32, 3, 1>> for PointXYZRGB {
fn from(point: Point<f32, 3, 1>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
})
}
}
}
impl From<PointXYZRGB> for Point<f32, 3, 1> {
fn from(point: PointXYZRGB) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [pack_rgb(point.r, point.g, point.b).into()],
}
}
}
impl MetaNames<1> for PointXYZRGB {
fn meta_names() -> [String; 1] {
["rgb"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 1] {
["rgb"]
}
}
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.
@ -151,43 +149,39 @@ pub struct PointXYZRGBA {
pub a: u8,
}
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok((
[self.x, self.y, self.z],
[PointMeta::new(rgb), PointMeta::new(self.a)],
))
}
}
impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBA {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
impl From<Point<f32, 3, 2>> for PointXYZRGBA {
fn from(point: Point<f32, 3, 2>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
a: data.1[1].get()?,
})
a: point.meta[1].get(),
}
}
}
impl From<PointXYZRGBA> for Point<f32, 3, 2> {
fn from(point: PointXYZRGBA) -> Self {
let rgb = pack_rgb(point.r, point.g, point.b);
Point {
coords: [point.x, point.y, point.z],
meta: [rgb.into(), point.a.into()],
}
}
}
impl MetaNames<2> for PointXYZRGBA {
fn meta_names() -> [String; 2] {
["rgb", "a"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 2] {
["rgb", "a"]
}
}
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.
@ -204,50 +198,46 @@ pub struct PointXYZRGBNormal {
pub normal_z: f32,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok((
[self.x, self.y, self.z],
[
PointMeta::new(rgb),
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
],
))
}
}
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
impl From<Point<f32, 3, 4>> for PointXYZRGBNormal {
fn from(point: Point<f32, 3, 4>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
normal_x: data.1[1].get()?,
normal_y: data.1[2].get()?,
normal_z: data.1[3].get()?,
})
normal_x: point.meta[1].get(),
normal_y: point.meta[2].get(),
normal_z: point.meta[3].get(),
}
}
}
impl From<PointXYZRGBNormal> for Point<f32, 3, 4> {
fn from(point: PointXYZRGBNormal) -> Self {
let rgb = pack_rgb(point.r, point.g, point.b);
Point {
coords: [point.x, point.y, point.z],
meta: [
rgb.into(),
point.normal_x.into(),
point.normal_y.into(),
point.normal_z.into(),
],
}
}
}
impl MetaNames<4> for PointXYZRGBNormal {
fn meta_names() -> [String; 4] {
["rgb", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 4] {
["rgb", "normal_x", "normal_y", "normal_z"]
}
}
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.
@ -262,45 +252,41 @@ pub struct PointXYZINormal {
pub normal_z: f32,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZINormal {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok((
[self.x, self.y, self.z],
[
PointMeta::new(self.intensity),
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
impl From<PointXYZINormal> for Point<f32, 3, 4> {
fn from(point: PointXYZINormal) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [
point.intensity.into(),
point.normal_x.into(),
point.normal_y.into(),
point.normal_z.into(),
],
))
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
intensity: data.1[0].get()?,
normal_x: data.1[1].get()?,
normal_y: data.1[2].get()?,
normal_z: data.1[3].get()?,
})
impl From<Point<f32, 3, 4>> for PointXYZINormal {
fn from(point: Point<f32, 3, 4>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
intensity: point.meta[0].get(),
normal_x: point.meta[1].get(),
normal_y: point.meta[2].get(),
normal_z: point.meta[3].get(),
}
}
}
impl MetaNames<4> for PointXYZINormal {
fn meta_names() -> [String; 4] {
["intensity", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 4] {
["intensity", "normal_x", "normal_y", "normal_z"]
}
}
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.
@ -312,34 +298,33 @@ pub struct PointXYZL {
pub label: u32,
}
impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZL {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> {
Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)]))
impl From<Point<f32, 3, 1>> for PointXYZL {
fn from(point: Point<f32, 3, 1>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
label: point.meta[0].get(),
}
}
}
impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
label: data.1[0].get()?,
})
impl From<PointXYZL> for Point<f32, 3, 1> {
fn from(point: PointXYZL) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [point.label.into()],
}
}
}
impl MetaNames<1> for PointXYZL {
fn meta_names() -> [String; 1] {
["label".to_string()]
fn meta_names() -> [&'static str; 1] {
["label"]
}
}
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.
@ -354,43 +339,41 @@ pub struct PointXYZRGBL {
pub label: u32,
}
impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> {
let rgb = pack_rgb(self.r, self.g, self.b);
Ok((
[self.x, self.y, self.z],
[PointMeta::new(rgb), PointMeta::new(self.label)],
))
}
}
impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBL {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result<Self, Self::Error> {
let rgb = data.1[0].get::<f32>()?;
impl From<Point<f32, 3, 2>> for PointXYZRGBL {
fn from(point: Point<f32, 3, 2>) -> Self {
let rgb = point.meta[0].get::<f32>();
let rgb_unpacked = unpack_rgb(rgb);
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: rgb_unpacked[0],
g: rgb_unpacked[1],
b: rgb_unpacked[2],
label: data.1[1].get()?,
})
label: point.meta[1].get(),
}
}
}
impl From<PointXYZRGBL> for Point<f32, 3, 2> {
fn from(point: PointXYZRGBL) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [
pack_rgb(point.r, point.g, point.b).into(),
point.label.into(),
],
}
}
}
impl MetaNames<2> for PointXYZRGBL {
fn meta_names() -> [String; 2] {
["rgb", "label"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 2] {
["rgb", "label"]
}
}
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.
@ -404,40 +387,36 @@ pub struct PointXYZNormal {
pub normal_z: f32,
}
impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZNormal {
type Error = ConversionError;
impl From<Point<f32, 3, 3>> for PointXYZNormal {
fn from(point: Point<f32, 3, 3>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
normal_x: point.meta[0].get(),
normal_y: point.meta[1].get(),
normal_z: point.meta[2].get(),
}
}
}
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> {
Ok((
[self.x, self.y, self.z],
[
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
impl From<PointXYZNormal> for Point<f32, 3, 3> {
fn from(point: PointXYZNormal) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [
point.normal_x.into(),
point.normal_y.into(),
point.normal_z.into(),
],
))
}
}
impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZNormal {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 3])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
normal_x: data.1[0].get()?,
normal_y: data.1[1].get()?,
normal_z: data.1[2].get()?,
})
}
}
impl MetaNames<3> for PointXYZNormal {
fn meta_names() -> [String; 3] {
["normal_x", "normal_y", "normal_z"].map(|s| s.to_string())
fn meta_names() -> [&'static str; 3] {
["normal_x", "normal_y", "normal_z"]
}
}
impl PointConvertible<f32, { size_of!(f32) }, 3, 3> for PointXYZNormal {}
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 3> for PointXYZNormal {}

422
src/reader.rs Normal file
View File

@ -0,0 +1,422 @@
use crate::{
pcl_utils::*,
Point,
PointCloud2Msg,
PointConvertible,
ConversionError,
MetaNames,
PointMeta,
convert::{
FromBytes,
FieldDatatype,
load_loadable,
Endianness,
check_coord,
},
};
/// Convenience type for a Reader that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderF32<const DIM: usize, const METADIM: usize, C> =
Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderF64<const DIM: usize, const METADIM: usize, C> =
Reader<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderI8<const DIM: usize, const METADIM: usize, C> =
Reader<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderI16<const DIM: usize, const METADIM: usize, C> =
Reader<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderI32<const DIM: usize, const METADIM: usize, C> =
Reader<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderU8<const DIM: usize, const METADIM: usize, C> =
Reader<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderU16<const DIM: usize, const METADIM: usize, C> =
Reader<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
/// Convenience type for a Reader that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type ReaderU32<const DIM: usize, const METADIM: usize, C> =
Reader<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
/// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`).
/// Every additional meta data is ignored.
pub type ReaderXYZ = ReaderF32<3, 0, PointXYZ>;
/// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`).
/// Every additional meta data is ignored.
pub type ReaderXYZI = ReaderF32<3, 1, PointXYZI>;
/// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`).
/// Every additional meta data is ignored.
pub type ReaderXYZNormal = ReaderF32<3, 3, PointXYZNormal>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb (see `PointXYZRGB`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGB = ReaderF32<3, 1, PointXYZRGB>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb and intensity (see `PointXYZRGBL`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGBL = ReaderF32<3, 2, PointXYZRGBL>;
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb with additional alpha channel (see `PointXYZRGBA`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGBA = ReaderF32<3, 2, PointXYZRGBA>;
/// Provides the message as an Iterator over xyz coordinates and normal and unpacked rgb (see `PointXYZRGBNormal`).
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
/// Every additional meta data is ignored.
pub type ReaderXYZRGBNormal = ReaderF32<3, 4, PointXYZRGBNormal>;
/// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
/// Every additional meta data is ignored.
pub type ReaderXYZINormal = ReaderF32<3, 4, PointXYZINormal>;
/// Provides the message as an Iterator over xyz coordinates and intensity with additional label (see `PointXYZL`).
/// Every additional meta data is ignored.
/// The label is typically used for semantic segmentation.
pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>;
/// The Reader provides a an iterator abstraction of the PointCloud2Msg.
///
/// The iterator is defined at compile time, so the point has to be described via template arguments.
///
/// # Predefined Readers
/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
///
/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
///
/// ROS1 with rosrust:
/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ROS2 with r2r:
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
///
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
///
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// let convert = ReaderXYZ::try_from(msg).unwrap();
/// // ^^^^^^^^^ conversion from PointCloud2Msg to Reader that implements Iterator
///
/// convert.for_each(|point: PointXYZ| {
/// // do something with the point
/// });
/// ```
/// # Fully Custom Reader
/// When the predefined types are not enough (like sensor specific metadata), you can describe your Reader with the following template arguments:
/// - T: The coordinate type, e.g. f32
/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
/// Afterwards, implement the PointConvertible trait for your custom point type.
///
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
/// };
///
/// type Xyz = f32; // coordinate type
/// const XYZ_S: usize = size_of!(Xyz);
/// const DIM: usize = 3; // helper constant for dimensionality
/// const METADIM: usize = 1; // helper constant for meta data fields
///
/// #[derive(Debug, PartialEq, Clone)]
/// struct CustomPoint {
/// x: f32,
/// y: f32,
/// z: f32,
/// i: u8,
/// }
///
/// impl From<Point<f32, 3, 1>> for CustomPoint {
/// fn from(point: Point<f32, 3, 1>) -> Self {
/// Self {
/// x: point.coords[0],
/// y: point.coords[1],
/// z: point.coords[2],
/// i: point.meta[0].get(),
/// }
/// }
/// }
///
///impl From<CustomPoint> for Point<f32, 3, 1> {
/// fn from(point: CustomPoint) -> Self {
/// Point {
/// coords: [point.x, point.y, point.z],
/// meta: [
/// point.i.into(),
/// ],
/// }
/// }
///}
///
/// impl MetaNames<METADIM> for CustomPoint {
/// fn meta_names() -> [&'static str; METADIM] {
/// ["intensity"]
/// }
/// }
///
/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
///
/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// // ^^^^^^^^ your custom Reader
/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// ```
pub struct Reader<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
where
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> Iterator
for Reader<T, SIZE, DIM, METADIM, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
type Item = C;
/// 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) -> Option<Self::Item> {
if self.iteration >= self.cloud_length {
return None; // iteration finished
}
let mut xyz = [T::default(); DIM];
xyz.iter_mut()
.zip(self.offsets.iter())
.for_each(|(p_xyz, in_point_offset)| {
*p_xyz = load_loadable::<T, SIZE>(
(self.iteration * self.point_step_size) + in_point_offset,
&self.data,
&self.endianness,
);
});
debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
debug_assert!(
self.offsets.len() == DIM + METADIM,
"Offset length mismatch"
);
let mut meta = [PointMeta::default(); METADIM];
meta.iter_mut()
.zip(self.offsets.iter().skip(DIM))
.zip(self.meta.iter())
.for_each(|((p_meta, in_point_offset), (_, meta_type))| {
let start = (self.iteration * self.point_step_size) + in_point_offset;
*p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
});
self.iteration += 1;
Some(C::from(Point { coords: xyz, meta }))
}
}
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::{
/// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
///
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
///
/// let convert = ReaderXYZ::try_from(msg).unwrap();
/// // ^^^^^^^^ 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) {
let meta_idx = idx - DIM;
debug_assert!(
meta_idx < meta_with_offsets.len(),
"Meta data length mismatch"
);
meta_with_offsets[meta_idx].0 = field.name.clone();
meta_with_offsets[meta_idx].1 = field.datatype.try_into()?;
meta_with_offsets[meta_idx].2 = field.offset as usize;
}
}
}
}
meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
debug_assert!(
meta_with_offsets.len() == METADIM,
"Meta data length mismatch"
);
let mut meta_offsets = [usize::default(); METADIM];
let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM];
meta_with_offsets
.into_iter()
.zip(meta.iter_mut())
.zip(meta_offsets.iter_mut())
.for_each(|(((name, datatype, offset), meta), meta_offset)| {
*meta = (name, datatype);
*meta_offset = offset;
});
let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
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;
let cloud_length = cloud.width as usize * cloud.height as usize;
if point_step_size * cloud_length != cloud.data.len() {
return Err(ConversionError::DataLengthMismatch);
}
let last_offset = offsets.last().expect("Dimensionality is 0.");
if let Some(last_meta) = meta.last() {
let size_with_last_meta = last_offset + last_meta.1.size();
if size_with_last_meta > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
} else if last_offset + xyz_field_type.size() > point_step_size {
return Err(ConversionError::DataLengthMismatch);
}
Ok(Self {
iteration: 0,
data: cloud.data,
point_step_size,
cloud_length: cloud.width as usize * cloud.height as usize,
offsets,
meta,
endianness: endian,
phantom_t: std::marker::PhantomData,
phantom_c: std::marker::PhantomData,
})
}
}

56
src/ros_types.rs
View File

@ -70,19 +70,19 @@ impl From<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
}
#[cfg(feature = "r2r_msg")]
impl Into<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
fn into(self) -> r2r::sensor_msgs::msg::PointCloud2 {
impl From<PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
fn from(msg: PointCloud2Msg) -> Self {
r2r::sensor_msgs::msg::PointCloud2 {
header: r2r::std_msgs::msg::Header {
stamp: r2r::builtin_interfaces::msg::Time {
sec: self.header.stamp.sec as i32,
nanosec: self.header.stamp.nsec,
sec: msg.header.stamp.sec as i32,
nanosec: msg.header.stamp.nsec,
},
frame_id: self.header.frame_id,
frame_id: msg.header.frame_id,
},
height: self.height,
width: self.width,
fields: self
height: msg.height,
width: msg.width,
fields: msg
.fields
.into_iter()
.map(|field| r2r::sensor_msgs::msg::PointField {
@ -92,11 +92,11 @@ impl Into<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
count: field.count,
})
.collect(),
is_bigendian: self.is_bigendian,
point_step: self.point_step,
row_step: self.row_step,
data: self.data,
is_dense: self.is_dense,
is_bigendian: msg.is_bigendian,
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
is_dense: msg.is_dense,
}
}
}
@ -135,20 +135,20 @@ impl From<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
}
#[cfg(feature = "rosrust_msg")]
impl Into<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
fn into(self) -> rosrust_msg::sensor_msgs::PointCloud2 {
impl From<PointCloud2Msg> for rosrust_msg::sensor_msgs::PointCloud2 {
fn from(msg: PointCloud2Msg) -> Self {
rosrust_msg::sensor_msgs::PointCloud2 {
header: rosrust_msg::std_msgs::Header {
seq: self.header.seq,
stamp: rosrust::Time {
sec: self.header.stamp.sec,
nsec: self.header.stamp.nsec,
seq: msg.header.seq,
stamp: TimeMsg {
sec: msg.header.stamp.sec,
nsec: msg.header.stamp.nsec,
},
frame_id: self.header.frame_id,
frame_id: msg.header.frame_id,
},
height: self.height,
width: self.width,
fields: self
height: msg.height,
width: msg.width,
fields: msg
.fields
.into_iter()
.map(|field| rosrust_msg::sensor_msgs::PointField {
@ -158,11 +158,11 @@ impl Into<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
count: field.count,
})
.collect(),
is_bigendian: self.is_bigendian,
point_step: self.point_step,
row_step: self.row_step,
data: self.data,
is_dense: self.is_dense,
is_bigendian: msg.is_bigendian,
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
is_dense: msg.is_dense,
}
}
}

346
src/writer.rs Normal file
View File

@ -0,0 +1,346 @@
use crate::{
pcl_utils::*,
Point,
PointCloud2Msg,
PointConvertible,
ConversionError,
ros_types::PointFieldMsg,
convert::{
FromBytes,
FieldDatatype,
},
};
/// Convenience type for a Writer that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterF32<const DIM: usize, const METADIM: usize, C> =
Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterF64<const DIM: usize, const METADIM: usize, C> =
Writer<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterI8<const DIM: usize, const METADIM: usize, C> =
Writer<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterI16<const DIM: usize, const METADIM: usize, C> =
Writer<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterI32<const DIM: usize, const METADIM: usize, C> =
Writer<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterU8<const DIM: usize, const METADIM: usize, C> =
Writer<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterU16<const DIM: usize, const METADIM: usize, C> =
Writer<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
/// Convenience type for a Writer that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
pub type WriterU32<const DIM: usize, const METADIM: usize, C> =
Writer<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
/// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`).
pub type WriterXYZ = WriterF32<3, 0, PointXYZ>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`).
pub type WriterXYZI = WriterF32<3, 1, PointXYZI>;
/// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`).
pub type WriterXYZNormal = WriterF32<3, 3, PointXYZNormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`).
pub type WriterXYZRGB = WriterF32<3, 1, PointXYZRGB>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`).
pub type WriterXYZRGBL = WriterF32<3, 2, PointXYZRGBL>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels and alpha channel (see `PointXYZRGBA`).
pub type WriterXYZRGBA = WriterF32<3, 2, PointXYZRGBA>;
/// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`).
pub type WriterXYZRGBNormal = WriterF32<3, 4, PointXYZRGBNormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
pub type WriterXYZINormal = WriterF32<3, 4, PointXYZINormal>;
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`).
pub type WriterXYZL = WriterF32<3, 1, PointXYZL>;
/// The Write creates a PointCloud2Msg out of your point iterator.
///
/// The iterator is defined at compile time, so the Point has to be described via template arguments.
///
/// # Predefined Readers
/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
///
/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
///
/// ROS1 with rosrust:
/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ROS2 with r2r:
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
///
/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
///
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
///
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// // ^^^^^^^^^ creating PointCloud2Msg from an iterator
/// ```
/// # Fully Custom Writer
/// When the predefined types are not enough (like sensor specific metadata), you can describe your Writer with the following template arguments:
/// - T: The coordinate type, e.g. f32
/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
/// Afterwards, implement the PointConvertible trait for your custom point type.
///
/// ## Example
/// ```
/// use ros_pointcloud2::{
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
/// };
///
/// type Xyz = f32; // coordinate type
/// const XYZ_S: usize = size_of!(Xyz);
/// const DIM: usize = 3; // helper constant for dimensionality
/// const METADIM: usize = 1; // helper constant for meta data fields
///
/// #[derive(Debug, PartialEq, Clone)]
/// struct CustomPoint {
/// x: f32,
/// y: f32,
/// z: f32,
/// i: u8,
/// }
///
/// impl From<Point<f32, 3, 1>> for CustomPoint {
/// fn from(point: Point<f32, 3, 1>) -> Self {
/// Self {
/// x: point.coords[0],
/// y: point.coords[1],
/// z: point.coords[2],
/// i: point.meta[0].get(),
/// }
/// }
/// }
///
///impl From<CustomPoint> for Point<f32, 3, 1> {
/// fn from(point: CustomPoint) -> Self {
/// Point {
/// coords: [point.x, point.y, point.z],
/// meta: [
/// point.i.into(),
/// ],
/// }
/// }
///}
///
/// impl MetaNames<METADIM> for CustomPoint {
/// fn meta_names() -> [&'static str; METADIM] {
/// ["intensity"]
/// }
/// }
/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
///
/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
/// // ^^^^^^^^ your custom Writer
/// ```
pub struct Writer<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
where
T: FromBytes,
C: PointConvertible<T, SIZE, DIM, METADIM>,
{
coordinates: Box<dyn Iterator<Item = C>>,
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;
/// Writes the points to a PointCloud2Msg.
///
/// First use the `from` method for initializing the Writer.
/// Then use the `try_into` method to do the actual conversion.
///
/// The operation is O(n) in time complexity where n is the number of points in the point cloud.
///
/// # Example
/// ```
/// use ros_pointcloud2::{
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
/// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// // ^^^^^^^^ ROS message conversion
/// ```
fn try_into(mut self) -> Result<PointCloud2Msg, Self::Error> {
if DIM > 3 {
return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time?
}
let 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".into(),
offset: 0,
datatype: T::field_datatype().into(),
count: 1,
});
fields.push(PointFieldMsg {
name: "y".into(),
offset: SIZE as u32,
datatype: T::field_datatype().into(),
count: 1,
});
}
if DIM == 3 {
fields.push(PointFieldMsg {
name: "z".into(),
offset: 2 * SIZE as u32,
datatype: T::field_datatype().into(),
count: 1,
});
}
let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
let point: Point<T, DIM, METADIM> = first_point.clone().into();
let meta_names = C::meta_names();
let mut meta_offsets_acc = DIM as u32 * SIZE as u32;
for (meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
let datatype_code = meta_value.datatype.into();
if FieldDatatype::try_from(datatype_code).is_err() {
return Err(ConversionError::UnsupportedFieldType);
}
fields.push(PointFieldMsg {
name: meta_name.into(),
offset: meta_offsets_acc,
datatype: datatype_code,
count: 1,
});
meta_offsets_acc += meta_value.datatype.size() as u32
}
let mut cloud = PointCloud2Msg {
point_step: fields.iter().fold(Default::default(), |acc, field| {
let field_type: FieldDatatype = field
.datatype
.try_into()
.expect("Unsupported field but checked before.");
let field_size = field_type.size();
acc + field.count * field_size as u32
}),
..Default::default()
};
// 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 Writer from any iterator that iterates over a template-defined point to a ROS message type.
/// First use the `from` method for initializing the Writer.
/// Then use the `try_into` method to do the actual conversion.
///
/// The operation is O(n) in time complexity where n is the number of points in the point cloud.
///
/// # Example
/// ```
/// use ros_pointcloud2::{
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
/// };
///
/// let cloud_points: Vec<PointXYZ> = vec![
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
/// ];
// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
/// // ^^^^ Writer creation
/// ```
pub fn from(coordinates: impl IntoIterator<Item = C> + 'static) -> Self {
Self {
coordinates: Box::new(coordinates.into_iter()),
phantom_t: std::marker::PhantomData,
}
}
}
#[inline(always)]
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 point: Point<T, DIM, METADIM> = coords.into();
// (x, y, z...)
point
.coords
.iter()
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
// meta data description
point.meta.iter().for_each(|meta| {
let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
cloud.data.extend_from_slice(truncated_bytes);
});
cloud.width += 1;
Ok(true)
}

477
tests/e2e_test.rs
View File

@ -1,31 +1,34 @@
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();
macro_rules! convert_from_into {
($reader:ty, $writer:ty, $point:ty, $cloud:expr) => {
convert_from_into_in_out_cloud!(
$reader,
$writer,
$point,
$cloud.clone(),
$point,
$cloud,
$point
);
};
}
macro_rules! convert_from_into_in_out_cloud {
($reader:ty, $writer:ty, $point:ty, $in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
let msg: Result<PointCloud2Msg, _> = <$writer>::from($in_cloud).try_into();
assert!(msg.is_ok());
let to_p_type = C::try_from(msg.unwrap());
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<P>>();
assert_eq!(copy, back_to_type.unwrap());
let back_to_type = to_p_type.collect::<Vec<$out_point>>();
assert_eq!($out_cloud, back_to_type);
};
}
#[test]
@ -39,30 +42,41 @@ fn custom_xyz_f32() {
y: f32,
z: f32,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
impl From<Point<f32, 3, 0>> for CustomPoint {
fn from(point: Point<f32, 3, 0>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
}
}
}
impl From<CustomPoint> for Point<f32, 3, 0> {
fn from(point: CustomPoint) -> Self {
([point.x, point.y, point.z], [])
Point {
coords: [point.x, point.y, point.z],
meta: [],
}
}
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, ConversionError> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
})
}
}
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
fn meta_names() -> [&'static str; METADIM] {
[]
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
convert_from_into::<MyConverter, CustomPoint>(vec![
convert_from_into!(
MyReader,
MyWriter,
CustomPoint,
vec![
CustomPoint {
x: 1.0,
y: 2.0,
@ -78,11 +92,14 @@ fn custom_xyz_f32() {
y: 8.0,
z: 9.0,
},
]);
]
);
}
#[test]
fn custom_xyzi_f32() {
type Xyz = f32;
const XYZ_S: usize = std::mem::size_of::<Xyz>();
const DIM: usize = 3;
const METADIM: usize = 1;
#[derive(Debug, PartialEq, Clone)]
@ -92,30 +109,39 @@ fn custom_xyzi_f32() {
z: f32,
i: u8,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
impl From<Point<f32, 3, 1>> for CustomPoint {
fn from(point: Point<f32, 3, 1>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
i: point.meta[0].get(),
}
}
}
impl From<CustomPoint> for Point<f32, 3, 1> {
fn from(point: CustomPoint) -> Self {
([point.x, point.y, point.z], [PointMeta::new(point.i)])
Point {
coords: [point.x, point.y, point.z],
meta: [point.i.into()],
}
}
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, ConversionError> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
i: data.1.first().unwrap().get().unwrap(),
})
}
}
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
["intensity"].map(|s| s.to_string())
fn meta_names() -> [&'static str; METADIM] {
["intensity"]
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
convert_from_into::<MyConverter, CustomPoint>(vec![
type MyReader = reader::Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
type MyWriter = writer::Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
let cloud = vec![
CustomPoint {
x: 0.0,
y: 1.0,
@ -140,7 +166,8 @@ fn custom_xyzi_f32() {
z: f32::MAX,
i: u8::MAX,
},
]);
];
convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
}
#[test]
@ -157,41 +184,44 @@ fn custom_rgba_f32() {
b: u8,
a: u8,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl From<CustomPoint> for ([f32; DIM], [PointMeta; METADIM]) {
type MyReader = reader::Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
type MyWriter = writer::Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
impl From<Point<f32, 3, 4>> for CustomPoint {
fn from(point: Point<f32, 3, 4>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
r: point.meta[0].get(),
g: point.meta[1].get(),
b: point.meta[2].get(),
a: point.meta[3].get(),
}
}
}
impl From<CustomPoint> for Point<f32, 3, 4> {
fn from(point: CustomPoint) -> Self {
(
[point.x, point.y, point.z],
[
PointMeta::new(point.r),
PointMeta::new(point.g),
PointMeta::new(point.b),
PointMeta::new(point.a),
Point {
coords: [point.x, point.y, point.z],
meta: [
point.r.into(),
point.g.into(),
point.b.into(),
point.a.into(),
],
)
}
}
impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
type Error = ConversionError;
fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
r: data.1[0].get()?,
g: data.1[1].get()?,
b: data.1[2].get()?,
a: data.1[3].get()?,
})
}
}
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [String; METADIM] {
["r", "g", "b", "a"].map(|s| s.to_string())
fn meta_names() -> [&'static str; METADIM] {
["r", "g", "b", "a"]
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
convert_from_into::<MyConverter, CustomPoint>(vec![
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, DIM, METADIM> for CustomPoint {}
let cloud = vec![
CustomPoint {
x: 0.0,
y: 1.0,
@ -228,7 +258,8 @@ fn custom_rgba_f32() {
b: u8::MAX,
a: u8::MAX,
},
]);
];
convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
}
#[test]
@ -256,12 +287,16 @@ fn converterxyz() {
},
];
convert_from_into::<ConvertXYZ, PointXYZ>(cloud);
convert_from_into!(ReaderXYZ, WriterXYZ, PointXYZ, cloud);
}
#[test]
fn converterxyzrgba() {
convert_from_into::<ConvertXYZRGBA, PointXYZRGBA>(vec![
convert_from_into!(
ReaderXYZRGBA,
WriterXYZRGBA,
PointXYZRGBA,
vec![
PointXYZRGBA {
x: 0.0,
y: 1.0,
@ -298,12 +333,17 @@ fn converterxyzrgba() {
b: u8::MAX,
a: u8::MAX,
},
]);
]
);
}
#[test]
fn converterxyzinormal() {
convert_from_into::<ConvertXYZINormal, PointXYZINormal>(vec![
convert_from_into!(
ReaderXYZINormal,
WriterXYZINormal,
PointXYZINormal,
vec![
PointXYZINormal {
x: 0.0,
y: 1.0,
@ -340,12 +380,17 @@ fn converterxyzinormal() {
normal_y: f32::MAX,
normal_z: f32::MAX,
},
]);
]
);
}
#[test]
fn converterxyzrgbnormal() {
convert_from_into::<ConvertXYZRGBNormal, PointXYZRGBNormal>(vec![
convert_from_into!(
ReaderXYZRGBNormal,
WriterXYZRGBNormal,
PointXYZRGBNormal,
vec![
PointXYZRGBNormal {
x: 0.0,
y: 1.0,
@ -390,12 +435,17 @@ fn converterxyzrgbnormal() {
normal_y: f32::MAX,
normal_z: f32::MAX,
},
]);
]
);
}
#[test]
fn converterxyznormal() {
convert_from_into::<ConvertXYZNormal, PointXYZNormal>(vec![
convert_from_into!(
ReaderXYZNormal,
WriterXYZNormal,
PointXYZNormal,
vec![
PointXYZNormal {
x: 0.0,
y: 1.0,
@ -428,12 +478,17 @@ fn converterxyznormal() {
normal_y: f32::MAX,
normal_z: f32::MAX,
},
]);
]
);
}
#[test]
fn converterxyzrgbl() {
convert_from_into::<ConvertXYZRGBL, PointXYZRGBL>(vec![
convert_from_into!(
ReaderXYZRGBL,
WriterXYZRGBL,
PointXYZRGBL,
vec![
PointXYZRGBL {
x: 0.0,
y: 1.0,
@ -470,12 +525,17 @@ fn converterxyzrgbl() {
b: u8::MAX,
label: u32::MAX,
},
]);
]
);
}
#[test]
fn converterxyzrgb() {
convert_from_into::<ConvertXYZRGB, PointXYZRGB>(vec![
convert_from_into!(
ReaderXYZRGB,
WriterXYZRGB,
PointXYZRGB,
vec![
PointXYZRGB {
x: 0.0,
y: 1.0,
@ -508,12 +568,17 @@ fn converterxyzrgb() {
g: u8::MAX,
b: u8::MAX,
},
]);
]
);
}
#[test]
fn converterxyzl() {
convert_from_into::<ConvertXYZL, PointXYZL>(vec![
convert_from_into!(
ReaderXYZL,
WriterXYZL,
PointXYZL,
vec![
PointXYZL {
x: 0.0,
y: 1.0,
@ -538,12 +603,17 @@ fn converterxyzl() {
z: f32::MAX,
label: u32::MAX,
},
]);
]
);
}
#[test]
fn converterxyzi() {
convert_from_into::<ConvertXYZI, PointXYZI>(vec![
convert_from_into!(
ReaderXYZI,
WriterXYZI,
PointXYZI,
vec![
PointXYZI {
x: 0.0,
y: 1.0,
@ -568,5 +638,214 @@ fn converterxyzi() {
z: f32::MAX,
intensity: f32::MAX,
},
]);
]
);
}
#[test]
fn write_xyzi_read_xyz() {
let write_cloud = vec![
PointXYZI {
x: 0.0,
y: 1.0,
z: 5.0,
intensity: 0.0,
},
PointXYZI {
x: 1.0,
y: 1.5,
z: 5.0,
intensity: 1.0,
},
PointXYZI {
x: 1.3,
y: 1.6,
z: 5.7,
intensity: 2.0,
},
PointXYZI {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
intensity: f32::MAX,
},
];
let read_cloud = vec![
PointXYZ {
x: 0.0,
y: 1.0,
z: 5.0,
},
PointXYZ {
x: 1.0,
y: 1.5,
z: 5.0,
},
PointXYZ {
x: 1.3,
y: 1.6,
z: 5.7,
},
PointXYZ {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
},
];
convert_from_into_in_out_cloud!(
ReaderXYZ,
WriterXYZI,
PointXYZI,
write_cloud,
PointXYZI,
read_cloud,
PointXYZ
);
}
#[test]
fn write_less_than_available() {
const DIM: usize = 3;
const METADIM: usize = 0;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32,
y: f32,
z: f32,
dummy: f32,
}
type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
impl From<Point<f32, 3, 0>> for CustomPoint {
fn from(point: Point<f32, 3, 0>) -> Self {
Self {
x: point.coords[0],
y: point.coords[1],
z: point.coords[2],
dummy: 0.0,
}
}
}
impl From<CustomPoint> for Point<f32, 3, 0> {
fn from(point: CustomPoint) -> Self {
Point {
coords: [point.x, point.y, point.z],
meta: [],
}
}
}
impl MetaNames<METADIM> for CustomPoint {
fn meta_names() -> [&'static str; METADIM] {
[]
}
}
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
let write_cloud = vec![
CustomPoint {
x: 1.0,
y: 2.0,
z: 3.0,
dummy: -10.0,
},
CustomPoint {
x: 4.0,
y: 5.0,
z: 6.0,
dummy: -10.0,
},
CustomPoint {
x: 7.0,
y: 8.0,
z: 9.0,
dummy: -10.0,
},
];
let read_cloud = vec![
CustomPoint {
x: 1.0,
y: 2.0,
z: 3.0,
dummy: 0.0,
},
CustomPoint {
x: 4.0,
y: 5.0,
z: 6.0,
dummy: 0.0,
},
CustomPoint {
x: 7.0,
y: 8.0,
z: 9.0,
dummy: 0.0,
},
];
convert_from_into_in_out_cloud!(
MyReader,
MyWriter,
CustomPoint,
write_cloud,
CustomPoint,
read_cloud,
CustomPoint
);
}
#[test]
fn readme() {
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
// Your points (here using the predefined type PointXYZ).
let cloud_points = vec![
PointXYZ {
x: 1337.0,
y: 42.0,
z: 69.0,
},
PointXYZ {
x: f32::MAX,
y: f32::MIN,
z: f32::MAX,
},
];
// For equality test later
let cloud_copy = cloud_points.clone();
// Vector -> Writer -> Message
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points)
.try_into() // iterating points here O(n)
.unwrap();
// Convert to your ROS crate message type, we will use r2r here.
// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
// Publish ...
// ... now incoming from a topic.
// let internal_msg: PointCloud2Msg = msg.into();
// Message -> Reader. The Reader implements the Iterator trait.
let reader = ReaderXYZ::try_from(internal_msg).unwrap();
let new_cloud_points = reader
.map(|point: PointXYZ| {
// Some logic here
point
})
.collect::<Vec<PointXYZ>>();
assert_eq!(new_cloud_points, cloud_copy);
}

16
tests/r2r_msg_test.rs
View File

@ -1,10 +1,10 @@
#[cfg(feature = "r2r_msg")]
#[test]
fn convertxyz_r2r_msg() {
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::ConvertXYZ;
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
use r2r::sensor_msgs::msg::PointCloud2;
let cloud = vec![
@ -25,10 +25,10 @@ fn convertxyz_r2r_msg() {
},
];
let copy = cloud.clone();
let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap();
let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
let r2r_msg_cloud: PointCloud2 = internal_cloud.into();
let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into();
let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap();
let back_to_type = to_convert.map(|point| Ok(point)).collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type.unwrap());
let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap();
let back_to_type = to_convert.collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type);
}

15
tests/rosrust_msg_test.rs
View File

@ -1,10 +1,9 @@
#[cfg(feature = "rosrust_msg")]
#[test]
fn convertxyz_rosrust_msg() {
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::ros_types::PointCloud2Msg;
use ros_pointcloud2::ConvertXYZ;
use ros_pointcloud2::{
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
};
let cloud = vec![
PointXYZ {
@ -24,10 +23,10 @@ fn convertxyz_rosrust_msg() {
},
];
let copy = cloud.clone();
let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap();
let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into();
let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into();
let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap();
let back_to_type = to_convert.map(|point| Ok(point)).collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type.unwrap());
let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap();
let back_to_type = to_convert.collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type);
}