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

first commit

This commit is contained in:
Christopher Sieh 2023-04-25 18:30:54 +02:00
commit 36a8272e23
8 changed files with 1792 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
.github/workflows/noetic_20.04.yml vendored Normal file
View File

@ -0,0 +1,104 @@
name: Ubuntu 20.04 Noetic from rosrust
on:
push:
pull_request:
workflow_dispatch:
jobs:
build:
runs-on: ubuntu-20.04
env:
ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...]
# CI_SOURCE_PATH: $(pwd)
ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall
CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options
ROS_PARALLEL_JOBS: '-j8 -l6'
# Set the python path manually to include /usr/-/python2.7/dist-packages
# as this is where apt-get installs python packages.
PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages
ROS_DISTRO: noetic
steps:
- name: rosrust
uses: actions/checkout@v1
- name: Install latest rust
run: |
curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
rustc --version
cargo --version
- name: Configure ROS for install
run: |
sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt-get update -qq
sudo apt-get install dpkg
sudo apt-get install -y libyaml-cpp-dev
- name: Install ROS basic packages
run: |
sudo apt-get install -y python3-catkin-pkg
sudo apt-get install -y python3-catkin-tools
sudo apt-get install -y python3-rosdep
sudo apt-get install -y python3-wstool
sudo apt-get install -y python3-osrf-pycommon
sudo apt-get install -y ros-cmake-modules
sudo apt-get install -y ros-$ROS_DISTRO-ros-base
source /opt/ros/$ROS_DISTRO/setup.bash
sudo rosdep init
rosdep update # --include-eol-distros # Support EOL distros.
- name: Install ROS additional packages (TODO maybe don't need these)
# Does installing these mean rosrust_msg builds more messages, which is a better
# check? Or does it just slow down the action?
run: |
sudo apt-get install -y ros-$ROS_DISTRO-actionlib
sudo apt-get install -y ros-$ROS_DISTRO-actionlib-msgs
sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager
sudo apt-get install -y ros-$ROS_DISTRO-compressed-image-transport
sudo apt-get install -y ros-$ROS_DISTRO-catkin
sudo apt-get install -y ros-$ROS_DISTRO-class-loader
sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules
sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge
sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure
sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure
# Not in noetic yet
# sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure-python
sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions
sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs
sudo apt-get install -y ros-$ROS_DISTRO-genmsg
sudo apt-get install -y ros-$ROS_DISTRO-image-geometry
sudo apt-get install -y ros-$ROS_DISTRO-image-proc
sudo apt-get install -y ros-$ROS_DISTRO-image-transport
sudo apt-get install -y ros-$ROS_DISTRO-message-generation
sudo apt-get install -y ros-$ROS_DISTRO-message-runtime
# sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core
# sudo apt-get install -y ros-$ROS_DISTRO-nodelet-topic-tools
# sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions
# sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros
sudo apt-get install -y ros-$ROS_DISTRO-pluginlib
sudo apt-get install -y ros-$ROS_DISTRO-roscpp
sudo apt-get install -y ros-$ROS_DISTRO-roslib
sudo apt-get install -y ros-$ROS_DISTRO-roslint
sudo apt-get install -y ros-$ROS_DISTRO-rospy
sudo apt-get install -y ros-$ROS_DISTRO-rospy-message-converter
sudo apt-get install -y ros-$ROS_DISTRO-rostest
sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs
sudo apt-get install -y ros-$ROS_DISTRO-std-msgs
sudo apt-get install -y ros-$ROS_DISTRO-tf
sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions
sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs
sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs
sudo apt-get install -y ros-$ROS_DISTRO-tf2-py
sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros
sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs
- name: build
run: |
source /opt/ros/$ROS_DISTRO/setup.bash
cargo build # --verbose
- name: Install ROS packages for tests
run: |
sudo apt-get install -y ros-$ROS_DISTRO-actionlib-tutorials
sudo apt-get install -y ros-$ROS_DISTRO-roscpp-tutorials
sudo apt-get install -y ros-$ROS_DISTRO-rospy-tutorials
- name: test
run: |
source /opt/ros/$ROS_DISTRO/setup.bash
cargo test # --verbose

3
.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
Cargo.lock
.idea/
target/

21
Cargo.toml Normal file
View File

@ -0,0 +1,21 @@
[package]
name = "ros_pointcloud2"
version = "0.1.0"
edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"]
description = "A library for working with pointcloud2 messages in Rust"
repository = "https://github.com/stelzo/ros_pointcloud2"
license = "MIT"
keywords = ["ros", "rosrust", "pointcloud2", "pointcloud", "message",]
categories = ["science::robotics", "encoding", "data-structures", "api-bindings"]
readme = "README.md"
documentation = "https://docs.rs/ros_pointcloud2"
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"
rosrust_msg = "0.1.7"
fallible-iterator = "0.2.0"

21
LICENSE Normal file
View File

@ -0,0 +1,21 @@
The MIT License (MIT)
Copyright (c) 2023 Christopher Sieh
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

154
README.md Normal file
View File

@ -0,0 +1,154 @@
# ROS PointCloud2
A Rust implementation for fast, safe and customizable conversions to and from the `sensor_msgs/PointCloud2` ROS message.
Providing a fast and memory efficient way for message conversion while allowing user defined types without the cost of iterations.
*Currently only supports [rosrust_msg](https:://github.com/adnanademovic/rosrust) with ROS1.*
```rust
use ros_pointcloud2::ConvertXYZ;
use ros_pointcloud2::pcl_utils::PointXYZ;
use ros_pointcloud2::fallible_iterator::FallibleIterator;
use rosrust_msg::sensor_msgs::PointCloud2;
// 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 },
];
let cloud_copy = cloud_points.clone(); // Only for checking equality later.
// Vector -> Convert -> Message
let msg: PointCloud2 = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
// Message -> Convert -> Vector
let convert: ConvertXYZ = ConvertXYZ::try_from(msg).unwrap();
let new_cloud_points = convert.map(|point: PointXYZ| {
// Insert your point business logic here or use other methods like .for_each().
// I will just copy the points into a vector as an example.
// Also, since we are using a fallible iterator, we need to return a Result.
Ok(point)
}).collect::<Vec<PointXYZ>>()
.unwrap(); // Handle point conversion or byte errors here.
assert_eq!(new_cloud_points, cloud_copy);
```
Instead of converting the entire cloud into a `Vector`, you get an `Iterator` that converts each point from the message on the fly.
An example for using this crate is [this filter node](https://github.com/stelzo/cloudfilter). It is also a good starting point for
implementing ROS1 nodes in Rust inside a catkin environment.
## Features
- Full support for `sensor_msgs/PointCloud2` messages
- Custom types with `From` and `Into` traits
- Predefined types for common conversions (compared to PCL)
- PointXYZ
- PointXYZI
- PointXYZL
- PointXYZRGB
- PointXYZRGBA
- PointXYZRGBL
- PointXYZNormal
- PointXYZINormal
- PointXYZRGBNormal
- 2D and 3D support
## Usage
Add this to your Cargo.toml:
```toml
[dependencies]
ros_pointcloud2 = "0.1.0"
```
When building, the `rosrust_msg` crate needs to have the ROS environment sourced or use `ROSRUST_MSG_PATH=/opt/ros/$ROS_DISTRO/share cargo build`.
## Custom Types
You can freely convert to your own types without reiterating the entire cloud.
You just need to implement the `Into` and `From` traits.
```rust
use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::{Convert, MetaNames, PointMeta, ConversionError, PointConvertible};
use rosrust_msg::sensor_msgs::PointCloud2;
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 Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) {
([self.x, self.y, self.z], [PointMeta::new(self.r), PointMeta::new(self.g), PointMeta::new(self.b), PointMeta::new(self.a)])
}
}
// 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: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 },
CustomPoint { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, a: 1 },
CustomPoint { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, a: 2 },
CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX },
];
// Cloud -> ROS message
let custom_msg: PointCloud2 = MyConverter::try_from(custom_cloud).unwrap().try_into().unwrap();
// ROS message -> Cloud
let to_custom_type = MyConverter::try_from(custom_msg).unwrap();
```
## Future Work
- ROS2 support(!)
- Removing rosrust dependency
- 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
## License
[MIT](https://choosealicense.com/licenses/mit/)

806
src/lib.rs Normal file
View File

@ -0,0 +1,806 @@
#![doc = include_str!("../README.md")]
pub mod pcl_utils;
use num_traits::Zero;
use crate::pcl_utils::*;
#[macro_use]
pub extern crate mem_macros;
pub extern crate fallible_iterator;
/// Errors that can occur when converting between PointCloud2 and a custom type.
#[derive(Debug)]
pub enum ConversionError {
InvalidFieldFormat,
NotEnoughFields,
TooManyDimensions,
UnsupportedFieldType,
NoPoints,
DataLengthMismatch,
MetaIndexLengthMismatch,
EndOfBuffer,
PointConversionError,
MetaDatatypeMissmatch,
}
/// Trait to convert a point to a tuple of coordinates and meta data.
/// Implement this trait for your point type to use the conversion.
pub trait PointConvertible<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize>: TryInto<([T; DIM], [PointMeta; METADIM])> + TryFrom<([T; DIM], [PointMeta; METADIM])> + MetaNames<METADIM> + Clone {}
/// 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: &Vec<rosrust_msg::sensor_msgs::PointField>, xyz_field_type: &FieldDatatype) -> Result<rosrust_msg::sensor_msgs::PointField, 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 => {
return 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};;
/// 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: PointConvertible<T, SIZE, DIM, METADIM>> {
iteration: usize,
coordinates: Vec<C>,
_phantom: 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)>,
}
/// X, Y, Z in f32 (float) without meta data.
pub type ConvertXYZ = Convert<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
/// X, Y, Z in f32 (float), meta: intensity in f32.
pub type ConvertXYZI = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZI>;
/// X, Y, Z in f32 (float), meta: normal_x, normal_y, normal_z in f32.
pub type ConvertXYZNormal = Convert<f32, { size_of!(f32) }, 3, 3, PointXYZNormal>;
/// X, Y, Z in f32 (float), meta: r, g, b in u8.
pub type ConvertXYZRGB = Convert<f32, { size_of!(f32) }, 3, 3, PointXYZRGB>;
/// X, Y, Z in f32 (float), meta: r, g, b, a in u8.
pub type ConvertXYZRGBA = Convert<f32, { size_of!(f32) }, 3, 4, PointXYZRGBA>;
/// X, Y, Z in f32 (float), meta: r, g, b in u8, normal_x, normal_y, normal_z in f32.
pub type ConvertXYZRGBNormal = Convert<f32, { size_of!(f32) }, 3, 6, PointXYZRGBNormal>;
/// X, Y, Z in f32 (float), meta: intensity in f32, normal_x, normal_y, normal_z in f32.
pub type ConvertXYZINormal = Convert<f32, { size_of!(f32) }, 3, 4, PointXYZINormal>;
/// X, Y, Z in f32 (float), meta: label in u32.
pub type ConvertXYZL = Convert<f32, { size_of!(f32) }, 3, 1, PointXYZL>;
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryFrom<Vec<C>> for Convert<T, SIZE, DIM, METADIM, C>
{
type Error = ConversionError;
/// Converts a vector of custom types into a Convert struct that implements the Iterator trait.
///
/// # Example
/// ```
/// use rosrust_msg::sensor_msgs::PointCloud2;
/// 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.clone()));
point_step_size += datatype_size(&value.datatype);
}
Ok(Self {
_phantom: 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
/// datatype that is encoded so that it can be decoded later.
///
/// # Example
/// ```
/// use ros_pointcloud2::PointMeta;
///
/// let original_data: f64 = 1.0;
/// let meta = PointMeta::new(original_data);
/// let my_data: f64 = meta.get().unwrap();
/// ```
#[derive(Debug, Clone, Copy)]
pub struct PointMeta {
bytes: [u8; size_of!(f64)],
datatype: FieldDatatype,
}
impl Default for PointMeta {
fn default() -> Self {
Self {
bytes: [0; size_of!(f64)],
datatype: FieldDatatype::F32,
}
}
}
impl PointMeta {
/// Create a new PointMeta from a value
///
/// # Example
/// ```
/// let meta = ros_pointcloud2::PointMeta::new(1.0);
/// ```
pub fn new<T: FromBytes>(value: T) -> Self {
let raw_bytes = T::bytes(&value);
let mut bytes = [0; size_of!(f64)];
for (idx, byte) in raw_bytes.iter().enumerate() {
bytes[idx] = *byte;
}
Self {
bytes,
datatype: T::field_datatype(),
}
}
fn new_from_buffer(data: &Vec<u8>, offset: usize, datatype: &FieldDatatype) -> Result<Self, ConversionError> {
let size = datatype_size(datatype);
let bytes = data.get(offset..offset + size).ok_or(ConversionError::DataLengthMismatch)?;
let mut bytes_array = [0; size_of!(f64)]; // 8 bytes as f64 is the largest type
for (idx, byte) in bytes.iter().enumerate() {
bytes_array[idx] = *byte;
}
Ok(Self {
bytes: bytes_array,
datatype: datatype.clone(),
})
}
/// Get the value from the PointMeta. It will return None if the datatype does not match.
///
/// # Example
/// ```
/// let original_data: f64 = 1.0;
/// let meta = ros_pointcloud2::PointMeta::new(original_data);
/// let my_data: f64 = meta.get().unwrap();
/// ```
pub fn get<T: FromBytes>(&self) -> Result<T, ConversionError> {
if self.datatype != T::field_datatype() {
return Err(ConversionError::MetaDatatypeMissmatch);
}
let size = datatype_size(&T::field_datatype());
if let Some(bytes) = self.bytes.get(0..size) {
Ok(T::from_le_bytes(bytes))
} else {
Err(ConversionError::DataLengthMismatch) // self.bytes are not long enough, already handled while parsing
}
}
}
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryFrom<rosrust_msg::sensor_msgs::PointCloud2> for Convert<T, SIZE, DIM, METADIM, C>
{
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 rosrust_msg::sensor_msgs::PointCloud2;
/// 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: PointCloud2 = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
///
/// let convert: ConvertXYZ = ConvertXYZ::try_from(msg).unwrap(); // parse message
/// ```
fn try_from(cloud: rosrust_msg::sensor_msgs::PointCloud2) -> Result<Self, Self::Error> {
if cloud.fields.len() < 2 {
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.clone())).collect();
let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?;
let mut offsets = vec![
x_field.offset as usize,
y_field.offset as usize
];
let 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: 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: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> TryInto<rosrust_msg::sensor_msgs::PointCloud2> for Convert<T, SIZE, DIM, METADIM, C> {
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 rosrust_msg::sensor_msgs::PointCloud2;
/// 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: PointCloud2 = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap();
/// ```
fn try_into(self) -> Result<rosrust_msg::sensor_msgs::PointCloud2, Self::Error> {
let mut cloud = rosrust_msg::sensor_msgs::PointCloud2::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(rosrust_msg::sensor_msgs::PointField {
name: "x".to_string(),
offset: 0 * SIZE as u32,
datatype,
count: 1,
});
fields.push(rosrust_msg::sensor_msgs::PointField {
name: "y".to_string(),
offset: 1 * SIZE as u32,
datatype,
count: 1,
});
}
if DIM == 3 {
fields.push(rosrust_msg::sensor_msgs::PointField {
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(rosrust_msg::sensor_msgs::PointField {
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 as u32 * 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: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> Convert<T, SIZE, DIM, METADIM, C>
{
/// Convenience getter for the number of points in the cloud.
pub fn len(&self) -> usize {
self.cloud_length
}
}
impl<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C: PointConvertible<T, SIZE, DIM, METADIM>> fallible_iterator::FallibleIterator
for Convert<T, SIZE, DIM, METADIM, C>
{
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(_) => return 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 in 0..S {
let raw_byte = data.get(start_idx + byte);
match raw_byte {
None => {
return None;
}
Some(some_byte) => {
buff[byte] = some_byte.clone();
}
}
}
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);
}
}

446
src/pcl_utils.rs Normal file
View File

@ -0,0 +1,446 @@
use crate::{ConversionError, MetaNames, PointConvertible, PointMeta};
/// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZ {
pub x: f32,
pub y: f32,
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 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 MetaNames<0> for PointXYZ {
fn meta_names() -> [String; 0] {
[]
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 0> for PointXYZ {}
/// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an intensity value.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZI {
pub x: f32,
pub y: f32,
pub z: f32,
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 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().unwrap(),
})
}
}
impl MetaNames<1> for PointXYZI {
fn meta_names() -> [String; 1] {
["intensity"].map(|s| s.to_string())
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for PointXYZI {}
/// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an RGB color value.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZRGB {
pub x: f32,
pub y: f32,
pub z: f32,
pub r: u8,
pub g: u8,
pub b: u8,
}
impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZRGB {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
]))
}
}
impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZRGB {
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],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
})
}
}
impl MetaNames<3> for PointXYZRGB {
fn meta_names() -> [String; 3] {
["r", "g", "b"].map(|s| s.to_string())
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 3> 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.
/// The alpha channel is commonly used as padding but this crate uses every channel and no padding.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZRGBA {
pub x: f32,
pub y: f32,
pub z: f32,
pub r: u8,
pub g: u8,
pub b: u8,
pub a: u8,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBA {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
PointMeta::new(self.a),
]))
}
}
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBA {
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],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
a: data.1[3].get().unwrap(),
})
}
}
impl MetaNames<4> for PointXYZRGBA {
fn meta_names() -> [String; 4] {
[
"r",
"g",
"b",
"a",
].map(|s| s.to_string())
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 4> 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.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZRGBNormal {
pub x: f32,
pub y: f32,
pub z: f32,
pub r: u8,
pub g: u8,
pub b: u8,
pub normal_x: f32,
pub normal_y: f32,
pub normal_z: f32,
}
impl TryInto<([f32; 3], [PointMeta; 6])> for PointXYZRGBNormal {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 6]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
]))
}
}
impl TryFrom<([f32; 3], [PointMeta; 6])> for PointXYZRGBNormal {
type Error = ConversionError;
fn try_from(data: ([f32; 3], [PointMeta; 6])) -> Result<Self, Self::Error> {
Ok(Self {
x: data.0[0],
y: data.0[1],
z: data.0[2],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
normal_x: data.1[3].get().unwrap(),
normal_y: data.1[4].get().unwrap(),
normal_z: data.1[5].get().unwrap(),
})
}
}
impl MetaNames<6> for PointXYZRGBNormal {
fn meta_names() -> [String; 6] {
[
"r",
"g",
"b",
"normal_x",
"normal_y",
"normal_z",
].map(|s| s.to_string())
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 6> 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.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZINormal {
pub x: f32,
pub y: f32,
pub z: f32,
pub intensity: f32,
pub normal_x: f32,
pub normal_y: f32,
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 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().unwrap(),
normal_x: data.1[1].get().unwrap(),
normal_y: data.1[2].get().unwrap(),
normal_z: data.1[3].get().unwrap(),
})
}
}
impl MetaNames<4> for PointXYZINormal {
fn meta_names() -> [String; 4] {
[
"intensity",
"normal_x",
"normal_y",
"normal_z",
].map(|s| s.to_string())
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 4> for PointXYZINormal {}
/// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a label.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZL {
pub x: f32,
pub y: f32,
pub z: f32,
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 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().unwrap(),
})
}
}
impl MetaNames<1> for PointXYZL {
fn meta_names() -> [String; 1] {
["label".to_string()]
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 1> for PointXYZL {}
/// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a label.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZRGBL {
pub x: f32,
pub y: f32,
pub z: f32,
pub r: u8,
pub g: u8,
pub b: u8,
pub label: u32,
}
impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBL {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.r),
PointMeta::new(self.g),
PointMeta::new(self.b),
PointMeta::new(self.label),
]))
}
}
impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBL {
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],
r: data.1[0].get().unwrap(),
g: data.1[1].get().unwrap(),
b: data.1[2].get().unwrap(),
label: data.1[3].get().unwrap(),
})
}
}
impl MetaNames<4> for PointXYZRGBL {
fn meta_names() -> [String; 4] {
[
"r",
"g",
"b",
"label",
].map(|s| s.to_string())
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 4> 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.
#[derive(Clone, Debug, PartialEq, Copy)]
pub struct PointXYZNormal {
pub x: f32,
pub y: f32,
pub z: f32,
pub normal_x: f32,
pub normal_y: f32,
pub normal_z: f32,
}
impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZNormal {
type Error = ConversionError;
fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> {
Ok(([self.x, self.y, self.z], [
PointMeta::new(self.normal_x),
PointMeta::new(self.normal_y),
PointMeta::new(self.normal_z),
]))
}
}
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().unwrap(),
normal_y: data.1[1].get().unwrap(),
normal_z: data.1[2].get().unwrap(),
})
}
}
impl MetaNames<3> for PointXYZNormal {
fn meta_names() -> [String; 3] {
[
"normal_x",
"normal_y",
"normal_z",
].map(|s| s.to_string())
}
}
impl PointConvertible<f32, {size_of!(f32)}, 3, 3> for PointXYZNormal {}

237
tests/e2e_test.rs Normal file
View File

@ -0,0 +1,237 @@
use ros_pointcloud2::{ConversionError, Convert, ConvertXYZ, ConvertXYZINormal, ConvertXYZRGBA, ConvertXYZRGBNormal, MetaNames, PointConvertible, PointMeta};
use ros_pointcloud2::mem_macros::size_of;
use ros_pointcloud2::pcl_utils::{PointXYZ, PointXYZINormal, PointXYZRGBA, PointXYZRGBNormal};
use fallible_iterator::FallibleIterator;
#[test]
fn custom_xyz_f32() {
const DIM : usize = 3;
const METADIM : usize = 0;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32,
y: f32,
z: f32,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) {
([self.x, self.y, self.z], [])
}
}
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] {
[]
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![
CustomPoint { x: 1.0, y: 2.0, z: 3.0 },
CustomPoint { x: 4.0, y: 5.0, z: 6.0 },
CustomPoint { x: 7.0, y: 8.0, z: 9.0 },
];
let copy = custom_cloud.clone();
let custom_msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
let to_custom_type = to_custom_type.unwrap().map(|point| Ok(point)).collect::<Vec<CustomPoint>>();
assert_eq!(copy, to_custom_type.unwrap());
}
#[test]
fn custom_xyzi_f32() {
const DIM : usize = 3;
const METADIM : usize = 1;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32,
y: f32,
z: f32,
i: u8,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) {
([self.x, self.y, self.z], [PointMeta::new(self.i)])
}
}
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())
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![
CustomPoint { x: 0.0, y: 1.0, z: 5.0, i: 0 },
CustomPoint { x: 1.0, y: 1.5, z: 5.0, i: 1 },
CustomPoint { x: 1.3, y: 1.6, z: 5.7, i: 2 },
CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, i: u8::MAX },
];
let copy = custom_cloud.clone();
let custom_msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
let to_custom_type = to_custom_type.unwrap();
let back_to_type = to_custom_type.map(|point| Ok(point)).collect::<Vec<CustomPoint>>();
assert_eq!(copy, back_to_type.unwrap());
}
#[test]
fn custom_rgba_f32() {
const DIM : usize = 3;
const METADIM : usize = 4;
#[derive(Debug, PartialEq, Clone)]
struct CustomPoint {
x: f32,
y: f32,
z: f32,
r: u8,
g: u8,
b: u8,
a: u8,
}
type MyConverter = Convert<f32, { size_of!(f32) }, DIM, METADIM, CustomPoint>;
impl Into<([f32; DIM], [PointMeta; METADIM])> for CustomPoint {
fn into(self) -> ([f32; DIM], [PointMeta; METADIM]) {
([self.x, self.y, self.z], [PointMeta::new(self.r), PointMeta::new(self.g), PointMeta::new(self.b), PointMeta::new(self.a)])
}
}
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())
}
}
impl PointConvertible<f32, { size_of!(f32) }, DIM, METADIM> for CustomPoint {}
let custom_cloud = vec![
CustomPoint { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 },
CustomPoint { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, a: 1 },
CustomPoint { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, a: 2 },
CustomPoint { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX },
];
let copy = custom_cloud.clone();
let custom_msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = MyConverter::try_from(custom_cloud).unwrap().try_into();
assert!(custom_msg.is_ok());
let to_custom_type = MyConverter::try_from(custom_msg.unwrap());
assert!(to_custom_type.is_ok());
let to_custom_type = to_custom_type.unwrap();
let back_to_type = to_custom_type.map(|point| Ok(point)).collect::<Vec<CustomPoint>>();
assert_eq!(copy, back_to_type.unwrap());
}
#[test]
fn converterxyz() {
let cloud = vec![
PointXYZ { x: 0.0, y: 1.0, z: 5.0 },
PointXYZ { x: 1.0, y: 1.5, z: 5.0 },
PointXYZ { x: 1.3, y: 1.6, z: 5.7 },
PointXYZ { x: f32::MAX, y: f32::MIN, z: f32::MAX },
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZ::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyz_type = ConvertXYZ::try_from(msg.unwrap());
assert!(to_xyz_type.is_ok());
let to_xyz_type: ConvertXYZ = to_xyz_type.unwrap();
let back_to_type = to_xyz_type.map(|point: PointXYZ| Ok(point)).collect::<Vec<PointXYZ>>();
assert!(back_to_type.is_ok());
assert_eq!(copy, back_to_type.unwrap());
}
#[test]
fn converterxyzrgba() {
let cloud = vec![
PointXYZRGBA { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, a: 0 },
PointXYZRGBA { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, a: 1 },
PointXYZRGBA { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, a: 2 },
PointXYZRGBA { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, a: u8::MAX },
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZRGBA::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzrgba_type = ConvertXYZRGBA::try_from(msg.unwrap());
assert!(to_xyzrgba_type.is_ok());
let to_xyzrgba_type = to_xyzrgba_type.unwrap();
let back_to_type = to_xyzrgba_type.map(|point| Ok(point)).collect::<Vec<PointXYZRGBA>>();
assert_eq!(copy, back_to_type.unwrap());
}
#[test]
fn converterxyzinormal() {
let cloud = vec![
PointXYZINormal { x: 0.0, y: 1.0, z: 5.0, intensity: 0.0, normal_x: 0.0, normal_y: 0.0, normal_z: 0.0 },
PointXYZINormal { x: 1.0, y: 1.5, z: 5.0, intensity: 1.0, normal_x: 1.0, normal_y: 1.0, normal_z: 1.0 },
PointXYZINormal { x: 1.3, y: 1.6, z: 5.7, intensity: 2.0, normal_x: 2.0, normal_y: 2.0, normal_z: 2.0 },
PointXYZINormal { x: f32::MAX, y: f32::MIN, z: f32::MAX, intensity: f32::MAX, normal_x: f32::MAX, normal_y: f32::MAX, normal_z: f32::MAX },
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZINormal::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzinormal_type = ConvertXYZINormal::try_from(msg.unwrap());
assert!(to_xyzinormal_type.is_ok());
let to_xyzinormal_type = to_xyzinormal_type.unwrap();
let back_to_type = to_xyzinormal_type.map(|point| Ok(point)).collect::<Vec<PointXYZINormal>>();
assert_eq!(copy, back_to_type.unwrap());
}
#[test]
fn converterxyzrgbnormal() {
let cloud = vec![
PointXYZRGBNormal { x: 0.0, y: 1.0, z: 5.0, r: 0, g: 0, b: 0, normal_x: 0.0, normal_y: 0.0, normal_z: 0.0 },
PointXYZRGBNormal { x: 1.0, y: 1.5, z: 5.0, r: 1, g: 1, b: 1, normal_x: 1.0, normal_y: 1.0, normal_z: 1.0 },
PointXYZRGBNormal { x: 1.3, y: 1.6, z: 5.7, r: 2, g: 2, b: 2, normal_x: 2.0, normal_y: 2.0, normal_z: 2.0 },
PointXYZRGBNormal { x: f32::MAX, y: f32::MIN, z: f32::MAX, r: u8::MAX, g: u8::MAX, b: u8::MAX, normal_x: f32::MAX, normal_y: f32::MAX, normal_z: f32::MAX },
];
let copy = cloud.clone();
let msg: Result<rosrust_msg::sensor_msgs::PointCloud2, _> = ConvertXYZRGBNormal::try_from(cloud).unwrap().try_into();
assert!(msg.is_ok());
let to_xyzrgbnormal_type = ConvertXYZRGBNormal::try_from(msg.unwrap());
assert!(to_xyzrgbnormal_type.is_ok());
let to_xyzrgbnormal_type = to_xyzrgbnormal_type.unwrap();
let back_to_type = to_xyzrgbnormal_type.map(|point| Ok(point)).collect::<Vec<PointXYZRGBNormal>>();
assert_eq!(copy, back_to_type.unwrap());
}