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

merge

This commit is contained in:
stelzo 2024-06-24 11:28:38 +02:00
parent 4c0db66585 43f3d4e42b
commit e6b7649a88
9 changed files with 559 additions and 434 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
.github/workflows/rclrs_jazzy.yml vendored Normal file
View File

@ -0,0 +1,21 @@
name: rclrs_jazzy
on:
push:
branches:
- rclrs
pull_request:
branches:
- rclrs
workflow_dispatch:
env:
CARGO_TERM_COLOR: always
jobs:
tests_jazzy:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_rclrs_jazzy --tag rclrs_jazzy
- run: docker run rclrs_jazzy cargo test --features derive,nalgebra,rayon

13
Cargo.toml
View File

@ -1,6 +1,6 @@
[package] [package]
name = "ros_pointcloud2" name = "ros_pointcloud2"
version = "0.5.0-rc.1" version = "0.5.0"
edition = "2021" edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"] authors = ["Christopher Sieh <stelzo@steado.de>"]
description = "Customizable conversions for working with sensor_msgs/PointCloud2." description = "Customizable conversions for working with sensor_msgs/PointCloud2."
@ -32,11 +32,11 @@ rust-version = "1.63"
[dependencies] [dependencies]
rosrust_msg = { version = "0.1", optional = true } rosrust_msg = { version = "0.1", optional = true }
rosrust = { version = "0.9.11", optional = true } rosrust = { version = "0.9.11", optional = true }
r2r = { version = "0.8.4", optional = true } r2r = { version = "0.9", optional = true }
rayon = { version = "1", optional = true } rayon = { version = "1", optional = true }
nalgebra = { version = "0.32.5", optional = true, default-features = false } nalgebra = { version = "0.33", optional = true, default-features = false }
rpcl2-derive = { version = "0.3", optional = true } rpcl2-derive = { version = "0.3", optional = true }
type-layout = { version = "0.2", package = "type-layout-syn2", optional = true } memoffset = { version = "0.9", optional = true }
sensor_msgs = { version = "*", optional = true } sensor_msgs = { version = "*", optional = true }
std_msgs = { version = "*", optional = true } std_msgs = { version = "*", optional = true }
@ -45,17 +45,18 @@ builtin_interfaces = { version = "*", optional = true }
[dev-dependencies] [dev-dependencies]
rand = "0.8" rand = "0.8"
criterion = { version = "0.5", features = ["html_reports"] } criterion = { version = "0.5", features = ["html_reports"] }
pretty_assertions = "1.0"
[features] [features]
rclrs_msg = ["dep:sensor_msgs", "dep:std_msgs", "dep:builtin_interfaces"] rclrs_msg = ["dep:sensor_msgs", "dep:std_msgs", "dep:builtin_interfaces"]
rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"] rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
r2r_msg = ["dep:r2r"] r2r_msg = ["dep:r2r"]
rayon = ["dep:rayon"] rayon = ["dep:rayon"]
derive = ["dep:rpcl2-derive", "dep:type-layout"] derive = ["dep:rpcl2-derive", "dep:memoffset"]
nalgebra = ["dep:nalgebra"] nalgebra = ["dep:nalgebra"]
std = ["nalgebra/std"] std = ["nalgebra/std"]
default = ["std", "derive", "rclrs_msg"] default = ["std", "rclrs_msg"]
[package.metadata.docs.rs] [package.metadata.docs.rs]
features = ["derive", "nalgebra", "rayon"] features = ["derive", "nalgebra", "rayon"]

43
README.md
View File

@ -7,7 +7,7 @@
ros_pointcloud2 uses its own type for the message `PointCloud2Msg` to keep the library framework agnostic. ROS1 and ROS2 are supported with feature flags. ros_pointcloud2 uses its own type for the message `PointCloud2Msg` to keep the library framework agnostic. ROS1 and ROS2 are supported with feature flags.
Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.0-rc.1/) for a complete guide. Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.0/) for a complete guide.
## Quickstart ## Quickstart
@ -57,9 +57,9 @@ You can use `rosrust` and `r2r` by enabling the respective feature:
```toml ```toml
[dependencies] [dependencies]
ros_pointcloud2 = { version = "*", features = ["r2r_msg", "derive"]} ros_pointcloud2 = { version = "*", features = ["r2r_msg"]}
# or # or
ros_pointcloud2 = { version = "*", features = ["rosrust_msg", "derive"]} ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]}
``` ```
### rclrs (ros2_rust) ### rclrs (ros2_rust)
@ -68,7 +68,7 @@ Features do not work properly with `rcrls` because the messages are linked exter
```toml ```toml
[dependencies] [dependencies]
ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.0-rc.1_rclrs" } ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.0_rclrs" }
``` ```
Also, indicate the following dependencies to your linker inside the `package.xml` of your package. Also, indicate the following dependencies to your linker inside the `package.xml` of your package.
@ -84,32 +84,21 @@ Please open an issue or PR if you need other integrations.
## Performance ## Performance
This library offers a speed up when compared to PointCloudLibrary (PCL) conversions but the specific factor depends heavily on the use case and system. This library offers a speed up when compared to PointCloudLibrary (PCL) conversions but the specific factor depends heavily on the use case and system.
The `_vec` conversions are on average ~6x faster than PCL while the single core iteration `_iter` functions are around ~2x faster. See [this repository](https://github.com/stelzo/ros_pcl_conv_bench) for a detailed benchmark.
Parallelization with `_par_iter` gives a ~9x speed up compared to an OpenMP accelerated PCL pipeline.
The results are measured on an Intel i7-14700 with benchmarks from [this repository](https://github.com/stelzo/ros_pcl_conv_bench).
For minimizing the conversion overhead in general, always use the functions that best fit your use case. For minimizing the conversion overhead in general, always use the functions that best fit your use case.
## `no_std` Environments ### License
The `_iter` conversions are compatible with `#[no_std]` environments if an allocator is provided. This is due to the fact that names for point fields do not have a maximum length, and PointCloud2 data vectors can have arbitrary sizes. Use `default-features = false` to disable std for this crate. Only `nalgebra` can be added as an additional feature in this case. <sup>
Licensed under either of <a href="LICENSE-APACHE">Apache License, Version
2.0</a> or <a href="LICENSE-MIT">MIT license</a> at your option.
</sup>
## License <br>
Licensed under either of <sub>
Unless you explicitly state otherwise, any contribution intentionally submitted
- Apache License, Version 2.0, ([LICENSE-APACHE](LICENSE-APACHE) or http://www.apache.org/licenses/LICENSE-2.0) for inclusion in this crate by you, as defined in the Apache-2.0 license, shall
- MIT license ([LICENSE-MIT](LICENSE-MIT) or http://opensource.org/licenses/MIT) be dual licensed as above, without any additional terms or conditions.
</sub>
at your option.
### type-layout
For compatibility reasons, a patched version of `type-layout` is included in this repository. The original crate can be found [here](https://crates.io/crates/type-layout). After the patch is applied on the original `type-layout` crate ([PR](https://github.com/LPGhatguy/type-layout/pull/9)), the local dependency will be changed to the original crate.
`type-layout` is licensed under MIT or Apache-2.0 and Copyright by Lucien Greathouse. The changes are highlighted in the diff of the PR.
### Contribution
Unless you explicitly state otherwise, any contribution intentionally submitted for inclusion in the work by you, as defined in the Apache-2.0 license, shall be dual licensed as above, without any additional terms or conditions.

34
src/iterator.rs
View File

@ -1,6 +1,6 @@
//! Iterator implementations for [`PointCloud2Msg`] including a parallel iterator for rayon. //! Iterator implementations for [`PointCloud2Msg`] including a parallel iterator for rayon.
use crate::{ use crate::{
Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, Endian, FieldDatatype, MsgConversionError, PointCloud2Msg, PointConvertible, PointData,
RPCL2Point, RPCL2Point,
}; };
@ -27,12 +27,12 @@ use alloc::vec::Vec;
/// ///
pub struct PointCloudIterator<const N: usize, C> pub struct PointCloudIterator<const N: usize, C>
where where
C: Fields<N>, C: PointConvertible<N>,
{ {
iteration: usize, iteration: usize,
iteration_back: usize, iteration_back: usize,
data: ByteBufferView<N>, data: ByteBufferView<N>,
phantom_c: core::marker::PhantomData<C>, // internally used for pdata names array _phantom: core::marker::PhantomData<C>,
} }
#[cfg(feature = "rayon")] #[cfg(feature = "rayon")]
@ -250,7 +250,7 @@ impl<const N: usize> ByteBufferView<N> {
impl<const N: usize, C> TryFrom<PointCloud2Msg> for PointCloudIterator<N, C> impl<const N: usize, C> TryFrom<PointCloud2Msg> for PointCloudIterator<N, C>
where where
C: Fields<N>, C: PointConvertible<N>,
{ {
type Error = MsgConversionError; type Error = MsgConversionError;
@ -262,8 +262,10 @@ where
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> { fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N]; let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];
let not_found_fieldnames = C::field_names_ordered() let fields_only = crate::ordered_field_names::<N, C>();
.into_iter()
let not_found_fieldnames = fields_only
.iter()
.map(|name| { .map(|name| {
let found = cloud.fields.iter().any(|field| field.name == *name); let found = cloud.fields.iter().any(|field| field.name == *name);
(name, found) (name, found)
@ -279,9 +281,8 @@ where
return Err(MsgConversionError::FieldsNotFound(names_not_found)); return Err(MsgConversionError::FieldsNotFound(names_not_found));
} }
let ordered_fieldnames = C::field_names_ordered();
for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) { for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) {
if ordered_fieldnames.contains(&field.name.as_str()) { if fields_only.contains(&field.name) {
*with_offset = ( *with_offset = (
field.name.clone(), field.name.clone(),
field.datatype.try_into()?, field.datatype.try_into()?,
@ -318,9 +319,16 @@ where
return Err(MsgConversionError::DataLengthMismatch); return Err(MsgConversionError::DataLengthMismatch);
} }
let last_offset = offsets.last().expect("Dimensionality is 0."); let last_offset = match offsets.last() {
Some(offset) => *offset,
None => return Err(MsgConversionError::DataLengthMismatch),
};
let last_pdata = match pdata.last() {
Some(pdata) => pdata,
None => return Err(MsgConversionError::DataLengthMismatch),
};
let last_pdata = pdata.last().expect("Dimensionality is 0.");
let size_with_last_pdata = last_offset + last_pdata.1.size(); let size_with_last_pdata = last_offset + last_pdata.1.size();
if size_with_last_pdata > point_step_size { if size_with_last_pdata > point_step_size {
return Err(MsgConversionError::DataLengthMismatch); return Err(MsgConversionError::DataLengthMismatch);
@ -342,14 +350,14 @@ where
iteration: 0, iteration: 0,
iteration_back: cloud_length - 1, iteration_back: cloud_length - 1,
data, data,
phantom_c: core::marker::PhantomData, _phantom: core::marker::PhantomData,
}) })
} }
} }
impl<const N: usize, C> PointCloudIterator<N, C> impl<const N: usize, C> PointCloudIterator<N, C>
where where
C: Fields<N>, C: PointConvertible<N>,
{ {
#[inline] #[inline]
#[must_use] #[must_use]
@ -358,7 +366,7 @@ where
iteration: 0, iteration: 0,
iteration_back: data.len() - 1, iteration_back: data.len() - 1,
data, data,
phantom_c: core::marker::PhantomData, _phantom: core::marker::PhantomData,
} }
} }

436
src/lib.rs
View File

@ -3,19 +3,19 @@
//! The library provides the [`PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators. //! The library provides the [`PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators.
//! //!
//! Vector conversions are optimized for direct copy. They are useful when you just move similar data around. They are usually a good default. //! Vector conversions are optimized for direct copy. They are useful when you just move similar data around. They are usually a good default.
//! - [`try_from_vec`](PointCloud2Msg::try_from_vec) requires `derive` feature (enabled by default) //! - [`try_from_vec`](PointCloud2Msg::try_from_vec)
//! - [`try_into_vec`](PointCloud2Msg::try_into_vec) requires `derive` feature (enabled by default) //! - [`try_into_vec`](PointCloud2Msg::try_into_vec)
//! //!
//! You can use the iterator functions for more control over the conversion process. //! You can use the iterator functions for more control over the conversion process.
//! - [`try_from_iter`](PointCloud2Msg::try_from_iter) //! - [`try_from_iter`](PointCloud2Msg::try_from_iter)
//! - [`try_into_iter`](PointCloud2Msg::try_into_iter) //! - [`try_into_iter`](PointCloud2Msg::try_into_iter)
//! //!
//! These feature predictable performance but they do not scale well with large clouds. Learn more about that in the [performance section](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#performance) of the repository. //! These feature predictable performance but they do not scale well with large clouds. Learn more about that in the [performance section](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#performance) of the repository.
//! The iterators are useful when your conversions are more complex than a simple copy or you the cloud is small enough. //! The iterators are useful when your conversions are more complex than a simple copy or the cloud is small enough.
//! //!
//! When the cloud is getting larger or you are doing a lot of processing per point, switch to the parallel iterators. //! When the cloud is getting larger or you are doing a lot of processing per point, switch to the parallel iterators.
//! - [`try_into_par_iter`](PointCloud2Msg::try_into_par_iter) requires `rayon` feature //! - [`try_into_par_iter`](PointCloud2Msg::try_into_par_iter) requires `rayon` feature
//! - [`try_from_par_iter`](PointCloud2Msg::try_from_par_iter) requires `rayon` + `derive` feature //! - [`try_from_par_iter`](PointCloud2Msg::try_from_par_iter) requires `rayon` feature
//! //!
//! For ROS interoperability, there are integrations avialable with feature flags. If you miss a message type, please open an issue or a PR. //! For ROS interoperability, there are integrations avialable with feature flags. If you miss a message type, please open an issue or a PR.
//! See the [`ros`] module for more information on how to integrate more libraries. //! See the [`ros`] module for more information on how to integrate more libraries.
@ -56,24 +56,37 @@
//! - r2r_msg — Integration for the ROS2 library [r2r](https://github.com/sequenceplanner/r2r). //! - r2r_msg — Integration for the ROS2 library [r2r](https://github.com/sequenceplanner/r2r).
//! - rosrust_msg — Integration with the [rosrust](https://github.com/adnanademovic/rosrust) library for ROS1 message types. //! - rosrust_msg — Integration with the [rosrust](https://github.com/adnanademovic/rosrust) library for ROS1 message types.
//! - (rclrs_msg) — Integration for ROS2 [rclrs](https://github.com/ros2-rust/ros2_rust) but it currently needs [this workaround](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#rclrs-ros2_rust). //! - (rclrs_msg) — Integration for ROS2 [rclrs](https://github.com/ros2-rust/ros2_rust) but it currently needs [this workaround](https://github.com/stelzo/ros_pointcloud2?tab=readme-ov-file#rclrs-ros2_rust).
//! - derive *(enabled by default)* — Enables the `_vec` functions and offers helpful custom point derive macros for the [`PointConvertible`] trait. //! - derive — Offers implementations for the [`PointConvertible`] trait needed for custom points.
//! - rayon — Parallel iterator support for `_par_iter` functions. [`PointCloud2Msg::try_from_par_iter`] additionally needs the 'derive' feature. //! - rayon — Parallel iterator support for `_par_iter` functions.
//! - nalgebra — Predefined points offer a nalgebra typed getter for coordinates (e.g. [`xyz`](points::PointXYZ::xyz)). //! - nalgebra — Predefined points offer a nalgebra typed getter for coordinates (e.g. [`xyz`](points::PointXYZ::xyz)).
//! - std *(enabled by default)* — Use the standard library. `no_std` only works standalone or with the 'nalgebra' feature. //! - std *(enabled by default)* — Omit this feature to use this library in no_std environments. ROS integrations and 'rayon' will not work with no_std.
//! //!
//! # Custom Points //! # Custom Points
//! Implement [`PointConvertible`] for your point with the `derive` feature or manually. //! Implement [`PointConvertible`] for your point with the `derive` feature or manually.
//! //!
//! ``` //! ## Derive (recommended)
//! use ros_pointcloud2::prelude::*; //! ```ignore
//! //! #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible)]
//! #[cfg_attr(feature = "derive", derive(PointConvertible, TypeLayout))] //! #[repr(C, align(4))]
//! #[derive(Clone, Debug, PartialEq, Copy, Default)] //! pub struct MyPointXYZI {
//! pub x: f32,
//! pub y: f32,
//! pub z: f32,
//! #[rpcl2(rename("i"))]
//! pub intensity: f32,
//! }
//! ```
//!
//! ## Manual
//! ```
//! use ros_pointcloud2::prelude::*;
//!
//! #[derive(Clone, Debug, PartialEq, Copy, Default)]
//! #[repr(C, align(4))]
//! pub struct MyPointXYZI { //! pub struct MyPointXYZI {
//! pub x: f32, //! pub x: f32,
//! pub y: f32, //! pub y: f32,
//! pub z: f32, //! pub z: f32,
//! #[cfg_attr(feature = "derive", rpcl2(rename("i")))]
//! pub intensity: f32, //! pub intensity: f32,
//! } //! }
//! //!
@ -83,30 +96,28 @@
//! } //! }
//! } //! }
//! //!
//! // Manual implementation of PointConvertible without derive.
//! #[cfg(not(feature = "derive"))]
//! impl Fields<4> for MyPointXYZI {
//! fn field_names_ordered() -> [&'static str; 4] {
//! ["x", "y", "z", "i"] // Note the different field name for intensity.
//! }
//! }
//!
//! #[cfg(not(feature = "derive"))]
//! impl From<RPCL2Point<4>> for MyPointXYZI { //! impl From<RPCL2Point<4>> for MyPointXYZI {
//! fn from(point: RPCL2Point<4>) -> Self { //! fn from(point: RPCL2Point<4>) -> Self {
//! Self::new(point[0].get(), point[1].get(), point[2].get(), point[3].get()) //! Self::new(point[0].get(), point[1].get(), point[2].get(), point[3].get())
//! } //! }
//! } //! }
//! //!
//! #[cfg(not(feature = "derive"))]
//! impl From<MyPointXYZI> for RPCL2Point<4> { //! impl From<MyPointXYZI> for RPCL2Point<4> {
//! fn from(point: MyPointXYZI) -> Self { //! fn from(point: MyPointXYZI) -> Self {
//! [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into() //! [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
//! } //! }
//! } //! }
//! //!
//! #[cfg(not(feature = "derive"))] //! unsafe impl PointConvertible<4> for MyPointXYZI {
//! impl PointConvertible<4> for MyPointXYZI {} //! fn layout() -> LayoutDescription {
//! LayoutDescription::new(&[
//! LayoutField::new("x", "f32", 4),
//! LayoutField::new("y", "f32", 4),
//! LayoutField::new("z", "f32", 4),
//! LayoutField::new("intensity", "f32", 4),
//! ])
//! }
//! }
//! //!
//! let first_p = MyPointXYZI::new(1.0, 2.0, 3.0, 0.5); //! let first_p = MyPointXYZI::new(1.0, 2.0, 3.0, 0.5);
//! let cloud_points = vec![first_p, MyPointXYZI::new(4.0, 5.0, 6.0, 0.5)]; //! let cloud_points = vec![first_p, MyPointXYZI::new(4.0, 5.0, 6.0, 0.5)];
@ -114,32 +125,13 @@
//! let cloud_points_out: Vec<MyPointXYZI> = msg_out.try_into_iter().unwrap().collect(); //! let cloud_points_out: Vec<MyPointXYZI> = msg_out.try_into_iter().unwrap().collect();
//! assert_eq!(first_p, *cloud_points_out.first().unwrap()); //! assert_eq!(first_p, *cloud_points_out.first().unwrap());
//! ``` //! ```
//!
//! An example without `#[cfg_attr]` looks like this:
//! ```ignore
//! #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)]
//! pub struct MyPointXYZI {
//! pub x: f32,
//! pub y: f32,
//! pub z: f32,
//! #[rpcl2(rename("i"))]
//! pub intensity: f32,
//! }
//! ```
#![crate_type = "lib"] #![crate_type = "lib"]
#![cfg_attr(docsrs, feature(doc_cfg))] #![cfg_attr(docsrs, feature(doc_cfg))]
#![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.0-rc.1")] #![doc(html_root_url = "https://docs.rs/ros_pointcloud2/0.5.0")]
#![warn(clippy::print_stderr)] #![warn(clippy::print_stderr)]
#![warn(clippy::print_stdout)] #![warn(clippy::print_stdout)]
#![warn(clippy::unwrap_used)] #![warn(clippy::unwrap_used)]
#![warn(clippy::cargo)] #![warn(clippy::expect_used)]
#![warn(clippy::std_instead_of_core)]
#![warn(clippy::alloc_instead_of_core)]
#![warn(clippy::std_instead_of_alloc)]
#![cfg_attr(not(feature = "std"), no_std)]
// Setup an allocator with #[global_allocator]
// see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html
#![warn(clippy::unwrap_used)]
#![warn(clippy::cargo)] #![warn(clippy::cargo)]
#![warn(clippy::std_instead_of_core)] #![warn(clippy::std_instead_of_core)]
#![warn(clippy::alloc_instead_of_core)] #![warn(clippy::alloc_instead_of_core)]
@ -157,6 +149,9 @@ pub mod iterator;
use crate::ros::{HeaderMsg, PointFieldMsg}; use crate::ros::{HeaderMsg, PointFieldMsg};
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
#[doc(hidden)]
pub use memoffset;
use core::str::FromStr; use core::str::FromStr;
#[macro_use] #[macro_use]
@ -176,6 +171,7 @@ pub enum MsgConversionError {
FieldsNotFound(Vec<String>), FieldsNotFound(Vec<String>),
UnsupportedFieldCount, UnsupportedFieldCount,
NumberConversion, NumberConversion,
ExhaustedSource,
} }
impl From<core::num::TryFromIntError> for MsgConversionError { impl From<core::num::TryFromIntError> for MsgConversionError {
@ -219,10 +215,17 @@ impl core::fmt::Display for MsgConversionError {
MsgConversionError::NumberConversion => { MsgConversionError::NumberConversion => {
write!(f, "The number is too large to be converted into a PointCloud2 supported datatype.") write!(f, "The number is too large to be converted into a PointCloud2 supported datatype.")
} }
MsgConversionError::ExhaustedSource => {
write!(
f,
"The conversion requests more data from the source type than is available."
)
}
} }
} }
} }
#[allow(clippy::std_instead_of_core)] // will be stable soon (https://github.com/rust-lang/rust/issues/103765)
#[cfg(feature = "std")] #[cfg(feature = "std")]
impl std::error::Error for MsgConversionError { impl std::error::Error for MsgConversionError {
fn source(&self) -> Option<&(dyn std::error::Error + 'static)> { fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
@ -230,7 +233,6 @@ impl std::error::Error for MsgConversionError {
} }
} }
#[cfg(feature = "derive")]
fn system_endian() -> Endian { fn system_endian() -> Endian {
if cfg!(target_endian = "big") { if cfg!(target_endian = "big") {
Endian::Big Endian::Big
@ -241,6 +243,43 @@ fn system_endian() -> Endian {
} }
} }
/// Description of the memory layout of a type with named fields.
#[derive(Clone, Debug)]
pub struct LayoutDescription(Vec<LayoutField>);
impl LayoutDescription {
pub fn new(fields: &[LayoutField]) -> Self {
Self(fields.into())
}
}
/// Enum to describe the field type and size in a padded or unpadded layout.
#[derive(Clone, Debug)]
pub enum LayoutField {
Field {
name: alloc::borrow::Cow<'static, str>,
ty: alloc::borrow::Cow<'static, str>,
size: usize,
},
Padding {
size: usize,
},
}
impl LayoutField {
pub fn new(name: &'static str, ty: &'static str, size: usize) -> Self {
LayoutField::Field {
name: name.into(),
ty: ty.into(),
size,
}
}
pub fn padding(size: usize) -> Self {
LayoutField::Padding { size }
}
}
/// The intermediate point cloud type for ROS integrations. /// The intermediate point cloud type for ROS integrations.
/// ///
/// To assert consistency, the type should be build with the [`PointCloud2MsgBuilder`]. /// To assert consistency, the type should be build with the [`PointCloud2MsgBuilder`].
@ -273,7 +312,6 @@ pub enum Denseness {
Sparse, Sparse,
} }
#[cfg(feature = "derive")]
enum ByteSimilarity { enum ByteSimilarity {
Equal, Equal,
Overlapping, Overlapping,
@ -428,36 +466,61 @@ pub struct CloudDimensions {
pub height: u32, pub height: u32,
} }
fn ordered_field_names<const N: usize, C: PointConvertible<N>>() -> Vec<String> {
C::layout()
.0
.iter()
.filter(|field| {
matches!(
field,
LayoutField::Field {
name: _,
ty: _,
size: _,
}
)
})
.map(|field| match field {
LayoutField::Field {
name,
ty: _,
size: _,
} => name.as_ref().into(),
_ => unreachable!("Fields must be filtered before."),
})
.collect()
}
impl PointCloud2Msg { impl PointCloud2Msg {
#[cfg(feature = "derive")]
#[inline] #[inline]
fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError> fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError>
where where
C: PointConvertible<N>, C: PointConvertible<N>,
{ {
let point: RPCL2Point<N> = C::default().into(); let field_names = ordered_field_names::<N, C>();
debug_assert!(point.fields.len() == N); let target_layout = KnownLayoutInfo::try_from(C::layout())?;
let field_names = C::field_names_ordered(); debug_assert!(field_names.len() <= target_layout.fields.len());
debug_assert!(field_names.len() == N); debug_assert!(self.fields.len() <= target_layout.fields.len());
let layout = TypeLayoutInfo::try_from(C::type_layout())?;
debug_assert!(field_names.len() == layout.fields.len());
let mut offset: u32 = 0; let mut offset: u32 = 0;
let mut field_counter = 0; let mut field_counter = 0;
for (f, msg_f) in layout.fields.iter().zip(self.fields.iter()) { for f in target_layout.fields.iter() {
match f { match f {
PointField::Field { PointField::Field {
datatype, datatype,
size, size,
count, count,
} => { } => {
let f_translated = String::from_str(field_names[field_counter]) if field_counter >= self.fields.len() || field_counter >= field_names.len() {
.expect("Field name is not a valid string."); return Err(MsgConversionError::ExhaustedSource);
}
let msg_f = unsafe { self.fields.get_unchecked(field_counter) };
let f_translated = unsafe { field_names.get_unchecked(field_counter) };
field_counter += 1; field_counter += 1;
if msg_f.name != f_translated if msg_f.name != *f_translated
|| msg_f.offset != offset || msg_f.offset != offset
|| msg_f.datatype != *datatype || msg_f.datatype != *datatype
|| msg_f.count != 1 || msg_f.count != 1
@ -480,7 +543,7 @@ impl PointCloud2Msg {
}) })
} }
/// Create a [`PointCloud2Msg`] from any iterable type. /// Create a [`PointCloud2Msg`] from any iterable type that implements [`PointConvertible`].
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -503,7 +566,7 @@ impl PointCloud2Msg {
let point: RPCL2Point<N> = C::default().into(); let point: RPCL2Point<N> = C::default().into();
debug_assert!(point.fields.len() == N); debug_assert!(point.fields.len() == N);
let field_names = C::field_names_ordered(); let field_names = crate::ordered_field_names::<N, C>();
debug_assert!(field_names.len() == N); debug_assert!(field_names.len() == N);
let mut pdata_offsets_acc: u32 = 0; let mut pdata_offsets_acc: u32 = 0;
@ -519,7 +582,7 @@ impl PointCloud2Msg {
let _ = FieldDatatype::try_from(datatype_code)?; let _ = FieldDatatype::try_from(datatype_code)?;
*field_val = PointFieldMsg { *field_val = PointFieldMsg {
name: field_name.into(), name: field_name,
offset: pdata_offsets_acc, offset: pdata_offsets_acc,
datatype: datatype_code, datatype: datatype_code,
count: 1, count: 1,
@ -557,8 +620,8 @@ impl PointCloud2Msg {
} }
/// Create a PointCloud2Msg from a parallel iterator. Requires the `rayon` and `derive` feature to be enabled. /// Create a PointCloud2Msg from a parallel iterator. Requires the `rayon` and `derive` feature to be enabled.
#[cfg(all(feature = "rayon", feature = "derive"))] #[cfg(feature = "rayon")]
#[cfg_attr(docsrs, doc(cfg(all(feature = "rayon", feature = "derive"))))] #[cfg_attr(docsrs, doc(cfg(feature = "rayon")))]
pub fn try_from_par_iter<const N: usize, C>( pub fn try_from_par_iter<const N: usize, C>(
iterable: impl rayon::iter::ParallelIterator<Item = C>, iterable: impl rayon::iter::ParallelIterator<Item = C>,
) -> Result<Self, MsgConversionError> ) -> Result<Self, MsgConversionError>
@ -585,8 +648,6 @@ impl PointCloud2Msg {
/// ///
/// # Errors /// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies. /// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
#[cfg(feature = "derive")]
#[cfg_attr(docsrs, doc(cfg(feature = "derive")))]
pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError> pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError>
where where
C: PointConvertible<N>, C: PointConvertible<N>,
@ -597,17 +658,15 @@ impl PointCloud2Msg {
let point: RPCL2Point<N> = C::default().into(); let point: RPCL2Point<N> = C::default().into();
debug_assert!(point.fields.len() == N); debug_assert!(point.fields.len() == N);
let field_names = C::field_names_ordered(); let field_names = crate::ordered_field_names::<N, C>();
debug_assert!(field_names.len() == N); debug_assert!(field_names.len() == N);
let layout = TypeLayoutInfo::try_from(C::type_layout())?; let layout = KnownLayoutInfo::try_from(C::layout())?;
debug_assert!(field_names.len() == layout.fields.len()); debug_assert!(field_names.len() <= layout.fields.len());
let mut offset = 0; let mut offset = 0;
let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(layout.fields.len()); let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(field_names.len());
for f in layout.fields.into_iter() { for f in layout.fields.into_iter() {
let f_translated = String::from_str(field_names[fields.len()])
.expect("Field name is not a valid string.");
match f { match f {
PointField::Field { PointField::Field {
datatype, datatype,
@ -615,7 +674,7 @@ impl PointCloud2Msg {
count, count,
} => { } => {
fields.push(PointFieldMsg { fields.push(PointFieldMsg {
name: f_translated, name: field_names[fields.len()].clone(),
offset, offset,
datatype, datatype,
..Default::default() ..Default::default()
@ -638,7 +697,7 @@ impl PointCloud2Msg {
let bytes_total = vec.len() * point_step as usize; let bytes_total = vec.len() * point_step as usize;
cloud.data.resize(bytes_total, u8::default()); cloud.data.resize(bytes_total, u8::default());
let raw_data: *mut C = cloud.data.as_ptr() as *mut C; let raw_data: *mut C = cloud.data.as_mut_ptr() as *mut C;
unsafe { unsafe {
core::ptr::copy_nonoverlapping( core::ptr::copy_nonoverlapping(
vec.as_ptr().cast::<u8>(), vec.as_ptr().cast::<u8>(),
@ -674,8 +733,6 @@ impl PointCloud2Msg {
/// ///
/// # Errors /// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies. /// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
#[cfg(feature = "derive")]
#[cfg_attr(docsrs, doc(cfg(feature = "derive")))]
pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError> pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError>
where where
C: PointConvertible<N>, C: PointConvertible<N>,
@ -778,14 +835,6 @@ pub struct RPCL2Point<const N: usize> {
fields: [PointData; N], fields: [PointData; N],
} }
impl<const N: usize> Default for RPCL2Point<N> {
fn default() -> Self {
Self {
fields: [PointData::default(); N],
}
}
}
impl<const N: usize> core::ops::Index<usize> for RPCL2Point<N> { impl<const N: usize> core::ops::Index<usize> for RPCL2Point<N> {
type Output = PointData; type Output = PointData;
@ -802,157 +851,100 @@ impl<const N: usize> From<[PointData; N]> for RPCL2Point<N> {
/// Trait to enable point conversions on the fly. /// Trait to enable point conversions on the fly.
/// ///
/// # Example /// Implement this trait for your custom point you want to read or write in the message.
/// It is strongly recommended to enable the `derive` feature and use the `#[derive(PointConvertible)]` macro.
/// This prevents common errors when implementing this trait by hand.
///
/// Be aware that Rust does not guarantee the memory layout of structs. Learn more [here](https://doc.rust-lang.org/reference/type-layout.html).
/// To make layouting more predictable and thus faster for C++ node interactions, use the `#[repr(C)]` attribute on your struct.
/// An example for diverging point layouts with padding can be seen in the source code of [this](points::PointXYZRGBA::layout) implementation.
///
/// The generic parameter `N` is the number of fields in the point type. There can be more (hidden) fields that pad the layout but they do not count for the N.
/// For
///
/// # Derive
/// ```ignore
/// use ros_pointcloud2::prelude::*;
///
/// #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible)]
/// #[repr(C, align(4))]
/// pub struct MyPointXYZL {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// #[rpcl2(rename("l"))]
/// pub label: u8,
/// }
/// ```
///
/// # Manual
/// ``` /// ```
/// use ros_pointcloud2::prelude::*; /// use ros_pointcloud2::prelude::*;
/// ///
/// #[derive(Clone, Debug, PartialEq, Copy, Default)] /// #[derive(Clone, Debug, PartialEq, Copy, Default)]
/// pub struct MyPointXYZI { /// #[repr(C, align(4))]
/// pub struct MyPointXYZL {
/// pub x: f32, /// pub x: f32,
/// pub y: f32, /// pub y: f32,
/// pub z: f32, /// pub z: f32,
/// pub intensity: f32, /// pub label: u8,
/// } /// }
/// ///
/// impl From<MyPointXYZI> for RPCL2Point<4> { /// impl From<MyPointXYZL> for RPCL2Point<4> {
/// fn from(point: MyPointXYZI) -> Self { /// fn from(point: MyPointXYZL) -> Self {
/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into() /// [point.x.into(), point.y.into(), point.z.into(), point.label.into()].into()
/// } /// }
/// } /// }
/// ///
/// impl From<RPCL2Point<4>> for MyPointXYZI { /// impl From<RPCL2Point<4>> for MyPointXYZL {
/// fn from(point: RPCL2Point<4>) -> Self { /// fn from(point: RPCL2Point<4>) -> Self {
/// Self { /// Self {
/// x: point[0].get(), /// x: point[0].get(),
/// y: point[1].get(), /// y: point[1].get(),
/// z: point[2].get(), /// z: point[2].get(),
/// intensity: point[3].get(), /// label: point[3].get(),
/// } /// }
/// } /// }
/// } /// }
/// ///
/// impl Fields<4> for MyPointXYZI { /// unsafe impl PointConvertible<4> for MyPointXYZL {
/// fn field_names_ordered() -> [&'static str; 4] { /// fn layout() -> LayoutDescription {
/// ["x", "y", "z", "intensity"] /// LayoutDescription::new(&[
/// } /// LayoutField::new("x", "f32", 4),
/// } /// LayoutField::new("y", "f32", 4),
/// /// LayoutField::new("z", "f32", 4),
/// impl PointConvertible<4> for MyPointXYZI {} /// LayoutField::new("l", "u8", 1),
/// ``` /// LayoutField::padding(3),
#[cfg_attr(docsrs, doc(cfg(not(feature = "derive"))))] /// ])
#[cfg(not(feature = "derive"))]
pub trait PointConvertible<const N: usize>:
From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Clone + Default
{
}
/// Trait to enable point conversions on the fly.
///
/// Implement this trait for your custom point you want to read or write in the message.
/// For a more convenient way to implement this trait, enable the `derive` feature and use the `#[derive(PointConvertible, TypeLayout)]` macro.
///
/// # Derive Example
/// ```
/// use ros_pointcloud2::prelude::*;
///
/// #[derive(Clone, Debug, PartialEq, Copy, Default, PointConvertible, TypeLayout)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
/// ```
///
/// # Manual Example
/// ```
/// use ros_pointcloud2::prelude::*;
///
/// #[derive(Clone, Debug, PartialEq, Copy, Default, TypeLayout)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
///
/// impl From<MyPointXYZI> for RPCL2Point<4> {
/// fn from(point: MyPointXYZI) -> Self {
/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
/// } /// }
/// } /// }
///
/// impl From<RPCL2Point<4>> for MyPointXYZI {
/// fn from(point: RPCL2Point<4>) -> Self {
/// Self {
/// x: point[0].get(),
/// y: point[1].get(),
/// z: point[2].get(),
/// intensity: point[3].get(),
/// }
/// }
/// }
///
/// impl Fields<4> for MyPointXYZI {
/// fn field_names_ordered() -> [&'static str; 4] {
/// ["x", "y", "z", "intensity"]
/// }
/// }
///
/// impl PointConvertible<4> for MyPointXYZI {}
/// ``` /// ```
#[cfg_attr(docsrs, doc(cfg(feature = "derive")))] /// # Safety
#[cfg(feature = "derive")] /// The layout is used for raw memory interpretation, where safety can not be guaranteed by the compiler.
pub trait PointConvertible<const N: usize>: /// Take care when implementing the layout, especially in combination with `#[repr]` or use the `derive` feature when possible to prevent common errors.
type_layout::TypeLayout + From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Default pub unsafe trait PointConvertible<const N: usize>:
From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Default + Sized
{ {
fn layout() -> LayoutDescription;
} }
/// Matching field names from each data point. #[derive(Debug, Clone)]
/// 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
/// ```
/// use ros_pointcloud2::prelude::*;
///
/// #[derive(Clone, Debug, PartialEq, Copy)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
/// pub z: f32,
/// pub intensity: f32,
/// }
///
/// impl Fields<4> for MyPointXYZI {
/// fn field_names_ordered() -> [&'static str; 4] {
/// ["x", "y", "z", "intensity"]
/// }
/// }
/// ```
pub trait Fields<const N: usize> {
fn field_names_ordered() -> [&'static str; N];
}
#[cfg(feature = "derive")]
enum PointField { enum PointField {
Padding(u32), Padding(u32),
Field { size: u32, datatype: u8, count: u32 }, Field { size: u32, datatype: u8, count: u32 },
} }
#[cfg(feature = "derive")] #[derive(Debug, Clone)]
struct TypeLayoutInfo { struct KnownLayoutInfo {
fields: Vec<PointField>, fields: Vec<PointField>,
} }
#[cfg(feature = "derive")] impl TryFrom<LayoutField> for PointField {
impl TryFrom<type_layout::Field> for PointField {
type Error = MsgConversionError; type Error = MsgConversionError;
fn try_from(f: type_layout::Field) -> Result<Self, Self::Error> { fn try_from(f: LayoutField) -> Result<Self, Self::Error> {
match f { match f {
type_layout::Field::Field { name: _, ty, size } => { LayoutField::Field { name: _, ty, size } => {
let typename: String = ty.into_owned().to_lowercase(); let typename: String = ty.into_owned().to_lowercase();
let datatype = FieldDatatype::from_str(typename.as_str())?; let datatype = FieldDatatype::from_str(typename.as_str())?;
Ok(Self::Field { Ok(Self::Field {
@ -961,21 +953,19 @@ impl TryFrom<type_layout::Field> for PointField {
count: 1, count: 1,
}) })
} }
type_layout::Field::Padding { size } => Ok(Self::Padding(size.try_into()?)), LayoutField::Padding { size } => Ok(Self::Padding(size.try_into()?)),
} }
} }
} }
#[cfg(feature = "derive")] impl TryFrom<LayoutDescription> for KnownLayoutInfo {
impl TryFrom<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
type Error = MsgConversionError; type Error = MsgConversionError;
fn try_from(t: type_layout::TypeLayoutInfo) -> Result<Self, Self::Error> { fn try_from(t: LayoutDescription) -> Result<Self, Self::Error> {
let fields: Vec<PointField> = t let fields: Vec<PointField> =
.fields t.0.into_iter()
.into_iter() .map(PointField::try_from)
.map(PointField::try_from) .collect::<Result<Vec<_>, _>>()?;
.collect::<Result<Vec<_>, _>>()?;
Ok(Self { fields }) Ok(Self { fields })
} }
} }
@ -1029,11 +1019,10 @@ impl PointData {
#[inline] #[inline]
fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self { fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self {
debug_assert!(data.len() >= offset + datatype.size()); debug_assert!(data.len() >= offset + datatype.size());
let bytes = [u8::default(); core::mem::size_of::<f64>()]; let mut bytes = [u8::default(); core::mem::size_of::<f64>()];
unsafe { unsafe {
let data_ptr = data.as_ptr().add(offset); let data_ptr = data.as_ptr().add(offset);
let bytes_ptr = bytes.as_ptr() as *mut u8; core::ptr::copy_nonoverlapping(data_ptr, bytes.as_mut_ptr(), datatype.size());
core::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size());
} }
Self { Self {
@ -1470,29 +1459,4 @@ impl FromBytes for u8 {
fn from_le_bytes(bytes: PointDataBuffer) -> Self { fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0]]) Self::from_le_bytes([bytes[0]])
} }
} }
#[cfg(test)]
#[cfg(feature = "derive")]
mod tests {
use super::Fields;
use rpcl2_derive::Fields;
use alloc::string::String;
#[allow(dead_code)]
#[derive(Fields)]
struct TestStruct {
field1: String,
#[rpcl2(rename("renamed_field"))]
field2: i32,
field3: f64,
field4: bool,
}
#[test]
fn test_struct_names() {
let names = TestStruct::field_names_ordered();
assert_eq!(names, ["field1", "renamed_field", "field3", "field4"]);
}
}

207
src/points.rs
View File

@ -1,18 +1,12 @@
//! Predefined point types commonly used in ROS. //! Predefined point types commonly used in ROS.
use crate::{Fields, PointConvertible, RPCL2Point}; use crate::{LayoutDescription, LayoutField, PointConvertible, RPCL2Point};
#[cfg(feature = "derive")]
use type_layout::TypeLayout;
#[cfg(feature = "derive")]
use alloc::vec::Vec;
/// A packed RGB color encoding as used in ROS tools. /// A packed RGB color encoding as used in ROS tools.
#[derive(Clone, Copy)] #[derive(Clone, Copy)]
#[repr(C)] #[repr(C, align(4))]
pub union RGB { pub union RGB {
packed: f32, packed: f32,
unpacked: [u8; 4], // 1 byte padding unpacked: [u8; 4], // Padding
} }
unsafe impl Send for RGB {} unsafe impl Send for RGB {}
@ -112,8 +106,7 @@ impl From<f32> for RGB {
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates. /// This is a 3D point with x, y, z coordinates.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZ { pub struct PointXYZ {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -155,12 +148,6 @@ impl PointXYZ {
unsafe impl Send for PointXYZ {} unsafe impl Send for PointXYZ {}
unsafe impl Sync for PointXYZ {} unsafe impl Sync for PointXYZ {}
impl Fields<3> for PointXYZ {
fn field_names_ordered() -> [&'static str; 3] {
["x", "y", "z"]
}
}
impl From<RPCL2Point<3>> for PointXYZ { impl From<RPCL2Point<3>> for PointXYZ {
fn from(point: RPCL2Point<3>) -> Self { fn from(point: RPCL2Point<3>) -> Self {
Self::new(point[0].get(), point[1].get(), point[2].get()) Self::new(point[0].get(), point[1].get(), point[2].get())
@ -173,13 +160,21 @@ impl From<PointXYZ> for RPCL2Point<3> {
} }
} }
impl PointConvertible<3> for PointXYZ {} unsafe impl PointConvertible<3> for PointXYZ {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::padding(4),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an intensity value. /// This is a 3D point with x, y, z coordinates and an intensity value.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZI { pub struct PointXYZI {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -203,12 +198,6 @@ impl PointXYZI {
unsafe impl Send for PointXYZI {} unsafe impl Send for PointXYZI {}
unsafe impl Sync for PointXYZI {} unsafe impl Sync for PointXYZI {}
impl Fields<4> for PointXYZI {
fn field_names_ordered() -> [&'static str; 4] {
["x", "y", "z", "intensity"]
}
}
impl From<RPCL2Point<4>> for PointXYZI { impl From<RPCL2Point<4>> for PointXYZI {
fn from(point: RPCL2Point<4>) -> Self { fn from(point: RPCL2Point<4>) -> Self {
Self::new( Self::new(
@ -232,13 +221,21 @@ impl From<PointXYZI> for RPCL2Point<4> {
} }
} }
impl PointConvertible<4> for PointXYZI {} unsafe impl PointConvertible<4> for PointXYZI {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("intensity", "f32", 4),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a label. /// This is a 3D point with x, y, z coordinates and a label.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZL { pub struct PointXYZL {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -262,12 +259,6 @@ impl PointXYZL {
unsafe impl Send for PointXYZL {} unsafe impl Send for PointXYZL {}
unsafe impl Sync for PointXYZL {} unsafe impl Sync for PointXYZL {}
impl Fields<4> for PointXYZL {
fn field_names_ordered() -> [&'static str; 4] {
["x", "y", "z", "label"]
}
}
impl From<RPCL2Point<4>> for PointXYZL { impl From<RPCL2Point<4>> for PointXYZL {
fn from(point: RPCL2Point<4>) -> Self { fn from(point: RPCL2Point<4>) -> Self {
Self::new( Self::new(
@ -291,13 +282,21 @@ impl From<PointXYZL> for RPCL2Point<4> {
} }
} }
impl PointConvertible<4> for PointXYZL {} unsafe impl PointConvertible<4> for PointXYZL {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("label", "u32", 4),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an RGB color value. /// This is a 3D point with x, y, z coordinates and an RGB color value.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZRGB { pub struct PointXYZRGB {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -338,12 +337,6 @@ impl PointXYZRGB {
unsafe impl Send for PointXYZRGB {} unsafe impl Send for PointXYZRGB {}
unsafe impl Sync for PointXYZRGB {} unsafe impl Sync for PointXYZRGB {}
impl Fields<4> for PointXYZRGB {
fn field_names_ordered() -> [&'static str; 4] {
["x", "y", "z", "rgb"]
}
}
impl From<RPCL2Point<4>> for PointXYZRGB { impl From<RPCL2Point<4>> for PointXYZRGB {
fn from(point: RPCL2Point<4>) -> Self { fn from(point: RPCL2Point<4>) -> Self {
Self { Self {
@ -367,14 +360,22 @@ impl From<PointXYZRGB> for RPCL2Point<4> {
} }
} }
impl PointConvertible<4> for PointXYZRGB {} unsafe impl PointConvertible<4> for PointXYZRGB {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("rgb", "RGB", 4),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and an RGBA color value. /// 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. /// The alpha channel is commonly used as padding but this crate uses every channel and no padding.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZRGBA { pub struct PointXYZRGBA {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -416,12 +417,6 @@ impl PointXYZRGBA {
unsafe impl Send for PointXYZRGBA {} unsafe impl Send for PointXYZRGBA {}
unsafe impl Sync for PointXYZRGBA {} unsafe impl Sync for PointXYZRGBA {}
impl Fields<5> for PointXYZRGBA {
fn field_names_ordered() -> [&'static str; 5] {
["x", "y", "z", "rgb", "a"]
}
}
impl From<RPCL2Point<5>> for PointXYZRGBA { impl From<RPCL2Point<5>> for PointXYZRGBA {
fn from(point: RPCL2Point<5>) -> Self { fn from(point: RPCL2Point<5>) -> Self {
Self { Self {
@ -447,13 +442,23 @@ impl From<PointXYZRGBA> for RPCL2Point<5> {
} }
} }
impl PointConvertible<5> for PointXYZRGBA {} unsafe impl PointConvertible<5> for PointXYZRGBA {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("rgb", "RGB", 4),
LayoutField::new("a", "u8", 1),
LayoutField::padding(15),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// 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. /// This is a 3D point with x, y, z coordinates, an RGB color value and a normal vector.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZRGBNormal { pub struct PointXYZRGBNormal {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -512,12 +517,6 @@ impl PointXYZRGBNormal {
unsafe impl Send for PointXYZRGBNormal {} unsafe impl Send for PointXYZRGBNormal {}
unsafe impl Sync for PointXYZRGBNormal {} unsafe impl Sync for PointXYZRGBNormal {}
impl Fields<7> for PointXYZRGBNormal {
fn field_names_ordered() -> [&'static str; 7] {
["x", "y", "z", "rgb", "normal_x", "normal_y", "normal_z"]
}
}
impl From<RPCL2Point<7>> for PointXYZRGBNormal { impl From<RPCL2Point<7>> for PointXYZRGBNormal {
fn from(point: RPCL2Point<7>) -> Self { fn from(point: RPCL2Point<7>) -> Self {
Self { Self {
@ -547,13 +546,25 @@ impl From<PointXYZRGBNormal> for RPCL2Point<7> {
} }
} }
impl PointConvertible<7> for PointXYZRGBNormal {} unsafe impl PointConvertible<7> for PointXYZRGBNormal {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("rgb", "RGB", 4),
LayoutField::new("normal_x", "f32", 4),
LayoutField::new("normal_y", "f32", 4),
LayoutField::new("normal_z", "f32", 4),
LayoutField::padding(4),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// 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. /// This is a 3D point with x, y, z coordinates, an intensity value and a normal vector.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZINormal { pub struct PointXYZINormal {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -597,12 +608,6 @@ impl PointXYZINormal {
unsafe impl Send for PointXYZINormal {} unsafe impl Send for PointXYZINormal {}
unsafe impl Sync for PointXYZINormal {} unsafe impl Sync for PointXYZINormal {}
impl Fields<7> for PointXYZINormal {
fn field_names_ordered() -> [&'static str; 7] {
["x", "y", "z", "i", "normal_x", "normal_y", "normal_z"]
}
}
impl From<RPCL2Point<7>> for PointXYZINormal { impl From<RPCL2Point<7>> for PointXYZINormal {
fn from(point: RPCL2Point<7>) -> Self { fn from(point: RPCL2Point<7>) -> Self {
Self::new( Self::new(
@ -632,13 +637,25 @@ impl From<PointXYZINormal> for RPCL2Point<7> {
} }
} }
impl PointConvertible<7> for PointXYZINormal {} unsafe impl PointConvertible<7> for PointXYZINormal {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("intensity", "f32", 4),
LayoutField::new("normal_x", "f32", 4),
LayoutField::new("normal_y", "f32", 4),
LayoutField::new("normal_z", "f32", 4),
LayoutField::padding(4),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a label. /// This is a 3D point with x, y, z coordinates and a label.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZRGBL { pub struct PointXYZRGBL {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -686,12 +703,6 @@ impl PointXYZRGBL {
} }
} }
impl Fields<5> for PointXYZRGBL {
fn field_names_ordered() -> [&'static str; 5] {
["x", "y", "z", "rgb", "label"]
}
}
impl From<RPCL2Point<5>> for PointXYZRGBL { impl From<RPCL2Point<5>> for PointXYZRGBL {
fn from(point: RPCL2Point<5>) -> Self { fn from(point: RPCL2Point<5>) -> Self {
Self { Self {
@ -717,13 +728,23 @@ impl From<PointXYZRGBL> for RPCL2Point<5> {
} }
} }
impl PointConvertible<5> for PointXYZRGBL {} unsafe impl PointConvertible<5> for PointXYZRGBL {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("rgb", "RGB", 4),
LayoutField::new("label", "u32", 4),
LayoutField::padding(12),
])
}
}
/// Predefined point type commonly used in ROS with PCL. /// Predefined point type commonly used in ROS with PCL.
/// This is a 3D point with x, y, z coordinates and a normal vector. /// This is a 3D point with x, y, z coordinates and a normal vector.
#[derive(Clone, Debug, PartialEq, Copy, Default)] #[derive(Clone, Debug, PartialEq, Copy, Default)]
#[cfg_attr(feature = "derive", derive(TypeLayout))] #[repr(C, align(16))]
#[repr(C)]
pub struct PointXYZNormal { pub struct PointXYZNormal {
pub x: f32, pub x: f32,
pub y: f32, pub y: f32,
@ -757,12 +778,6 @@ impl PointXYZNormal {
unsafe impl Send for PointXYZNormal {} unsafe impl Send for PointXYZNormal {}
unsafe impl Sync for PointXYZNormal {} unsafe impl Sync for PointXYZNormal {}
impl Fields<6> for PointXYZNormal {
fn field_names_ordered() -> [&'static str; 6] {
["x", "y", "z", "normal_x", "normal_y", "normal_z"]
}
}
impl From<RPCL2Point<6>> for PointXYZNormal { impl From<RPCL2Point<6>> for PointXYZNormal {
fn from(point: RPCL2Point<6>) -> Self { fn from(point: RPCL2Point<6>) -> Self {
Self::new( Self::new(
@ -790,4 +805,16 @@ impl From<PointXYZNormal> for RPCL2Point<6> {
} }
} }
impl PointConvertible<6> for PointXYZNormal {} unsafe impl PointConvertible<6> for PointXYZNormal {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("normal_x", "f32", 4),
LayoutField::new("normal_y", "f32", 4),
LayoutField::new("normal_z", "f32", 4),
LayoutField::padding(8),
])
}
}

7
src/prelude.rs
View File

@ -1,7 +1,7 @@
//! Commonly used types and traits for predefined and custom point conversions. //! Commonly used types and traits for predefined and custom point conversions.
pub use crate::{ pub use crate::{
FieldDatatype, Fields, FromBytes, GetFieldDatatype, MsgConversionError, PointCloud2Msg, FieldDatatype, FromBytes, GetFieldDatatype, LayoutDescription, LayoutField, MsgConversionError,
PointConvertible, PointDataBuffer, RPCL2Point, PointCloud2Msg, PointConvertible, PointDataBuffer, RPCL2Point,
}; };
pub use crate::points::*; pub use crate::points::*;
@ -10,8 +10,5 @@ pub use crate::ros::*;
#[cfg(feature = "rayon")] #[cfg(feature = "rayon")]
pub use rayon::prelude::*; pub use rayon::prelude::*;
#[cfg(feature = "derive")]
pub use type_layout::TypeLayout;
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
pub use rpcl2_derive::*; pub use rpcl2_derive::*;

35
tests/Dockerfile_rclrs_jazzy Normal file
View File

@ -0,0 +1,35 @@
# syntax=docker/dockerfile:1
FROM ros:jazzy
# Update default packages
RUN apt-get update
# Get Ubuntu packages
RUN apt-get install -y \
build-essential \
curl \
libclang-dev \
git \
python3 \
python3-pip \
python3-vcstool
# Get Rust
RUN curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | bash -s -- -y
RUN echo 'source $HOME/.cargo/env' >> $HOME/.bashrc
RUN . $HOME/.cargo/env && cargo install --debug cargo-ament-build
RUN pip install --break-system-packages git+https://github.com/colcon/colcon-cargo.git
RUN pip install --break-system-packages git+https://github.com/colcon/colcon-ros-cargo.git
WORKDIR /ros2_rust_build
RUN git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust
RUN vcs import src < src/ros2_rust/ros2_rust_jazzy.repos
WORKDIR /ros2_rust_build/src/ros_pointcloud2_tests
COPY . .
WORKDIR /ros2_rust_build
RUN . $HOME/.cargo/env && . /opt/ros/jazzy/setup.sh && colcon build
RUN chmod +x /ros2_rust_build/src/ros_pointcloud2_tests/rpcl2/tests/rclrs_test.bash
ENTRYPOINT [ "/ros2_rust_build/src/ros_pointcloud2_tests/rpcl2/tests/rclrs_test.bash" ]

197
tests/e2e_test.rs
View File

@ -1,3 +1,4 @@
use pretty_assertions::assert_eq;
use ros_pointcloud2::prelude::*; use ros_pointcloud2::prelude::*;
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
@ -9,7 +10,6 @@ macro_rules! convert_from_into {
}; };
} }
#[cfg(feature = "derive")]
macro_rules! convert_from_into_vec { macro_rules! convert_from_into_vec {
($point:ty, $cloud:expr) => { ($point:ty, $cloud:expr) => {
convert_from_into_in_out_cloud_vec!($cloud, $point, $cloud, $point); convert_from_into_in_out_cloud_vec!($cloud, $point, $cloud, $point);
@ -30,7 +30,6 @@ macro_rules! convert_from_into_in_out_cloud {
}; };
} }
#[cfg(feature = "derive")]
macro_rules! convert_from_into_in_out_cloud_vec { macro_rules! convert_from_into_in_out_cloud_vec {
($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => { ($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
let msg = PointCloud2Msg::try_from_vec($in_cloud.clone()); let msg = PointCloud2Msg::try_from_vec($in_cloud.clone());
@ -57,9 +56,21 @@ fn write_cloud() {
let msg = PointCloud2Msg::try_from_iter(cloud); let msg = PointCloud2Msg::try_from_iter(cloud);
assert!(msg.is_ok()); assert!(msg.is_ok());
} }
/*
#[test]
fn collect_vec() {
let cloud = vec![
PointXYZ::new(0.0, 1.0, 5.0),
PointXYZ::new(1.0, 1.5, 5.0),
PointXYZ::new(1.3, 1.6, 5.7),
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
]
.into_iter();
let msg: Result<PointCloud2Msg, MsgConversionError> = cloud.collect();
}*/
#[test] #[test]
#[cfg(feature = "derive")]
fn write_cloud_from_vec() { fn write_cloud_from_vec() {
let cloud = vec![ let cloud = vec![
PointXYZ::new(0.0, 1.0, 5.0), PointXYZ::new(0.0, 1.0, 5.0),
@ -73,7 +84,6 @@ fn write_cloud_from_vec() {
} }
#[test] #[test]
#[cfg(feature = "derive")]
fn write_empty_cloud_vec() { fn write_empty_cloud_vec() {
let cloud: Vec<PointXYZ> = vec![]; let cloud: Vec<PointXYZ> = vec![];
let msg = PointCloud2Msg::try_from_vec(cloud); let msg = PointCloud2Msg::try_from_vec(cloud);
@ -90,7 +100,7 @@ fn write_empty_cloud_iter() {
} }
#[test] #[test]
#[cfg(all(feature = "derive", feature = "rayon"))] #[cfg(feature = "rayon")]
fn conv_cloud_par_iter() { fn conv_cloud_par_iter() {
let cloud = vec![ let cloud = vec![
PointXYZ::new(0.0, 1.0, 5.0), PointXYZ::new(0.0, 1.0, 5.0),
@ -110,7 +120,7 @@ fn conv_cloud_par_iter() {
} }
#[test] #[test]
#[cfg(all(feature = "derive", feature = "rayon"))] #[cfg(feature = "rayon")]
fn conv_cloud_par_par_iter() { fn conv_cloud_par_par_iter() {
let cloud = vec![ let cloud = vec![
PointXYZ::new(0.0, 1.0, 5.0), PointXYZ::new(0.0, 1.0, 5.0),
@ -133,14 +143,40 @@ fn conv_cloud_par_par_iter() {
#[test] #[test]
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
fn custom_xyz_f32() { fn custom_xyz_f32() {
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)] #[derive(Debug, PartialEq, Clone, Default)]
#[repr(C)] #[repr(C, align(4))]
struct CustomPoint { struct CustomPoint {
x: f32, x: f32,
y: f32, y: f32,
z: f32, z: f32,
} }
impl From<RPCL2Point<3>> for CustomPoint {
fn from(point: RPCL2Point<3>) -> Self {
Self {
x: point[0].get(),
y: point[1].get(),
z: point[2].get(),
}
}
}
impl From<CustomPoint> for RPCL2Point<3> {
fn from(point: CustomPoint) -> Self {
[point.x.into(), point.y.into(), point.z.into()].into()
}
}
unsafe impl PointConvertible<3> for CustomPoint {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
])
}
}
convert_from_into!( convert_from_into!(
CustomPoint, CustomPoint,
vec![ vec![
@ -164,7 +200,6 @@ fn custom_xyz_f32() {
} }
#[test] #[test]
#[cfg(feature = "derive")]
fn custom_xyzi_f32() { fn custom_xyzi_f32() {
let cloud: Vec<CustomPointXYZI> = vec![ let cloud: Vec<CustomPointXYZI> = vec![
CustomPointXYZI { CustomPointXYZI {
@ -193,8 +228,8 @@ fn custom_xyzi_f32() {
}, },
]; ];
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)] #[derive(Debug, PartialEq, Clone, Default)]
#[repr(C)] #[repr(C, align(4))]
struct CustomPointXYZI { struct CustomPointXYZI {
x: f32, x: f32,
y: f32, y: f32,
@ -202,14 +237,49 @@ fn custom_xyzi_f32() {
i: u8, i: u8,
} }
impl From<RPCL2Point<4>> for CustomPointXYZI {
fn from(point: RPCL2Point<4>) -> Self {
Self {
x: point[0].get(),
y: point[1].get(),
z: point[2].get(),
i: point[3].get(),
}
}
}
impl From<CustomPointXYZI> for RPCL2Point<4> {
fn from(point: CustomPointXYZI) -> Self {
[
point.x.into(),
point.y.into(),
point.z.into(),
point.i.into(),
]
.into()
}
}
unsafe impl PointConvertible<4> for CustomPointXYZI {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("i", "u8", 1),
LayoutField::padding(3),
])
}
}
convert_from_into!(CustomPointXYZI, cloud); convert_from_into!(CustomPointXYZI, cloud);
} }
#[test] #[test]
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
fn custom_rgba_f32() { fn custom_rgba_f32() {
#[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)] #[derive(Debug, PartialEq, Clone, Default)]
#[repr(C)] #[repr(C, align(4))]
struct CustomPoint { struct CustomPoint {
x: f32, x: f32,
y: f32, y: f32,
@ -220,6 +290,53 @@ fn custom_rgba_f32() {
a: u8, a: u8,
} }
impl From<RPCL2Point<7>> for CustomPoint {
fn from(point: RPCL2Point<7>) -> Self {
Self {
x: point[0].get(),
y: point[1].get(),
z: point[2].get(),
r: point[3].get(),
g: point[4].get(),
b: point[5].get(),
a: point[6].get(),
}
}
}
impl From<CustomPoint> for RPCL2Point<7> {
fn from(point: CustomPoint) -> Self {
[
point.x.into(),
point.y.into(),
point.z.into(),
point.r.into(),
point.g.into(),
point.b.into(),
point.a.into(),
]
.into()
}
}
unsafe impl PointConvertible<7> for CustomPoint {
fn layout() -> LayoutDescription {
LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
LayoutField::new("r", "u8", 1),
LayoutField::padding(3),
LayoutField::new("g", "u8", 1),
LayoutField::padding(3),
LayoutField::new("b", "u8", 1),
LayoutField::padding(3),
LayoutField::new("a", "u8", 1),
LayoutField::padding(3),
])
}
}
let cloud = vec![ let cloud = vec![
CustomPoint { CustomPoint {
x: 0.0, x: 0.0,
@ -375,7 +492,6 @@ fn converterxyzrgb() {
} }
#[test] #[test]
#[cfg(feature = "derive")]
fn converterxyzrgb_from_vec() { fn converterxyzrgb_from_vec() {
convert_from_into_vec!( convert_from_into_vec!(
PointXYZRGB, PointXYZRGB,
@ -433,7 +549,6 @@ fn write_xyzi_read_xyz() {
} }
#[test] #[test]
#[cfg(feature = "derive")]
fn write_xyzi_read_xyz_vec() { fn write_xyzi_read_xyz_vec() {
let write_cloud = vec![ let write_cloud = vec![
PointXYZI::new(0.0, 1.0, 5.0, 0.0), PointXYZI::new(0.0, 1.0, 5.0, 0.0),
@ -453,10 +568,9 @@ fn write_xyzi_read_xyz_vec() {
} }
#[test] #[test]
#[cfg(feature = "derive")]
fn write_less_than_available() { fn write_less_than_available() {
#[derive(Debug, PartialEq, Clone, Default, TypeLayout)] #[derive(Debug, PartialEq, Clone, Default)]
#[repr(C)] #[repr(C, align(4))]
struct CustomPoint { struct CustomPoint {
x: f32, x: f32,
y: f32, y: f32,
@ -481,14 +595,16 @@ fn write_less_than_available() {
} }
} }
impl Fields<3> for CustomPoint { unsafe impl PointConvertible<3> for CustomPoint {
fn field_names_ordered() -> [&'static str; 3] { fn layout() -> LayoutDescription {
["x", "y", "z"] LayoutDescription::new(&[
LayoutField::new("x", "f32", 4),
LayoutField::new("y", "f32", 4),
LayoutField::new("z", "f32", 4),
])
} }
} }
impl PointConvertible<3> for CustomPoint {}
let write_cloud = vec![ let write_cloud = vec![
CustomPoint { CustomPoint {
x: 1.0, x: 1.0,
@ -532,37 +648,4 @@ fn write_less_than_available() {
]; ];
convert_from_into_in_out_cloud!(write_cloud, CustomPoint, read_cloud, CustomPoint); convert_from_into_in_out_cloud!(write_cloud, CustomPoint, read_cloud, CustomPoint);
} }
#[test]
#[cfg(feature = "derive")]
#[allow(unused_variables)]
fn readme() {
use ros_pointcloud2::prelude::*;
// PointXYZ (and many others) are provided by the crate.
let cloud_points = vec![
PointXYZI::new(91.486, -4.1, 42.0001, 0.1),
PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MIN),
];
let out_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
// Convert the ROS crate message type, we will use r2r here.
// let msg: r2r::sensor_msgs::msg::PointCloud2 = out_msg.into();
// Publish ...
// ... now incoming from a topic.
// let in_msg: PointCloud2Msg = msg.into();
let in_msg = out_msg;
let processed_cloud = in_msg
.try_into_iter()
.unwrap()
.map(|point: PointXYZ| {
// Define the info you want to have from the Msg.
// Some logic here ...
point
})
.collect::<Vec<_>>();
}