diff --git a/.github/workflows/r2r_galactic.yml b/.github/workflows/r2r_galactic.yml
index 5c5f3d1..45a609f 100644
--- a/.github/workflows/r2r_galactic.yml
+++ b/.github/workflows/r2r_galactic.yml
@@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_r2r_galactic --tag r2r_galactic
- - run: docker run r2r_galactic cargo test --features r2r_msg
+ - run: docker run r2r_galactic cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/r2r_humble.yml b/.github/workflows/r2r_humble.yml
index 5373957..a88752c 100644
--- a/.github/workflows/r2r_humble.yml
+++ b/.github/workflows/r2r_humble.yml
@@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_r2r_humble --tag r2r_humble
- - run: docker run r2r_humble cargo test --features r2r_msg
+ - run: docker run r2r_humble cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/r2r_iron.yml b/.github/workflows/r2r_iron.yml
index 5de642d..ef3ab9d 100644
--- a/.github/workflows/r2r_iron.yml
+++ b/.github/workflows/r2r_iron.yml
@@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_r2r_iron --tag r2r_iron
- - run: docker run r2r_iron cargo test --features r2r_msg
+ - run: docker run r2r_iron cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/rclrs_humble.yml b/.github/workflows/rclrs_humble.yml
index 7ac4349..1c0f88b 100644
--- a/.github/workflows/rclrs_humble.yml
+++ b/.github/workflows/rclrs_humble.yml
@@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble
- - run: docker run rclrs_humble cargo test
+ - run: docker run rclrs_humble cargo test --features derive,nalgebra,rayon
diff --git a/.github/workflows/rclrs_iron.yml b/.github/workflows/rclrs_iron.yml
index d9ac7cf..390419b 100644
--- a/.github/workflows/rclrs_iron.yml
+++ b/.github/workflows/rclrs_iron.yml
@@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_rclrs_iron --tag rclrs_iron
- - run: docker run rclrs_iron cargo test
+ - run: docker run rclrs_iron cargo test --features derive,nalgebra,rayon
diff --git a/.github/workflows/rosrust_noetic.yml b/.github/workflows/rosrust_noetic.yml
index 4662df4..40a5c12 100644
--- a/.github/workflows/rosrust_noetic.yml
+++ b/.github/workflows/rosrust_noetic.yml
@@ -105,4 +105,4 @@ jobs:
- name: test
run: |
source /opt/ros/$ROS_DISTRO/setup.bash
- cargo test --features rosrust_msg
+ cargo test --features rosrust_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index 7f940b7..04c5cee 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -25,4 +25,4 @@ jobs:
- name: Linting
run: cargo clippy --all-targets -- -D warnings
- name: Tests
- run: cargo test
+ run: cargo test --features derive,nalgebra,rayon
diff --git a/Cargo.toml b/Cargo.toml
index a70ded8..c347ffd 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -7,17 +7,48 @@ description = "Customizable conversions for working with sensor_msgs/PointCloud2
repository = "https://github.com/stelzo/ros_pointcloud2"
license = "MIT"
keywords = ["ros", "pointcloud2", "pointcloud", "message"]
-categories = ["science::robotics", "encoding", "data-structures", "api-bindings"]
+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/**"]
+exclude = [
+ "**/tests/**",
+ "**/examples/**",
+ "**/benches/**",
+ "**/target/**",
+ "**/build/**",
+ "**/dist/**",
+ "**/docs/**",
+ "**/doc/**",
+]
[dependencies]
rosrust_msg = { version = "0.1", optional = true }
rosrust = { version = "0.9.11", optional = true }
r2r = { version = "0.8.4", optional = true }
+rayon = { version = "1", optional = true }
+nalgebra = { version = "0", optional = true }
+rpcl2_derive = { path = "./rpcl2_derive", optional = true }
+type-layout = { version = "0.2", optional = true }
+
+[dev-dependencies]
+rand = "0.8"
+criterion = { version = "0.4", features = ["html_reports"] }
+
+[[bench]]
+name = "roundtrip"
+harness = false
[features]
rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
-r2r_msg = ["dep:r2r"]
\ No newline at end of file
+r2r_msg = ["dep:r2r"]
+rayon = ["dep:rayon"]
+derive = ["dep:rpcl2_derive", "dep:type-layout"]
+nalgebra = ["dep:nalgebra"]
+
+default = ["rayon", "derive", "nalgebra"]
\ No newline at end of file
diff --git a/README.md b/README.md
index 4bcd698..7143c6c 100644
--- a/README.md
+++ b/README.md
@@ -1,59 +1,53 @@
+## !! Note !!
+This library is currently in development for v1.0.0, for the documentation of v0.4.0 on crates.io, visit the [docs](https://docs.rs/ros_pointcloud2/0.4.0/ros_pointcloud2/).
+
ROS PointCloud2
- Customizable conversions to and from the PointCloud2 ROS message.
+ A complete and versatile abstraction of PointCloud2.
-Providing an easy to use, generics defined, point-wise iterator abstraction over the byte buffer in `PointCloud2` to minimize iterations in your processing pipeline.
+To keep the crate a general purpose library for the problem, it uses its own type for the message `PointCloud2Msg`.
+ROS1 and ROS2 is supported with feature flags.
-To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `PointCloud2Msg`.
+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/1.0.0/ros_pointcloud2/) for a complete guide.
+
+## Quickstart
+
+The following example uses the iterator APIs. For memory bound pipelines, you may also try the `try_from_vec` or `try_into_vec` functions by enabling the `derive` feature.
-## Examples
```rust
-use ros_pointcloud2::{
- pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-};
+use ros_pointcloud2::prelude::*;
-// Your points (here using the predefined type PointXYZ).
+// PointXYZ (and many others) are provided by the crate.
let cloud_points = vec![
- PointXYZ {x: 91.486, y: -4.1, z: 42.0001,},
- PointXYZ {x: f32::MAX, y: f32::MIN, z: f32::MAX,},
+ PointXYZ::new(91.486, -4.1, 42.0001),
+ PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
];
-// For equality test later
-let cloud_copy = cloud_points.clone();
-
-// Vector -> Writer -> Message.
-// You can also just give the Vec or anything that implements `IntoIterator`.
-let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
- .try_into() // iterating points here O(n)
- .unwrap();
-
-// Convert to your ROS crate message type, we will use r2r here.
-// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
+// Give the Vec or anything that implements `IntoIterator`.
+let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+// Convert the ROS crate message type, we will use r2r here.
+// let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
// Publish ...
-
// ... now incoming from a topic.
-// let internal_msg: PointCloud2Msg = msg.into();
+// let in_msg: PointCloud2Msg = msg.into();
-// Message -> Reader -> your pipeline. The Reader implements the Iterator trait.
-let reader = ReaderXYZ::try_from(internal_msg).unwrap();
-let new_cloud_points = reader
- .map(|point: PointXYZ| {
- // Some logic here
+let new_pcl = in_msg.try_into_iter().unwrap()
+ .map(|point: PointXYZ| { // Define the type of point here.
+ // Some logic here ...
- point
- })
- .collect::>(); // iterating points here O(n)
-
-assert_eq!(new_cloud_points, cloud_copy);
+ point
+ })
+ .collect::>();
```
## Integrations
There are currently 3 integrations for common ROS crates.
+
- [rosrust_msg](https://github.com/adnanademovic/rosrust)
- [](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml)
- [r2r_msg](https://github.com/sequenceplanner/r2r)
@@ -65,6 +59,7 @@ There are currently 3 integrations for common ROS crates.
- [](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_iron.yml)
You can use `rosrust` and `r2r` by enabling the respective feature:
+
```toml
[dependencies]
ros_pointcloud2 = { version = "*", features = ["r2r_msg"]}
@@ -73,12 +68,16 @@ ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]}
```
### rclrs (ros2_rust)
+
Features do not work properly with `rcrls` because the messages are linked externally. You need to use tags instead:
+
```toml
[dependencies]
ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.4.0_rclrs" }
```
+
Also, indicate the following dependencies to your linker inside the `package.xml` of your package.
+
```xml
std_msgs
sensor_msgs
@@ -87,11 +86,6 @@ Also, indicate the following dependencies to your linker inside the `package.xml
Please open an issue or PR if you want to see support for other crates.
-## Future Work
-- Benchmark vs PCL
-- Add more predefined types
-- Optional derive macros for custom point implementations
-
-
## License
+
[MIT](https://choosealicense.com/licenses/mit/)
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
new file mode 100644
index 0000000..cf4945b
--- /dev/null
+++ b/benches/roundtrip.rs
@@ -0,0 +1,164 @@
+use criterion::{black_box, criterion_group, criterion_main, Criterion};
+use ros_pointcloud2::prelude::*;
+
+use rand::Rng;
+
+pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec {
+ let mut rng = rand::thread_rng();
+ let mut pointcloud = Vec::with_capacity(num_points);
+ for _ in 0..num_points {
+ let point = PointXYZ {
+ x: rng.gen_range(min..max),
+ y: rng.gen_range(min..max),
+ z: rng.gen_range(min..max),
+ };
+ pointcloud.push(point);
+ }
+ pointcloud
+}
+
+#[cfg(feature = "derive")]
+fn roundtrip_vec(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
+ let total = internal_msg
+ .try_into_iter()
+ .unwrap()
+ .collect::>();
+ orig_len == total.len()
+}
+
+fn roundtrip(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_iter()
+ .unwrap()
+ .collect::>();
+ orig_len == total.len()
+}
+
+#[cfg(feature = "derive")]
+fn roundtrip_filter_vec(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
+ let total = internal_msg
+ .try_into_iter()
+ .unwrap()
+ .filter(|point: &PointXYZ| {
+ (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+ })
+ .fold(PointXYZ::default(), |acc, point| PointXYZ {
+ x: acc.x + point.x,
+ y: acc.y + point.y,
+ z: acc.z + point.z,
+ });
+ orig_len == total.x as usize
+}
+
+fn roundtrip_filter(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_iter()
+ .unwrap()
+ .filter(|point: &PointXYZ| {
+ (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+ })
+ .fold(PointXYZ::default(), |acc, point| PointXYZ {
+ x: acc.x + point.x,
+ y: acc.y + point.y,
+ z: acc.z + point.z,
+ });
+ orig_len == total.x as usize
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_par(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_par_iter()
+ .unwrap()
+ .collect::>();
+ orig_len != total.len()
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_filter_par(cloud: Vec) -> bool {
+ let orig_len: usize = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_par_iter()
+ .unwrap()
+ .filter(|point: &PointXYZ| {
+ (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+ })
+ .reduce(PointXYZ::default, |acc, point| PointXYZ {
+ x: acc.x + point.x,
+ y: acc.y + point.y,
+ z: acc.z + point.z,
+ });
+ orig_len == total.x as usize
+}
+
+fn roundtrip_benchmark(c: &mut Criterion) {
+ let cloud_points_500k = generate_random_pointcloud(500_000, f32::MIN / 2.0, f32::MAX / 2.0);
+ let cloud_points_1_5m = generate_random_pointcloud(1_500_000, f32::MIN / 2.0, f32::MAX / 2.0);
+
+ c.bench_function("roundtrip 500k", |b| {
+ b.iter(|| {
+ black_box(roundtrip(cloud_points_500k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("roundtrip_par 500k", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par(cloud_points_500k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("roundtrip_vec 500k", |b| {
+ b.iter(|| {
+ black_box(roundtrip_vec(cloud_points_500k.clone()));
+ })
+ });
+
+ c.bench_function("roundtrip_filter 500k", |b| {
+ b.iter(|| {
+ roundtrip_filter(black_box(cloud_points_500k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("roundtrip_filter_par 500k", |b| {
+ b.iter(|| {
+ roundtrip_filter_par(black_box(cloud_points_500k.clone()));
+ })
+ });
+
+ c.bench_function("roundtrip_filter 1.5m", |b| {
+ b.iter(|| {
+ roundtrip_filter(black_box(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("roundtrip_filter_par 1.5m", |b| {
+ b.iter(|| {
+ black_box(roundtrip_filter_par(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("roundtrip_filter_vec 1.5m", |b| {
+ b.iter(|| {
+ roundtrip_filter_vec(black_box(cloud_points_1_5m.clone()));
+ })
+ });
+}
+
+criterion_group!(benches, roundtrip_benchmark);
+criterion_main!(benches);
diff --git a/examples/custom_enum_field_filter.rs b/examples/custom_enum_field_filter.rs
new file mode 100644
index 0000000..264422b
--- /dev/null
+++ b/examples/custom_enum_field_filter.rs
@@ -0,0 +1,148 @@
+/// This example demonstrates how to use a custom point with encoded metadata.
+/// The use case is a segmentation point cloud where each point holds a label and we want to filter by it.
+/// Since the datatypes for the PointCloud2 message are very limited,
+/// we need to encode the enum into a supported type.
+/// This needs some manual work to tell the library how to encode and decode the enum.
+///
+/// Important Note: This example is only possible with disabled `derive` feature,
+/// because the library (currently) does not know the size of your chosen supported type at compile time.
+/// This makes direct copies impossible.
+use ros_pointcloud2::prelude::*;
+
+#[derive(Debug, PartialEq, Clone, Default, Copy)]
+enum Label {
+ #[default]
+ Human,
+ Deer,
+ Car,
+}
+
+// Define a custom point with an enum (not supported by PointCloud2)
+#[derive(Debug, PartialEq, Clone, Default)]
+struct CustomPoint {
+ x: f32,
+ y: f32,
+ z: f32,
+ intensity: f32,
+ my_custom_label: Label,
+}
+
+// For our convenience
+impl From for u8 {
+ fn from(label: Label) -> Self {
+ match label {
+ Label::Human => 0,
+ Label::Deer => 1,
+ Label::Car => 2,
+ }
+ }
+}
+
+impl From for Label {
+ fn from(label: u8) -> Self {
+ match label {
+ 0 => Label::Human,
+ 1 => Label::Deer,
+ 2 => Label::Car,
+ _ => panic!("Invalid label"),
+ }
+ }
+}
+
+// We implement the PointConvertible trait (needed for every custom point)
+
+// A ros_pointcloud2::Point takes as generic arguments:
+// - coordinate type
+// - dimension (xyz = 3)
+// - metadata dimension (intensity + my_custom_label = 2)
+impl From for RPCL2Point<5> {
+ fn from(point: CustomPoint) -> Self {
+ RPCL2Point {
+ fields: [
+ point.x.into(),
+ point.y.into(),
+ point.z.into(),
+ point.intensity.into(),
+ u8::from(point.my_custom_label).into(),
+ ],
+ }
+ }
+}
+
+impl From> for CustomPoint {
+ fn from(point: RPCL2Point<5>) -> Self {
+ Self {
+ x: point.fields[0].get(),
+ y: point.fields[1].get(),
+ z: point.fields[2].get(),
+ intensity: point.fields[3].get(),
+ my_custom_label: point.fields[4].get(), // decoding the label from u8
+ }
+ }
+}
+
+impl Fields<5> for CustomPoint {
+ fn field_names_ordered() -> [&'static str; 5] {
+ ["x", "y", "z", "intensity", "my_custom_label"] // how we want to name the metadata in the message
+ }
+}
+
+// We implemented everything that is needed so we declare it as a PointConvertible
+#[cfg(not(feature = "derive"))]
+impl PointConvertible<5> for CustomPoint {}
+
+// Now we tell the library how to encode and decode the label.
+// You don't need to do this if your CustomPoint has a field that is already supported by PointCloud2.
+impl GetFieldDatatype for Label {
+ fn field_datatype() -> FieldDatatype {
+ FieldDatatype::U8 // we want to encode the label as u8
+ }
+}
+
+// Again, you don't need this with only supported field types.
+// Technically, PointCloud2 supports big and little endian even though it is rarely used.
+// 'be' stands for big endian and 'le' for little endian.
+impl FromBytes for Label {
+ // u8 -> Label
+ fn from_be_bytes(bytes: &[u8]) -> Self {
+ u8::from_be_bytes([bytes[0]]).into()
+ }
+
+ fn from_le_bytes(bytes: &[u8]) -> Self {
+ u8::from_le_bytes([bytes[0]]).into()
+ }
+
+ // Label -> u8
+ fn bytes(label: &Self) -> Vec {
+ u8::bytes(&u8::from(*label))
+ }
+}
+
+fn main() {
+ #[cfg(not(feature = "derive"))]
+ {
+ let cloud = vec![
+ CustomPoint::new(1.0, 2.0, 3.0, 4.0, Label::Deer),
+ CustomPoint::new(4.0, 5.0, 6.0, 7.0, Label::Car),
+ CustomPoint::new(7.0, 8.0, 9.0, 10.0, Label::Human),
+ ];
+
+ println!("Original cloud: {:?}", cloud);
+
+ let msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+
+ println!("filtering by label == Deer");
+ let out = msg
+ .try_into_iter()
+ .unwrap()
+ .filter(|point: &CustomPoint| point.my_custom_label == Label::Deer)
+ .collect::>();
+
+ println!("Filtered cloud: {:?}", out);
+
+ assert_eq!(
+ vec![CustomPoint::new(1.0, 2.0, 3.0, 4.0, Label::Deer),],
+ out
+ );
+ }
+}
diff --git a/examples/rayon_bench.rs b/examples/rayon_bench.rs
new file mode 100644
index 0000000..ef21641
--- /dev/null
+++ b/examples/rayon_bench.rs
@@ -0,0 +1,135 @@
+use rand::Rng;
+/// This example implements a naive benchmark for the library so you can evaluate the use of rayon for parallel processing.
+/// It generates a random point cloud and measures the time it takes to iterate over it.
+/// The code works mainly as a showcase. For actual benchmarks, check the `benches` directory or run `cargo bench`.
+use std::time::Duration;
+
+use ros_pointcloud2::prelude::*;
+
+pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec {
+ let mut rng = rand::thread_rng();
+ let mut pointcloud = Vec::with_capacity(num_points);
+ for _ in 0..num_points {
+ let point = PointXYZ {
+ x: rng.gen_range(min..max),
+ y: rng.gen_range(min..max),
+ z: rng.gen_range(min..max),
+ };
+ pointcloud.push(point);
+ }
+ pointcloud
+}
+
+fn roundtrip(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_iter()
+ .unwrap()
+ .collect::>();
+ orig_len == total.len()
+}
+
+fn roundtrip_filter(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_iter()
+ .unwrap()
+ .filter(|point: &PointXYZ| {
+ (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+ })
+ .fold(PointXYZ::default(), |acc, point| PointXYZ {
+ x: acc.x + point.x,
+ y: acc.y + point.y,
+ z: acc.z + point.z,
+ });
+ orig_len == total.x as usize
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_par(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_par_iter()
+ .unwrap()
+ .collect::>();
+ orig_len != total.len()
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_filter_par(cloud: Vec) -> bool {
+ let orig_len: usize = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_par_iter()
+ .unwrap()
+ .filter(|point: &PointXYZ| {
+ (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+ })
+ .reduce(PointXYZ::default, |acc, point| PointXYZ {
+ x: acc.x + point.x,
+ y: acc.y + point.y,
+ z: acc.z + point.z,
+ });
+ orig_len == total.x as usize
+}
+
+// call measure_func X times and print the average time
+fn measure_func_avg(
+ num_iterations: u32,
+ pcl_size: usize,
+ func: fn(Vec) -> bool,
+) -> Duration {
+ let mut total_time = Duration::new(0, 0);
+ for _ in 0..num_iterations {
+ total_time += measure_func(pcl_size, func);
+ }
+ total_time / num_iterations
+}
+
+fn measure_func(pcl_size: usize, func: F) -> Duration
+where
+ F: Fn(Vec) -> bool,
+{
+ let cloud_points = generate_random_pointcloud(pcl_size, f32::MIN / 2.0, f32::MAX / 2.0);
+ let start = std::time::Instant::now();
+ let _ = func(cloud_points);
+ start.elapsed()
+}
+
+fn main() {
+ println!("100k");
+ let how_many = 10_000;
+ let how_often = 1_000;
+
+ let dur = measure_func_avg(how_often, how_many, roundtrip);
+ println!("roundtrip: {:?}", dur);
+
+ #[cfg(feature = "rayon")]
+ let dur = measure_func_avg(how_often, how_many, roundtrip_par);
+ println!("roundtrip_par: {:?}", dur);
+
+ println!("200k");
+ let how_many = 200_000;
+ let how_often = 100;
+
+ let dur = measure_func_avg(how_often, how_many, roundtrip_filter);
+ println!("roundtrip_filter: {:?}", dur);
+
+ #[cfg(feature = "rayon")]
+ let dur = measure_func_avg(how_often, how_many, roundtrip_filter_par);
+ println!("roundtrip_filter_par: {:?}", dur);
+
+ println!("10m");
+ let how_many = 10_000_000;
+ let how_often = 10;
+
+ let dur = measure_func_avg(how_often, how_many, roundtrip_filter);
+ println!("roundtrip_filter: {:?}", dur);
+
+ #[cfg(feature = "rayon")]
+ let dur = measure_func_avg(how_often, how_many, roundtrip_filter_par);
+ println!("roundtrip_filter_par: {:?}", dur);
+}
diff --git a/examples/simple_distance_filter.rs b/examples/simple_distance_filter.rs
new file mode 100644
index 0000000..d594765
--- /dev/null
+++ b/examples/simple_distance_filter.rs
@@ -0,0 +1,31 @@
+/// This example demonstrates a very simple distance filter with predefined point types.
+/// Note that this example is a simplified version of the custom_enum_field_filter.rs example.
+/// Also, it effectively demonstrates a typesafe byte-to-byte buffer filter with a single iteration.
+///
+/// It also works without any dependencies, making it a good "hello world" example.
+use ros_pointcloud2::prelude::*;
+
+fn main() {
+ let cloud = vec![
+ PointXYZ::new(1.0, 1.0, 1.0),
+ PointXYZ::new(2.0, 2.0, 2.0),
+ PointXYZ::new(3.0, 3.0, 3.0),
+ ];
+
+ println!("Original cloud: {:?}", cloud);
+
+ let msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+
+ println!("filtering by distance < 1.9m");
+ let out = msg
+ .try_into_iter()
+ .unwrap()
+ .filter(|point: &PointXYZ| {
+ (point.x.powi(2) + point.y.powi(2) + point.z.powi(2)).sqrt() < 1.9
+ })
+ .collect::>();
+
+ println!("Filtered cloud: {:?}", out);
+
+ assert_eq!(vec![PointXYZ::new(1.0, 1.0, 1.0),], out);
+}
diff --git a/rpcl2_derive/Cargo.toml b/rpcl2_derive/Cargo.toml
new file mode 100644
index 0000000..05371e7
--- /dev/null
+++ b/rpcl2_derive/Cargo.toml
@@ -0,0 +1,12 @@
+[package]
+name = "rpcl2_derive"
+version = "0.1.0"
+edition = "2021"
+
+[lib]
+proc-macro = true
+
+[dependencies]
+syn = "2"
+quote = "1"
+proc-macro2 = "1"
\ No newline at end of file
diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs
new file mode 100644
index 0000000..a03e5bc
--- /dev/null
+++ b/rpcl2_derive/src/lib.rs
@@ -0,0 +1,155 @@
+extern crate proc_macro;
+
+use std::collections::HashMap;
+
+use proc_macro::TokenStream;
+use quote::{quote, ToTokens};
+use syn::{parse_macro_input, DeriveInput};
+
+fn get_allowed_types() -> HashMap<&'static str, usize> {
+ let mut allowed_datatypes = HashMap::<&'static str, usize>::new();
+ allowed_datatypes.insert("f32", 4);
+ allowed_datatypes.insert("f64", 8);
+ allowed_datatypes.insert("i32", 4);
+ allowed_datatypes.insert("u8", 1);
+ allowed_datatypes.insert("u16", 2);
+ allowed_datatypes.insert("u32", 4);
+ allowed_datatypes.insert("i8", 1);
+ allowed_datatypes.insert("i16", 2);
+ allowed_datatypes
+}
+
+/// Derive macro for the `Fields` trait.
+///
+/// Given the ordering from the source code of your struct, this macro will generate an array of field names.
+#[proc_macro_derive(RosFields)]
+pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
+ let input = parse_macro_input!(input as DeriveInput);
+ let name = input.clone().ident;
+
+ let fields = match input.data {
+ syn::Data::Struct(ref data) => data.fields.clone(),
+ _ => return syn::Error::new_spanned(input, "Only structs are supported").to_compile_error().into(),
+ };
+
+ let allowed_datatypes = get_allowed_types();
+
+ if fields.is_empty() {
+ return syn::Error::new_spanned(input, "No fields found").to_compile_error().into();
+ }
+
+ for field in fields.iter() {
+ let ty = field.ty.to_token_stream().to_string();
+ if !allowed_datatypes.contains_key(&ty.as_str()) {
+ return syn::Error::new_spanned(field, "Field type not allowed").to_compile_error().into();
+ }
+ }
+
+ let field_len_token: usize = fields.len();
+
+ let field_names = fields.iter().map(|field| {
+ let field_name = field.ident.as_ref().unwrap();
+ quote! { stringify!(#field_name) }
+ }).collect::>();
+
+ let field_impl = quote! {
+ impl Fields<#field_len_token> for #name {
+ fn field_names_ordered() -> [&'static str; #field_len_token] {
+ [
+ #(#field_names,)*
+ ]
+ }
+ }
+ };
+
+ TokenStream::from(field_impl)
+}
+
+/// This macro will fully implement the `PointConvertible` trait for your struct so you can use your point for the PointCloud2 conversion.
+///
+/// Note that the repr(C) attribute is required for the struct to work efficiently with C++ PCL.
+/// With Rust layout optimizations, the struct might not work with the PCL library but the message still conforms to the specification of PointCloud2.
+/// Furthermore, Rust layout can lead to smaller messages to be send over the network.
+#[proc_macro_derive(RosFull)]
+pub fn ros_point_derive(input: TokenStream) -> TokenStream {
+ let input = parse_macro_input!(input as DeriveInput);
+ let name = input.clone().ident;
+
+ let fields = match input.data {
+ syn::Data::Struct(ref data) => data.fields.clone(),
+ _ => return syn::Error::new_spanned(input, "Only structs are supported").to_compile_error().into(),
+ };
+
+ let allowed_datatypes = get_allowed_types();
+
+ if fields.is_empty() {
+ return syn::Error::new_spanned(input, "No fields found").to_compile_error().into();
+ }
+
+ for field in fields.iter() {
+ let ty = field.ty.to_token_stream().to_string();
+ if !allowed_datatypes.contains_key(&ty.as_str()) {
+ return syn::Error::new_spanned(field, "Field type not allowed").to_compile_error().into();
+ }
+ }
+
+ let field_len_token: usize = fields.len();
+
+ let field_names = fields.iter().map(|field| {
+ let field_name = field.ident.as_ref().unwrap();
+ quote! { stringify!(#field_name) }
+ }).collect::>();
+
+ let field_impl = quote! {
+ impl ros_pointcloud2::Fields<#field_len_token> for #name {
+ fn field_names_ordered() -> [&'static str; #field_len_token] {
+ [
+ #(#field_names,)*
+ ]
+ }
+ }
+ };
+
+ let field_names_get = fields.iter().enumerate().map(|(idx, f)| {
+ let field_name = f.ident.as_ref().unwrap();
+ quote! { #field_name: point.fields[#idx].get() }
+ }).collect::>();
+
+ let from_my_point = quote! {
+ impl From> for #name {
+ fn from(point: ros_pointcloud2::RPCL2Point<#field_len_token>) -> Self {
+ Self {
+ #(#field_names_get,)*
+ }
+ }
+ }
+ };
+
+ let field_names_into = fields.iter().map(|f| {
+ let field_name = f.ident.as_ref().unwrap();
+ quote! { point.#field_name.into() }
+ }).collect::>();
+
+ let from_custom_point = quote! {
+ impl From<#name> for ros_pointcloud2::RPCL2Point<#field_len_token> {
+ fn from(point: #name) -> Self {
+ ros_pointcloud2::RPCL2Point {
+ fields: [ #(#field_names_into,)* ]
+ }
+ }
+ }
+ };
+
+ let convertible = quote! {
+ impl ros_pointcloud2::PointConvertible<#field_len_token> for #name {}
+ };
+
+ let out = TokenStream::from(quote! {
+ #field_impl
+ #from_my_point
+ #from_custom_point
+ #convertible
+ });
+
+ TokenStream::from(out)
+}
\ No newline at end of file
diff --git a/src/convert.rs b/src/convert.rs
index c5d055f..fc6edf3 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -12,19 +12,44 @@ pub enum FieldDatatype {
U32,
I8,
I16,
+
+ /// While RGB is not officially supported by ROS, it is used in practice as a packed f32.
+ /// To make it easier to work with and avoid packing code, the
+ /// [`ros_pointcloud2::points::RGB`] union is supported here and handled like a f32.
+ RGB,
}
impl FieldDatatype {
pub fn size(&self) -> usize {
match self {
- FieldDatatype::U8 => 1,
- FieldDatatype::U16 => 2,
- FieldDatatype::U32 => 4,
- FieldDatatype::I8 => 1,
- FieldDatatype::I16 => 2,
- FieldDatatype::I32 => 4,
- FieldDatatype::F32 => 4,
- FieldDatatype::F64 => 8,
+ FieldDatatype::U8 => std::mem::size_of::(),
+ FieldDatatype::U16 => std::mem::size_of::(),
+ FieldDatatype::U32 => std::mem::size_of::(),
+ FieldDatatype::I8 => std::mem::size_of::(),
+ FieldDatatype::I16 => std::mem::size_of::(),
+ FieldDatatype::I32 => std::mem::size_of::(),
+ FieldDatatype::F32 => std::mem::size_of::(),
+ FieldDatatype::F64 => std::mem::size_of::(),
+ FieldDatatype::RGB => std::mem::size_of::(), // packed in f32
+ }
+ }
+}
+
+impl TryFrom for FieldDatatype {
+ type Error = MsgConversionError;
+
+ fn try_from(value: String) -> Result {
+ match value.to_lowercase().as_str() {
+ "f32" => Ok(FieldDatatype::F32),
+ "f64" => Ok(FieldDatatype::F64),
+ "i32" => Ok(FieldDatatype::I32),
+ "u8" => Ok(FieldDatatype::U8),
+ "u16" => Ok(FieldDatatype::U16),
+ "u32" => Ok(FieldDatatype::U32),
+ "i8" => Ok(FieldDatatype::I8),
+ "i16" => Ok(FieldDatatype::I16),
+ "rgb" => Ok(FieldDatatype::RGB),
+ _ => Err(MsgConversionError::UnsupportedFieldType(value)),
}
}
}
@@ -82,8 +107,15 @@ impl GetFieldDatatype for i16 {
}
}
+/// Convenience implementation for the RGB union.
+impl GetFieldDatatype for crate::points::RGB {
+ fn field_datatype() -> FieldDatatype {
+ FieldDatatype::RGB
+ }
+}
+
impl TryFrom for FieldDatatype {
- type Error = ConversionError;
+ type Error = MsgConversionError;
fn try_from(value: u8) -> Result {
match value {
@@ -95,7 +127,7 @@ impl TryFrom for FieldDatatype {
6 => Ok(FieldDatatype::U32),
7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64),
- _ => Err(ConversionError::UnsupportedFieldType),
+ _ => Err(MsgConversionError::UnsupportedFieldType(value.to_string())),
}
}
}
@@ -111,35 +143,19 @@ impl From for u8 {
FieldDatatype::U32 => 6,
FieldDatatype::F32 => 7,
FieldDatatype::F64 => 8,
+ FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer
}
}
}
-pub(crate) fn check_coord(
- coord: Option,
- fields: &[PointFieldMsg],
- xyz_field_type: &FieldDatatype,
-) -> Result {
- match coord {
- Some(y_idx) => {
- let field = &fields[y_idx];
- if field.datatype != u8::from(*xyz_field_type) {
- return Err(ConversionError::InvalidFieldFormat);
- }
- Ok(field.clone())
- }
- None => Err(ConversionError::NotEnoughFields),
- }
-}
-
-/// Matching field names from each meta data per point to the PointField name.
+/// Matching field names from each data point.
/// Always make sure to use the same order as in your conversion implementation to have a correct mapping.
///
/// This trait is needed to implement the `PointConvertible` trait.
///
-/// # Example for full point conversion.
+/// # Example
/// ```
-/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
+/// use ros_pointcloud2::prelude::*;
///
/// #[derive(Clone, Debug, PartialEq, Copy)]
/// pub struct MyPointXYZI {
@@ -149,14 +165,14 @@ pub(crate) fn check_coord(
/// pub intensity: f32,
/// }
///
-/// impl MetaNames<1> for MyPointXYZI {
-/// fn meta_names() -> [&'static str; 1] {
-/// ["intensity"]
+/// impl Fields<4> for MyPointXYZI {
+/// fn field_names_ordered() -> [&'static str; 4] {
+/// ["x", "y", "z", "intensity"]
/// }
/// }
/// ```
-pub trait MetaNames {
- fn meta_names() -> [&'static str; METADIM];
+pub trait Fields {
+ fn field_names_ordered() -> [&'static str; N];
}
/// This trait is used to convert a byte slice to a primitive type.
@@ -253,6 +269,23 @@ impl FromBytes for f32 {
}
}
+impl FromBytes for points::RGB {
+ #[inline]
+ fn from_be_bytes(bytes: &[u8]) -> Self {
+ Self::new_from_packed_f32(f32::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]))
+ }
+
+ #[inline]
+ fn from_le_bytes(bytes: &[u8]) -> Self {
+ Self::new_from_packed_f32(f32::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]))
+ }
+
+ #[inline]
+ fn bytes(x: &points::RGB) -> Vec {
+ Vec::from(x.raw().to_le_bytes())
+ }
+}
+
impl FromBytes for i32 {
#[inline]
fn from_be_bytes(bytes: &[u8]) -> Self {
@@ -305,44 +338,14 @@ impl FromBytes for u8 {
}
}
-#[derive(Default, Clone, Debug, PartialEq)]
-pub(crate) enum Endianness {
+#[derive(Default, Clone, Debug, PartialEq, Copy)]
+pub enum Endianness {
Big,
#[default]
Little,
}
-#[inline(always)]
-pub(crate) fn load_loadable(
- start_idx: usize,
- data: &[u8],
- endian: &Endianness,
-) -> T {
- match endian {
- Endianness::Big => T::from_be_bytes(load_bytes::(start_idx, data).as_slice()),
- Endianness::Little => T::from_le_bytes(load_bytes::(start_idx, data).as_slice()),
- }
-}
-
-/// Note: check if the data slice is long enough to load the bytes beforehand! Uses unsafe indexing.
-#[inline(always)]
-fn load_bytes(start_idx: usize, data: &[u8]) -> [u8; S] {
- let mut target = [u8::default(); S];
-
- debug_assert!(target.len() == S);
- debug_assert!(data.len() >= start_idx + S);
-
- let source = unsafe { data.get_unchecked(start_idx..start_idx + S) };
- target
- .iter_mut()
- .zip(source.iter())
- .for_each(|(target, source)| {
- *target = *source;
- });
- target
-}
-
#[cfg(test)]
mod tests {
use super::*;
diff --git a/src/iterator.rs b/src/iterator.rs
new file mode 100644
index 0000000..99135de
--- /dev/null
+++ b/src/iterator.rs
@@ -0,0 +1,492 @@
+use crate::{
+ convert::{Endianness, FieldDatatype},
+ Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointMeta, RPCL2Point,
+};
+
+/// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg.
+///
+/// The iterator is defined at compile time, so the point has to be described via template arguments.
+///
+/// When using within a ROS node, the PointCloud2 (created by the ROS crate) must be converted first.
+/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
+///
+/// ROS1 with rosrust:
+/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
+/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
+///
+/// ROS2 with r2r:
+/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
+/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
+///
+/// ros_pointcloud2 supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags.
+///
+pub struct PointCloudIterator
+where
+ C: Fields,
+{
+ iteration: usize,
+ iteration_back: usize,
+ data: ByteBufferView,
+ phantom_c: std::marker::PhantomData, // internally used for meta names array
+}
+
+#[cfg(feature = "rayon")]
+impl ExactSizeIterator for PointCloudIterator
+where
+ C: PointConvertible + Send + Sync,
+{
+ fn len(&self) -> usize {
+ self.data.len()
+ }
+}
+
+#[cfg(feature = "rayon")]
+impl DoubleEndedIterator for PointCloudIterator
+where
+ C: PointConvertible + Send + Sync,
+{
+ fn next_back(&mut self) -> Option {
+ if self.iteration_back < self.iteration {
+ return None; // iteration finished
+ }
+
+ let p = self.data.point_at(self.iteration_back);
+ self.iteration_back -= 1;
+ Some(C::from(p))
+ }
+}
+
+#[cfg(feature = "rayon")]
+impl rayon::iter::ParallelIterator for PointCloudIterator
+where
+ C: PointConvertible + Send + Sync,
+{
+ type Item = C;
+
+ fn drive_unindexed(self, consumer: Co) -> Co::Result
+ where
+ Co: rayon::iter::plumbing::UnindexedConsumer,
+ {
+ rayon::iter::plumbing::bridge(self, consumer)
+ }
+
+ fn opt_len(&self) -> Option {
+ Some(self.data.len())
+ }
+}
+
+#[cfg(feature = "rayon")]
+impl rayon::iter::IndexedParallelIterator for PointCloudIterator
+where
+ C: PointConvertible + Send + Sync,
+{
+ fn len(&self) -> usize {
+ self.data.len()
+ }
+
+ fn drive(self, consumer: Co) -> Co::Result
+ where
+ Co: rayon::iter::plumbing::Consumer,
+ {
+ rayon::iter::plumbing::bridge(self, consumer)
+ }
+
+ fn with_producer>(
+ self,
+ callback: CB,
+ ) -> CB::Output {
+ callback.callback(RayonProducer::from(self))
+ }
+}
+
+#[cfg(feature = "rayon")]
+struct RayonProducer> {
+ iter: PointCloudIterator,
+}
+
+#[cfg(feature = "rayon")]
+impl rayon::iter::plumbing::Producer for RayonProducer
+where
+ C: PointConvertible + Send + Sync,
+{
+ type Item = C;
+ type IntoIter = PointCloudIterator;
+
+ fn into_iter(self) -> Self::IntoIter {
+ self.iter
+ }
+
+ fn split_at(self, point_index: usize) -> (Self, Self) {
+ let (left, right) = self.iter.split_at(point_index);
+ (RayonProducer { iter: left }, RayonProducer { iter: right })
+ }
+}
+
+#[cfg(feature = "rayon")]
+impl From> for RayonProducer
+where
+ C: PointConvertible + Send + Sync,
+{
+ fn from(iterator: PointCloudIterator) -> Self {
+ Self { iter: iterator }
+ }
+}
+
+/// Implementation of the iterator trait.
+impl Iterator for PointCloudIterator
+where
+ C: PointConvertible,
+{
+ type Item = C;
+
+ fn size_hint(&self) -> (usize, Option) {
+ let buf_len = self.data.len();
+ (buf_len, Some(buf_len))
+ }
+
+ fn next(&mut self) -> Option {
+ if self.iteration >= self.data.len() || self.iteration_back < self.iteration {
+ return None; // iteration finished
+ }
+
+ let p = self.data.point_at(self.iteration);
+ self.iteration += 1;
+ Some(C::from(p))
+ }
+}
+
+struct ByteBufferView {
+ data: std::sync::Arc<[u8]>,
+ start_point_idx: usize,
+ end_point_idx: usize,
+ point_step_size: usize,
+ offsets: [usize; N],
+ meta: Vec<(String, FieldDatatype)>,
+ endianness: Endianness,
+}
+
+impl ByteBufferView {
+ fn new(
+ data: Vec,
+ point_step_size: usize,
+ start_point_idx: usize,
+ end_point_idx: usize,
+ offsets: [usize; N],
+ meta: Vec<(String, FieldDatatype)>,
+ endianness: Endianness,
+ ) -> Self {
+ Self {
+ data: std::sync::Arc::<[u8]>::from(data),
+ start_point_idx,
+ end_point_idx,
+ point_step_size,
+ offsets,
+ meta,
+ endianness,
+ }
+ }
+
+ #[inline]
+ fn len(&self) -> usize {
+ self.end_point_idx - self.start_point_idx + 1
+ }
+
+ #[inline(always)]
+ fn point_at(&self, idx: usize) -> RPCL2Point {
+ let offset = (self.start_point_idx + idx) * self.point_step_size;
+
+ // TODO memcpy entire point at once, then extract fields?
+ let mut meta = [PointMeta::default(); N];
+ meta.iter_mut()
+ .zip(self.offsets.iter())
+ .zip(self.meta.iter())
+ .for_each(|((p_meta, in_point_offset), (_, meta_type))| {
+ *p_meta = PointMeta::from_buffer(
+ &self.data,
+ offset + in_point_offset,
+ *meta_type,
+ self.endianness,
+ );
+ });
+
+ RPCL2Point { fields: meta }
+ }
+
+ #[inline]
+ fn clone_with_bounds(&self, start: usize, size: usize) -> Self {
+ Self {
+ data: self.data.clone(),
+ start_point_idx: start,
+ end_point_idx: start + size - 1,
+ point_step_size: self.point_step_size,
+ offsets: self.offsets,
+ meta: self.meta.clone(),
+ endianness: self.endianness,
+ }
+ }
+
+ #[inline]
+ pub fn split_at(self, point_index: usize) -> (Self, Self) {
+ let left_start = self.start_point_idx;
+ let left_size = point_index;
+
+ let right_start = point_index;
+ let right_size = self.len() - point_index;
+
+ (
+ self.clone_with_bounds(left_start, left_size),
+ self.clone_with_bounds(right_start, right_size),
+ )
+ }
+}
+
+impl TryFrom for PointCloudIterator
+where
+ C: Fields,
+{
+ type Error = MsgConversionError;
+
+ /// Convert a PointCloud2Msg into an iterator.
+ /// Converting a PointCloud2Msg into an iterator is a fallible operation since the message can contain only a subset of the required fields.
+ ///
+ /// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
+ /// It therefore has a constant time complexity O(1) for practical purposes.
+ fn try_from(cloud: PointCloud2Msg) -> Result {
+ let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];
+
+ let not_found_fieldnames = C::field_names_ordered()
+ .into_iter()
+ .map(|name| {
+ let found = cloud.fields.iter().any(|field| field.name == *name);
+ (name, found)
+ })
+ .filter(|(_, found)| !*found)
+ .collect::>();
+
+ if !not_found_fieldnames.is_empty() {
+ let names_not_found = not_found_fieldnames
+ .into_iter()
+ .map(|(name, _)| (*name).to_owned())
+ .collect::>();
+ return Err(MsgConversionError::FieldNotFound(names_not_found));
+ }
+
+ let ordered_fieldnames = C::field_names_ordered();
+ for (field, with_offset) in cloud.fields.iter().zip(meta_with_offsets.iter_mut()) {
+ if ordered_fieldnames.contains(&field.name.as_str()) {
+ *with_offset = (
+ field.name.clone(),
+ field.datatype.try_into()?,
+ field.offset as usize,
+ );
+ }
+ }
+
+ meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
+
+ debug_assert!(
+ meta_with_offsets.len() == N,
+ "Not all fields were found in the message. Expected {} but found {}.",
+ N,
+ meta_with_offsets.len()
+ );
+
+ let mut offsets = [usize::default(); N];
+ let mut meta = vec![(String::default(), FieldDatatype::default()); N];
+
+ meta_with_offsets
+ .into_iter()
+ .zip(meta.iter_mut())
+ .zip(offsets.iter_mut())
+ .for_each(|(((name, datatype, offset), meta), meta_offset)| {
+ *meta = (name, datatype);
+ *meta_offset = offset;
+ });
+
+ let endian = if cloud.is_bigendian {
+ Endianness::Big
+ } else {
+ Endianness::Little
+ };
+
+ let point_step_size = cloud.point_step as usize;
+ let cloud_length = cloud.width as usize * cloud.height as usize;
+ if point_step_size * cloud_length != cloud.data.len() {
+ return Err(MsgConversionError::DataLengthMismatch);
+ }
+
+ let last_offset = offsets.last().expect("Dimensionality is 0.");
+
+ let last_meta = meta.last().expect("Dimensionality is 0.");
+ let size_with_last_meta = last_offset + last_meta.1.size();
+ if size_with_last_meta > point_step_size {
+ return Err(MsgConversionError::DataLengthMismatch);
+ }
+
+ let cloud_length = cloud.width as usize * cloud.height as usize;
+
+ let data = ByteBufferView::new(
+ cloud.data,
+ point_step_size,
+ 0,
+ cloud_length - 1,
+ offsets,
+ meta,
+ endian,
+ );
+
+ Ok(Self {
+ iteration: 0,
+ iteration_back: cloud_length - 1,
+ data,
+ phantom_c: std::marker::PhantomData,
+ })
+ }
+}
+
+impl PointCloudIterator
+where
+ C: Fields,
+{
+ #[inline]
+ fn from_byte_buffer_view(data: ByteBufferView) -> Self {
+ Self {
+ iteration: 0,
+ iteration_back: data.len() - 1,
+ data,
+ phantom_c: std::marker::PhantomData,
+ }
+ }
+
+ #[inline]
+ pub fn split_at(self, point_index: usize) -> (Self, Self) {
+ let (left_data, right_data) = self.data.split_at(point_index);
+ (
+ Self::from_byte_buffer_view(left_data),
+ Self::from_byte_buffer_view(right_data),
+ )
+ }
+}
+
+#[cfg(feature = "rayon")]
+mod test {
+
+ #[test]
+ fn test_double_ended_iter() {
+ let cloud = vec![
+ crate::points::PointXYZ {
+ x: 1.0,
+ y: 1.0,
+ z: 1.0,
+ },
+ crate::points::PointXYZ {
+ x: 2.0,
+ y: 2.0,
+ z: 2.0,
+ },
+ crate::points::PointXYZ {
+ x: 3.0,
+ y: 3.0,
+ z: 3.0,
+ },
+ ];
+
+ let internal_msg = crate::PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let mut iter = crate::iterator::PointCloudIterator::try_from(internal_msg).unwrap();
+ let last_p = iter.next_back();
+
+ assert!(last_p.is_some());
+ let last_p: crate::points::PointXYZ = last_p.unwrap();
+
+ assert_eq!(last_p.x, 3.0);
+ assert_eq!(last_p.y, 3.0);
+ assert_eq!(last_p.z, 3.0);
+
+ let first_p = iter.next();
+ assert!(first_p.is_some());
+ let first_p: crate::points::PointXYZ = first_p.unwrap();
+
+ assert_eq!(first_p.x, 1.0);
+ assert_eq!(first_p.y, 1.0);
+ assert_eq!(first_p.z, 1.0);
+
+ let last_p = iter.next_back();
+ assert!(last_p.is_some());
+ let last_p: crate::points::PointXYZ = last_p.unwrap();
+
+ assert_eq!(last_p.x, 2.0);
+ assert_eq!(last_p.y, 2.0);
+ assert_eq!(last_p.z, 2.0);
+
+ let last_p = iter.next_back();
+ assert!(last_p.is_none());
+
+ let first_p = iter.next();
+ assert!(first_p.is_none());
+ }
+
+ #[test]
+ fn test_split_at() {
+ let cloud = vec![
+ crate::points::PointXYZ {
+ x: 1.0,
+ y: 1.0,
+ z: 1.0,
+ },
+ crate::points::PointXYZ {
+ x: 2.0,
+ y: 2.0,
+ z: 2.0,
+ },
+ crate::points::PointXYZ {
+ x: 3.0,
+ y: 3.0,
+ z: 3.0,
+ },
+ ];
+
+ let internal_msg = crate::PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let iter = crate::iterator::PointCloudIterator::try_from(internal_msg).unwrap();
+
+ let (mut left, mut right) = iter.split_at(1);
+
+ assert_eq!(left.size_hint(), (1, Some(1)));
+ assert_eq!(right.size_hint(), (2, Some(2)));
+
+ let first_left = left.next();
+ assert!(first_left.is_some());
+ let first_left: crate::points::PointXYZ = first_left.unwrap();
+
+ assert_eq!(first_left.x, 1.0);
+ assert_eq!(first_left.y, 1.0);
+ assert_eq!(first_left.z, 1.0);
+
+ let first_right = right.next();
+ assert!(first_right.is_some());
+ let first_right: crate::points::PointXYZ = first_right.unwrap();
+
+ assert_eq!(first_right.x, 2.0);
+ assert_eq!(first_right.y, 2.0);
+ assert_eq!(first_right.z, 2.0);
+
+ let last_right = right.next_back();
+ assert!(last_right.is_some());
+
+ let last_right: crate::points::PointXYZ = last_right.unwrap();
+ assert_eq!(last_right.x, 3.0);
+ assert_eq!(last_right.y, 3.0);
+ assert_eq!(last_right.z, 3.0);
+
+ let last_left = left.next_back();
+ assert!(last_left.is_none());
+
+ let last_right = right.next_back();
+ assert!(last_right.is_none());
+
+ let first_left = left.next();
+ assert!(first_left.is_none());
+
+ let first_right = right.next();
+ assert!(first_right.is_none());
+ }
+}
diff --git a/src/lib.rs b/src/lib.rs
index dd868a1..9956fc4 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,167 +1,440 @@
//! A library to work with the PointCloud2 message type from ROS.
//!
-//! ros_pointcloud2 mainly provides iterator-based [`ros_pointcloud2::reader`] and [`ros_pointcloud2::writer`] to interact
-//! with the internal message type [`ros_pointcloud2::PointCloud2Msg`] without forcing an iteration.
+//! ros_pointcloud2 mainly provides a [`ros_pointcloud2::PointCloud2Msg`] type that implements
+//! functions for conversion to and from iterators.
+//!
+//! - [`ros_pointcloud2::PointCloud2Msg::try_from_iter`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_into_iter`]
//!
//! For ROS interoperability, the message type generated by the respective crate must be converted to
//! the [`ros_pointcloud2::PointCloud2Msg`] using the `From` trait,
-//! which is mostly a ownership transfer without copying the point data.
+//! which mostly does ownership transfers without copying the point data.
//!
//! There are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust_msg` message types
//! available in the feature flags. If you miss a message type, please open an issue or a PR.
//! See the [`ros_pointcloud2::ros_types`] module for more information.
//!
-//! Common point types like `PointXYZ` are provided in the [`ros_pointcloud2::pcl_utils`] module. You can implement any custom point type
+//! Common point types like [`ros_pointcloud2::pcl_utils::PointXYZ`] or
+//! [`ros_pointcloud2::pcl_utils::PointXYZI`] are provided in the
+//! [`ros_pointcloud2::pcl_utils`] module. You can implement any custom point type
//! that can be described by the specification.
-//! See the [`ros_pointcloud2::Reader`] or [`ros_pointcloud2::Writer`] struct documentation for an example.
//!
-//! # Example
+//! # Example PointXYZ
//! ```
-//! use ros_pointcloud2::{
-//! pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
-//! };
+//! use ros_pointcloud2::prelude::*;
//!
-//! // Your points (here using the predefined type PointXYZ).
+//! // PointXYZ is predefined
//! let cloud_points = vec![
-//! PointXYZ {x: 9.0006, y: 42.0, z: -6.2,},
-//! PointXYZ {x: f32::MAX, y: f32::MIN,z: f32::MAX,},
+//! PointXYZ::new(9.0006, 42.0, -6.2),
+//! PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
//! ];
//!
//! // For equality test later
//! let cloud_copy = cloud_points.clone();
//!
-//! // Vector -> Writer -> Message.
-//! // You can also just give the Vec or anything that implements `IntoIterator`.
-//! let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points.into_iter())
-//! .try_into() // iterating points here O(n)
-//! .unwrap();
+//! // Give the Vec or anything that implements `IntoIterator`.
+//! let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
//!
//! // Convert to your ROS crate message type, we will use r2r here.
-//! // let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
-//!
+//! // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
//! // Publish ...
-//!
//! // ... now incoming from a topic.
-//! // let internal_msg: PointCloud2Msg = msg.into();
+//! // let in_msg: PointCloud2Msg = msg.into();
//!
-//! // Message -> Reader -> your pipeline. The Reader implements the Iterator trait.
-//! let reader = ReaderXYZ::try_from(internal_msg).unwrap();
-//! let new_cloud_points = reader
-//! .map(|point: PointXYZ| {
+//! let new_pcl = in_msg.try_into_iter().unwrap()
+//! .map(|point: PointXYZ| { // Define the data you want from the point
//! // Some logic here
//!
//! point
//! })
-//! .collect::>(); // iterating points here O(n)
+//! .collect::>(); // iterating points here O(n)
//!
-//! assert_eq!(new_cloud_points, cloud_copy);
+//! assert_eq!(new_pcl, cloud_copy);
//! ```
pub mod convert;
-pub mod pcl_utils;
+pub mod points;
+pub mod prelude;
pub mod ros_types;
-/// Macro to get the size of a type at compile time. This is a convenience macro to avoid writing out the full std::mem::size_of::().
-/// Use it for your custom Reader and Writer implementations.
-/// # Example
-/// ```
-/// use ros_pointcloud2::{
-/// writer::Writer, size_of, pcl_utils::PointXYZ,
-/// };
-///
-/// type MyWriterXYZ = Writer;
-/// ```
-#[macro_export]
-macro_rules! size_of {
- ($t:ty) => {
- std::mem::size_of::<$t>()
- };
+pub mod iterator;
+
+use crate::convert::{FieldDatatype, FromBytes};
+use crate::ros_types::{HeaderMsg, PointFieldMsg};
+use convert::Endianness;
+pub use convert::Fields;
+
+/// All errors that can occur while converting to or from the PointCloud2 message.
+pub enum MsgConversionError {
+ InvalidFieldFormat,
+ NotEnoughFields,
+ TooManyDimensions,
+ UnsupportedFieldType(String),
+ NoPoints,
+ DataLengthMismatch,
+ FieldNotFound(Vec),
}
-pub mod reader;
-pub mod writer;
+impl std::fmt::Display for MsgConversionError {
+ fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+ match self {
+ MsgConversionError::InvalidFieldFormat => {
+ write!(f, "The field does not match the expected datatype.")
+ }
+ MsgConversionError::NotEnoughFields => {
+ write!(f, "Not enough fields in the PointCloud2 message.")
+ }
+ MsgConversionError::TooManyDimensions => {
+ write!(
+ f,
+ "The dimensionality of the point inside the message is too high."
+ )
+ }
+ MsgConversionError::UnsupportedFieldType(datatype) => {
+ write!(
+ f,
+ "The field datatype is not supported by the ROS message description: {}",
+ datatype
+ )
+ }
+ MsgConversionError::NoPoints => {
+ write!(f, "There are no points in the point cloud.")
+ }
+ MsgConversionError::DataLengthMismatch => {
+ write!(f, "The length of the byte buffer in the message does not match the expected length computed from the fields.")
+ }
+ MsgConversionError::FieldNotFound(fields) => {
+ write!(f, "There are fields missing in the message: {:?}", fields)
+ }
+ }
+ }
+}
-pub use convert::MetaNames;
-pub use ros_types::PointCloud2Msg;
+impl std::fmt::Debug for MsgConversionError {
+ fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+ ::fmt(self, f)
+ }
+}
-use crate::convert::{
- FieldDatatype,
- FromBytes,
-};
+impl std::error::Error for MsgConversionError {}
-use crate::ros_types::PointFieldMsg;
+#[cfg(feature = "derive")]
+fn system_endian() -> Endianness {
+ if cfg!(target_endian = "big") {
+ Endianness::Big
+ } else if cfg!(target_endian = "little") {
+ Endianness::Little
+ } else {
+ panic!("Unsupported endianness");
+ }
+}
-/// All errors that can occur for creating Reader and Writer.
-#[derive(Debug)]
-pub enum ConversionError {
- /// The coordinate field does not match the expected datatype.
- InvalidFieldFormat,
+#[derive(Clone, Debug)]
+pub struct PointCloud2Msg {
+ pub header: HeaderMsg,
+ pub height: u32,
+ pub width: u32,
+ pub fields: Vec,
+ pub is_bigendian: bool,
+ pub point_step: u32,
+ pub row_step: u32,
+ pub data: Vec,
+ pub is_dense: bool,
+}
- /// Not enough meta or dimensional fields in the PointCloud2 message.
- NotEnoughFields,
+impl Default for PointCloud2Msg {
+ fn default() -> Self {
+ Self {
+ header: HeaderMsg::default(),
+ height: 1, // everything is in one row (unstructured)
+ width: 0,
+ fields: Vec::new(),
+ is_bigendian: false, // ROS default
+ point_step: 0,
+ row_step: 0,
+ data: Vec::new(),
+ is_dense: false, // ROS default
+ }
+ }
+}
- /// The dimensionality of the point inside the message is too high.
- TooManyDimensions,
+impl PointCloud2Msg {
+ #[cfg(feature = "derive")]
+ fn prepare_direct_copy() -> Result
+ where
+ C: PointConvertible,
+ {
+ let point: RPCL2Point = C::default().into();
+ debug_assert!(point.fields.len() == N);
- /// The field type is not supported by the ROS message description.
- UnsupportedFieldType,
+ let meta_names = C::field_names_ordered();
+ debug_assert!(meta_names.len() == N);
- /// There are no points in the point cloud.
- NoPoints,
+ let mut offset: u32 = 0;
+ let layout = TypeLayoutInfo::try_from(C::type_layout())?;
+ let mut fields: Vec = Vec::with_capacity(layout.fields.len());
+ for f in layout.fields.into_iter() {
+ match f {
+ PointField::Field {
+ name,
+ datatype,
+ size,
+ } => {
+ fields.push(PointFieldMsg {
+ name,
+ offset,
+ datatype,
+ ..Default::default()
+ });
+ offset += size; // assume field_count 1
+ }
+ PointField::Padding(size) => {
+ offset += size; // assume field_count 1
+ }
+ }
+ }
- /// The length of the byte buffer in the message does not match the expected length computed from the fields.
- DataLengthMismatch,
+ Ok(PointCloud2Msg {
+ point_step: offset,
+ fields,
+ ..Default::default()
+ })
+ }
+
+ #[cfg(feature = "derive")]
+ fn assert_byte_similarity(&self) -> Result
+ where
+ C: PointConvertible,
+ {
+ let point: RPCL2Point = C::default().into();
+ debug_assert!(point.fields.len() == N);
+
+ let meta_names = C::field_names_ordered();
+ debug_assert!(meta_names.len() == N);
+
+ let mut offset: u32 = 0;
+ let layout = TypeLayoutInfo::try_from(C::type_layout())?;
+ for (f, msg_f) in layout.fields.into_iter().zip(self.fields.iter()) {
+ match f {
+ PointField::Field {
+ name,
+ datatype,
+ size,
+ } => {
+ if msg_f.name != name {
+ return Err(MsgConversionError::FieldNotFound(vec![name.clone()]));
+ }
+
+ if msg_f.datatype != datatype {
+ return Err(MsgConversionError::InvalidFieldFormat);
+ }
+
+ if msg_f.offset != offset {
+ return Err(MsgConversionError::DataLengthMismatch);
+ }
+
+ if msg_f.count != 1 {
+ return Err(MsgConversionError::DataLengthMismatch);
+ }
+
+ offset += size; // assume field_count 1
+ }
+ PointField::Padding(size) => {
+ offset += size; // assume field_count 1
+ }
+ }
+ }
+
+ Ok(true)
+ }
+
+ #[inline(always)]
+ fn prepare() -> Result
+ where
+ C: PointConvertible,
+ {
+ let point: RPCL2Point = C::default().into();
+ debug_assert!(point.fields.len() == N);
+
+ let meta_names = C::field_names_ordered();
+ debug_assert!(meta_names.len() == N);
+
+ let mut meta_offsets_acc = 0;
+ let mut fields = vec![PointFieldMsg::default(); N];
+ for ((meta_value, meta_name), field_val) in point
+ .fields
+ .into_iter()
+ .zip(meta_names.into_iter())
+ .zip(fields.iter_mut())
+ {
+ let datatype_code = meta_value.datatype.into();
+ FieldDatatype::try_from(datatype_code)?;
+
+ let field_count = 1;
+
+ *field_val = PointFieldMsg {
+ name: meta_name.into(),
+ offset: meta_offsets_acc,
+ datatype: datatype_code,
+ count: 1,
+ };
+
+ meta_offsets_acc += field_count * meta_value.datatype.size() as u32
+ }
+
+ Ok(PointCloud2Msg {
+ point_step: meta_offsets_acc,
+ fields,
+ ..Default::default()
+ })
+ }
+
+ /// Create a PointCloud2Msg from any iterable type.
+ ///
+ /// The operation is O(n) in time complexity where n is the number of points in the point cloud.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZ::new(1.0, 2.0, 3.0),
+ /// PointXYZ::new(4.0, 5.0, 6.0),
+ /// ];
+ ///
+ // let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+ /// ```
+ pub fn try_from_iter(
+ iterable: impl IntoIterator- ,
+ ) -> Result
+ where
+ C: PointConvertible,
+ {
+ let mut cloud = Self::prepare::()?;
+
+ iterable.into_iter().for_each(|coords| {
+ let point: RPCL2Point = coords.into();
+
+ point.fields.iter().for_each(|meta| {
+ let truncated_bytes = unsafe {
+ std::slice::from_raw_parts(meta.bytes.as_ptr(), meta.datatype.size())
+ };
+ cloud.data.extend_from_slice(truncated_bytes);
+ });
+
+ cloud.width += 1;
+ });
+
+ cloud.row_step = cloud.width * cloud.point_step;
+
+ Ok(cloud)
+ }
+
+ #[cfg(feature = "derive")]
+ pub fn try_from_vec(vec: Vec) -> Result
+ where
+ C: PointConvertible,
+ {
+ match system_endian() {
+ Endianness::Big => Self::try_from_iter(vec),
+ Endianness::Little => {
+ let mut cloud = Self::prepare_direct_copy::()?;
+
+ let bytes_total = vec.len() * cloud.point_step as usize;
+ cloud.data.resize(bytes_total, u8::default());
+ let raw_data: *mut C = cloud.data.as_ptr() as *mut C;
+ unsafe {
+ std::ptr::copy_nonoverlapping(
+ vec.as_ptr() as *const u8,
+ raw_data as *mut u8,
+ bytes_total,
+ );
+ }
+
+ cloud.width = vec.len() as u32;
+ cloud.row_step = cloud.width * cloud.point_step;
+
+ Ok(cloud)
+ }
+ }
+ }
+
+ #[cfg(feature = "derive")]
+ pub fn try_into_vec(self) -> Result, MsgConversionError>
+ where
+ C: PointConvertible,
+ {
+ self.assert_byte_similarity::()?;
+
+ match system_endian() {
+ Endianness::Big => Ok(self.try_into_iter()?.collect()),
+ Endianness::Little => {
+ let mut vec = Vec::with_capacity(self.width as usize);
+ let raw_data: *const C = self.data.as_ptr() as *const C;
+ unsafe {
+ for i in 0..self.width {
+ let point = raw_data.add(i as usize).read();
+ vec.push(point);
+ }
+ }
+
+ Ok(vec)
+ }
+ }
+ }
+
+ pub fn try_into_iter(
+ self,
+ ) -> Result, MsgConversionError>
+ where
+ C: PointConvertible,
+ {
+ iterator::PointCloudIterator::try_from(self)
+ }
+
+ #[cfg(feature = "rayon")]
+ pub fn try_into_par_iter(
+ self,
+ ) -> Result, MsgConversionError>
+ where
+ C: PointConvertible + Send + Sync,
+ {
+ iterator::PointCloudIterator::try_from(self)
+ }
}
/// Internal point representation. It is used to store the coordinates and meta data of a point.
-/// In each iteration, the Reader will convert the internal representation to the desired point type.
-/// Implement the `From` traits for your point type to use the conversion as part of the [`PointConvertible`] trait.
+/// In each iteration, an internal point representation is converted to the desired point type.
+/// Implement the `From` traits for your point type to use the conversion.
///
-/// # Example
-/// ```
-/// use ros_pointcloud2::Point;
-///
-/// #[derive(Clone, Debug, PartialEq, Copy)]
-/// pub struct MyPointXYZI {
-/// pub x: f32,
-/// pub y: f32,
-/// pub z: f32,
-/// pub intensity: f32,
-/// }
-///
-/// impl From for Point {
-/// fn from(point: MyPointXYZI) -> Self {
-/// Point {
-/// coords: [point.x, point.y, point.z],
-/// meta: [point.intensity.into()],
-/// }
-/// }
-/// }
-///
-/// impl From> for MyPointXYZI {
-/// fn from(point: Point) -> Self {
-/// Self {
-/// x: point.coords[0],
-/// y: point.coords[1],
-/// z: point.coords[2],
-/// intensity: point.meta[0].get(),
-/// }
-/// }
-/// }
-/// ```
-pub struct Point {
- pub coords: [T; DIM],
- pub meta: [PointMeta; METADIM],
+/// See the [`ros_pointcloud2::PointConvertible`] trait for more information.
+pub struct RPCL2Point {
+ pub fields: [PointMeta; N],
}
-/// Trait to convert a point to a tuple of coordinates and meta data.
-/// Implement this trait for your point type to use the conversion.
-///
+impl Default for RPCL2Point {
+ fn default() -> Self {
+ Self {
+ fields: [PointMeta::default(); N],
+ }
+ }
+}
+
+impl From<[PointMeta; N]> for RPCL2Point {
+ fn from(fields: [PointMeta; N]) -> Self {
+ Self { fields }
+ }
+}
+
+/// Trait to enable point conversions on the fly while iterating.
+///
+/// Implement this trait for your custom point you want to read or write in the message.
+///
/// # Example
/// ```
-/// use ros_pointcloud2::{Point, PointConvertible, MetaNames, size_of};
+/// use ros_pointcloud2::prelude::*;
///
-/// #[derive(Clone, Debug, PartialEq, Copy)]
+/// #[derive(Clone, Debug, PartialEq, Copy, Default)]
/// pub struct MyPointXYZI {
/// pub x: f32,
/// pub y: f32,
@@ -169,41 +442,94 @@ pub struct Point {
/// pub intensity: f32,
/// }
///
-/// impl From for Point {
+/// impl From for RPCL2Point<4> {
/// fn from(point: MyPointXYZI) -> Self {
-/// Point {
-/// coords: [point.x, point.y, point.z],
-/// meta: [point.intensity.into()],
+/// RPCL2Point {
+/// fields: [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()],
/// }
/// }
/// }
///
-/// impl From> for MyPointXYZI {
-/// fn from(point: Point) -> Self {
+/// impl From> for MyPointXYZI {
+/// fn from(point: RPCL2Point<4>) -> Self {
/// Self {
-/// x: point.coords[0],
-/// y: point.coords[1],
-/// z: point.coords[2],
-/// intensity: point.meta[0].get(),
+/// x: point.fields[0].get(),
+/// y: point.fields[1].get(),
+/// z: point.fields[2].get(),
+/// intensity: point.fields[3].get(),
/// }
/// }
/// }
///
-/// impl MetaNames<1> for MyPointXYZI {
-/// fn meta_names() -> [&'static str; 1] {
-/// ["intensity"]
+/// impl Fields<4> for MyPointXYZI {
+/// fn field_names_ordered() -> [&'static str; 4] {
+/// ["x", "y", "z", "intensity"]
/// }
/// }
///
-/// impl PointConvertible for MyPointXYZI {}
+/// impl PointConvertible<4> for MyPointXYZI {}
/// ```
-pub trait PointConvertible:
- From> + Into> + MetaNames + Clone + 'static
-where
- T: FromBytes,
+#[cfg(not(feature = "derive"))]
+pub trait PointConvertible:
+ From> + Into> + Fields + Clone + 'static + Default
{
}
+#[cfg(feature = "derive")]
+pub trait PointConvertible:
+ type_layout::TypeLayout + From> + Into> + Fields + 'static + Default
+{
+}
+
+#[cfg(feature = "derive")]
+enum PointField {
+ Padding(u32),
+ Field {
+ name: String,
+ size: u32,
+ datatype: u8,
+ },
+}
+
+#[cfg(feature = "derive")]
+struct TypeLayoutInfo {
+ fields: Vec,
+}
+
+#[cfg(feature = "derive")]
+impl TryFrom for PointField {
+ type Error = MsgConversionError;
+
+ fn try_from(f: type_layout::Field) -> Result {
+ match f {
+ type_layout::Field::Field { name, ty, size } => {
+ let typename: String = ty.into_owned();
+ let datatype = FieldDatatype::try_from(typename)?;
+ Ok(Self::Field {
+ name: name.into_owned(),
+ size: size as u32,
+ datatype: datatype.into(),
+ })
+ }
+ type_layout::Field::Padding { size } => Ok(Self::Padding(size as u32)),
+ }
+ }
+}
+
+#[cfg(feature = "derive")]
+impl TryFrom for TypeLayoutInfo {
+ type Error = MsgConversionError;
+
+ fn try_from(t: type_layout::TypeLayoutInfo) -> Result {
+ let fields: Vec = t
+ .fields
+ .into_iter()
+ .map(PointField::try_from)
+ .collect::, _>>()?;
+ Ok(Self { fields })
+ }
+}
+
/// Metadata representation for a point.
///
/// This struct is used to store meta data in a fixed size byte buffer along the with the
@@ -220,6 +546,7 @@ where
#[derive(Debug, Clone, Copy)]
pub struct PointMeta {
bytes: [u8; std::mem::size_of::()],
+ endianness: Endianness,
datatype: FieldDatatype,
}
@@ -228,6 +555,7 @@ impl Default for PointMeta {
Self {
bytes: [u8::default(); std::mem::size_of::()],
datatype: FieldDatatype::F32,
+ endianness: Endianness::default(),
}
}
}
@@ -250,22 +578,29 @@ impl PointMeta {
Self {
bytes,
datatype: T::field_datatype(),
+ ..Default::default()
}
}
#[inline(always)]
- fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self {
+ fn from_buffer(
+ data: &[u8],
+ offset: usize,
+ datatype: FieldDatatype,
+ endianness: Endianness,
+ ) -> Self {
debug_assert!(data.len() >= offset + datatype.size());
-
- let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) };
- let mut bytes_array = [0; std::mem::size_of::()]; // 8 bytes as f64 is the largest type
- for (byte, save_byte) in bytes.iter().zip(bytes_array.iter_mut()) {
- *save_byte = *byte;
+ let bytes = [u8::default(); std::mem::size_of::()];
+ unsafe {
+ let data_ptr = data.as_ptr().add(offset);
+ let bytes_ptr = bytes.as_ptr() as *mut u8;
+ std::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size());
}
Self {
- bytes: bytes_array,
- datatype: *datatype,
+ bytes,
+ datatype,
+ endianness,
}
}
@@ -283,7 +618,11 @@ impl PointMeta {
.bytes
.get(0..size)
.expect("Exceeds bounds of f64, which is the largest type");
- T::from_le_bytes(bytes)
+
+ match self.endianness {
+ Endianness::Big => T::from_be_bytes(bytes),
+ Endianness::Little => T::from_le_bytes(bytes),
+ }
}
}
diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs
deleted file mode 100644
index 2439e9d..0000000
--- a/src/pcl_utils.rs
+++ /dev/null
@@ -1,422 +0,0 @@
-use crate::{MetaNames, Point, PointConvertible};
-
-/// Pack an RGB color into a single f32 value as used in ROS with PCL for RViz usage.
-#[inline]
-fn pack_rgb(r: u8, g: u8, b: u8) -> f32 {
- let packed = ((r as u32) << 16) + ((g as u32) << 8) + (b as u32);
- f32::from_bits(packed)
-}
-
-/// Unpack an RGB color from a single f32 value as used in ROS with PCL for RViz usage.
-#[inline]
-fn unpack_rgb(rgb: f32) -> [u8; 3] {
- let packed: u32 = rgb.to_bits();
- let r: u8 = (packed >> 16) as u8;
- let g: u8 = (packed >> 8) as u8;
- let b: u8 = packed as u8;
- [r, g, b]
-}
-
-/// 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 From> for PointXYZ {
- fn from(point: Point) -> Self {
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- }
- }
-}
-
-impl From for Point {
- fn from(point: PointXYZ) -> Self {
- Point {
- coords: [point.x, point.y, point.z],
- meta: [],
- }
- }
-}
-
-impl MetaNames<0> for PointXYZ {
- fn meta_names() -> [&'static str; 0] {
- []
- }
-}
-
-impl PointConvertible() }, 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 From for Point {
- fn from(point: PointXYZI) -> Self {
- Point {
- coords: [point.x, point.y, point.z],
- meta: [point.intensity.into()],
- }
- }
-}
-
-impl From> for PointXYZI {
- fn from(point: Point) -> Self {
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- intensity: point.meta[0].get(),
- }
- }
-}
-
-impl MetaNames<1> for PointXYZI {
- fn meta_names() -> [&'static str; 1] {
- ["intensity"]
- }
-}
-
-impl PointConvertible() }, 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 From> for PointXYZRGB {
- fn from(point: Point) -> Self {
- let rgb = point.meta[0].get::();
- let rgb_unpacked = unpack_rgb(rgb);
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- r: rgb_unpacked[0],
- g: rgb_unpacked[1],
- b: rgb_unpacked[2],
- }
- }
-}
-
-impl From for Point {
- fn from(point: PointXYZRGB) -> Self {
- Point {
- coords: [point.x, point.y, point.z],
- meta: [pack_rgb(point.r, point.g, point.b).into()],
- }
- }
-}
-
-impl MetaNames<1> for PointXYZRGB {
- fn meta_names() -> [&'static str; 1] {
- ["rgb"]
- }
-}
-
-impl PointConvertible() }, 3, 1> for PointXYZRGB {}
-
-/// Predefined point type commonly used in ROS with PCL.
-/// This is a 3D point with x, y, z coordinates and an RGBA color value.
-/// 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 From> for PointXYZRGBA {
- fn from(point: Point) -> Self {
- let rgb = point.meta[0].get::();
- let rgb_unpacked = unpack_rgb(rgb);
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- r: rgb_unpacked[0],
- g: rgb_unpacked[1],
- b: rgb_unpacked[2],
- a: point.meta[1].get(),
- }
- }
-}
-
-impl From for Point {
- fn from(point: PointXYZRGBA) -> Self {
- let rgb = pack_rgb(point.r, point.g, point.b);
- Point {
- coords: [point.x, point.y, point.z],
- meta: [rgb.into(), point.a.into()],
- }
- }
-}
-
-impl MetaNames<2> for PointXYZRGBA {
- fn meta_names() -> [&'static str; 2] {
- ["rgb", "a"]
- }
-}
-
-impl PointConvertible() }, 3, 2> for PointXYZRGBA {}
-
-/// Predefined point type commonly used in ROS with PCL.
-/// This is a 3D point with x, y, z coordinates, an RGB color value and a normal vector.
-#[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 From> for PointXYZRGBNormal {
- fn from(point: Point) -> Self {
- let rgb = point.meta[0].get::();
- let rgb_unpacked = unpack_rgb(rgb);
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- r: rgb_unpacked[0],
- g: rgb_unpacked[1],
- b: rgb_unpacked[2],
- normal_x: point.meta[1].get(),
- normal_y: point.meta[2].get(),
- normal_z: point.meta[3].get(),
- }
- }
-}
-
-impl From for Point {
- fn from(point: PointXYZRGBNormal) -> Self {
- let rgb = pack_rgb(point.r, point.g, point.b);
- Point {
- coords: [point.x, point.y, point.z],
- meta: [
- rgb.into(),
- point.normal_x.into(),
- point.normal_y.into(),
- point.normal_z.into(),
- ],
- }
- }
-}
-
-impl MetaNames<4> for PointXYZRGBNormal {
- fn meta_names() -> [&'static str; 4] {
- ["rgb", "normal_x", "normal_y", "normal_z"]
- }
-}
-
-impl PointConvertible() }, 3, 4> for PointXYZRGBNormal {}
-
-/// Predefined point type commonly used in ROS with PCL.
-/// This is a 3D point with x, y, z coordinates, an intensity value and a normal vector.
-#[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 From for Point {
- fn from(point: PointXYZINormal) -> Self {
- Point {
- coords: [point.x, point.y, point.z],
- meta: [
- point.intensity.into(),
- point.normal_x.into(),
- point.normal_y.into(),
- point.normal_z.into(),
- ],
- }
- }
-}
-
-impl From> for PointXYZINormal {
- fn from(point: Point) -> Self {
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- intensity: point.meta[0].get(),
- normal_x: point.meta[1].get(),
- normal_y: point.meta[2].get(),
- normal_z: point.meta[3].get(),
- }
- }
-}
-
-impl MetaNames<4> for PointXYZINormal {
- fn meta_names() -> [&'static str; 4] {
- ["intensity", "normal_x", "normal_y", "normal_z"]
- }
-}
-
-impl PointConvertible() }, 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 From> for PointXYZL {
- fn from(point: Point) -> Self {
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- label: point.meta[0].get(),
- }
- }
-}
-
-impl From for Point {
- fn from(point: PointXYZL) -> Self {
- Point {
- coords: [point.x, point.y, point.z],
- meta: [point.label.into()],
- }
- }
-}
-
-impl MetaNames<1> for PointXYZL {
- fn meta_names() -> [&'static str; 1] {
- ["label"]
- }
-}
-
-impl PointConvertible() }, 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 From> for PointXYZRGBL {
- fn from(point: Point) -> Self {
- let rgb = point.meta[0].get::();
- let rgb_unpacked = unpack_rgb(rgb);
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- r: rgb_unpacked[0],
- g: rgb_unpacked[1],
- b: rgb_unpacked[2],
- label: point.meta[1].get(),
- }
- }
-}
-
-impl From for Point {
- fn from(point: PointXYZRGBL) -> Self {
- Point {
- coords: [point.x, point.y, point.z],
- meta: [
- pack_rgb(point.r, point.g, point.b).into(),
- point.label.into(),
- ],
- }
- }
-}
-
-impl MetaNames<2> for PointXYZRGBL {
- fn meta_names() -> [&'static str; 2] {
- ["rgb", "label"]
- }
-}
-
-impl PointConvertible() }, 3, 2> for PointXYZRGBL {}
-
-/// Predefined point type commonly used in ROS with PCL.
-/// This is a 3D point with x, y, z coordinates and a normal vector.
-#[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 From> for PointXYZNormal {
- fn from(point: Point) -> Self {
- Self {
- x: point.coords[0],
- y: point.coords[1],
- z: point.coords[2],
- normal_x: point.meta[0].get(),
- normal_y: point.meta[1].get(),
- normal_z: point.meta[2].get(),
- }
- }
-}
-
-impl From for Point {
- fn from(point: PointXYZNormal) -> Self {
- Point {
- coords: [point.x, point.y, point.z],
- meta: [
- point.normal_x.into(),
- point.normal_y.into(),
- point.normal_z.into(),
- ],
- }
- }
-}
-
-impl MetaNames<3> for PointXYZNormal {
- fn meta_names() -> [&'static str; 3] {
- ["normal_x", "normal_y", "normal_z"]
- }
-}
-
-impl PointConvertible() }, 3, 3> for PointXYZNormal {}
diff --git a/src/points.rs b/src/points.rs
new file mode 100644
index 0000000..aa0b7aa
--- /dev/null
+++ b/src/points.rs
@@ -0,0 +1,749 @@
+use crate::{Fields, PointConvertible, RPCL2Point};
+
+#[cfg(feature = "derive")]
+use type_layout::TypeLayout;
+
+/// A packed RGB color encoding as used in ROS tools.
+#[derive(Clone, Copy)]
+#[repr(C)]
+pub union RGB {
+ packed: f32,
+ unpacked: [u8; 4], // 1 byte padding
+}
+
+unsafe impl Send for RGB {}
+unsafe impl Sync for RGB {}
+
+impl Default for RGB {
+ fn default() -> Self {
+ Self { packed: 0.0 }
+ }
+}
+
+impl PartialEq for RGB {
+ fn eq(&self, other: &Self) -> bool {
+ self.r() == other.r() && self.g() == other.g() && self.b() == other.b()
+ }
+}
+
+impl core::fmt::Display for RGB {
+ fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+ write!(f, "#{:02X}{:02X}{:02X}", self.r(), self.g(), self.b())
+ }
+}
+
+impl core::fmt::Debug for RGB {
+ fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
+ f.debug_struct("RGB")
+ .field("r", &self.r())
+ .field("g", &self.g())
+ .field("b", &self.b())
+ .finish()
+ }
+}
+
+impl RGB {
+ pub fn new(r: u8, g: u8, b: u8) -> Self {
+ Self {
+ unpacked: [r, g, b, 0],
+ }
+ }
+
+ pub fn new_from_packed_f32(packed: f32) -> Self {
+ Self { packed }
+ }
+
+ pub fn new_from_packed(packed: u32) -> Self {
+ Self::new_from_packed_f32(f32::from_bits(packed))
+ }
+
+ pub fn raw(&self) -> f32 {
+ unsafe { self.packed }
+ }
+
+ pub fn r(&self) -> u8 {
+ unsafe { self.unpacked[0] }
+ }
+
+ pub fn g(&self) -> u8 {
+ unsafe { self.unpacked[1] }
+ }
+
+ pub fn b(&self) -> u8 {
+ unsafe { self.unpacked[2] }
+ }
+
+ pub fn set_r(&mut self, r: u8) {
+ unsafe { self.unpacked[0] = r }
+ }
+
+ pub fn set_g(&mut self, g: u8) {
+ unsafe { self.unpacked[1] = g }
+ }
+
+ pub fn set_b(&mut self, b: u8) {
+ unsafe { self.unpacked[2] = b }
+ }
+}
+
+impl From for f32 {
+ fn from(rgb: RGB) -> Self {
+ unsafe { rgb.packed }
+ }
+}
+
+impl From for RGB {
+ fn from(packed: f32) -> Self {
+ RGB::new_from_packed_f32(packed)
+ }
+}
+
+/// Predefined point type commonly used in ROS with PCL.
+/// This is a 3D point with x, y, z coordinates.
+#[derive(Clone, Debug, PartialEq, Copy, Default)]
+#[cfg_attr(feature = "derive", derive(TypeLayout))]
+#[repr(C)]
+pub struct PointXYZ {
+ pub x: f32,
+ pub y: f32,
+ pub z: f32,
+}
+
+#[cfg(feature = "nalgebra")]
+impl From> for PointXYZ {
+ fn from(point: nalgebra::Point3) -> Self {
+ Self {
+ x: point.x,
+ y: point.y,
+ z: point.z,
+ }
+ }
+}
+
+#[cfg(feature = "nalgebra")]
+impl From for nalgebra::Point3 {
+ fn from(point: PointXYZ) -> Self {
+ nalgebra::Point3::new(point.x, point.y, point.z)
+ }
+}
+
+impl PointXYZ {
+ pub fn new(x: f32, y: f32, z: f32) -> Self {
+ Self { x, y, z }
+ }
+
+ #[cfg(feature = "nalgebra")]
+ pub fn xyz(&self) -> nalgebra::Point3 
