diff --git a/Cargo.toml b/Cargo.toml
index 3483be9..cfa998a 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -51,4 +51,7 @@ rayon = ["dep:rayon"]
derive = ["dep:rpcl2_derive", "dep:type-layout"]
nalgebra = ["dep:nalgebra"]
-default = ["rayon", "derive", "nalgebra"]
\ No newline at end of file
+default = ["derive"]
+
+[package.metadata.docs.rs]
+rustdoc-args = ["--generate-link-to-definition"]
diff --git a/README.md b/README.md
index 7143c6c..8c4a8d1 100644
--- a/README.md
+++ b/README.md
@@ -1,22 +1,20 @@
## !! 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
- A complete and versatile abstraction of PointCloud2.
+ A PointCloud2 message conversion library.
-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.
+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/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.
-
```rust
use ros_pointcloud2::prelude::*;
@@ -27,7 +25,7 @@ let cloud_points = vec![
];
// Give the Vec or anything that implements `IntoIterator`.
-let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+let in_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 = in_msg.into();
@@ -36,7 +34,7 @@ let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
// let in_msg: PointCloud2Msg = msg.into();
let new_pcl = in_msg.try_into_iter().unwrap()
- .map(|point: PointXYZ| { // Define the type of point here.
+ .map(|point: PointXYZ| { // Define the info you want to have from the Msg.
// Some logic here ...
point
@@ -84,7 +82,15 @@ Also, indicate the following dependencies to your linker inside the `package.xml
builtin_interfaces
```
-Please open an issue or PR if you want to see support for other crates.
+Please open an issue or PR if you need other integrations.
+
+## Performance
+
+The library offers a speed up when compared to PointCloudLibrary (PCL) conversions but the specific factor depends heavily on the use case and system.
+`vec` conversions are on average ~7.5x faster than PCL while the single core iteration `_iter` APIs are around 2x faster.
+Parallelization with `_par_iter` showcases a 10.5x speed up compared to an OpenMP accelerated PCL pipeline.
+
+The full benchmarks are publicly available in this [repository](https://github.com/stelzo/ros_pcl_conv_bench).
## License
diff --git a/benches/roundtrip.rs b/benches/roundtrip.rs
index cf4945b..afe4473 100644
--- a/benches/roundtrip.rs
+++ b/benches/roundtrip.rs
@@ -3,32 +3,174 @@ use ros_pointcloud2::prelude::*;
use rand::Rng;
-pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec {
+pub type PointXYZB = PointXYZINormal;
+
+pub fn distance_to_origin(point: &PointXYZ) -> f32 {
+ ((point.x.powi(2)) + (point.y.powi(2)) + (point.z.powi(2))).sqrt()
+}
+
+pub fn dot_product(point1: &PointXYZ, point2: &PointXYZ) -> f32 {
+ point1.x * point2.x + point1.y * point2.y + point1.z * point2.z
+}
+
+pub fn cross_product(point1: &PointXYZ, point2: &PointXYZ) -> PointXYZ {
+ PointXYZ {
+ x: point1.y * point2.z - point1.z * point2.y,
+ y: point1.z * point2.x - point1.x * point2.z,
+ z: point1.x * point2.y - point1.y * point2.x,
+ }
+}
+
+pub fn scalar_multiply(point: &PointXYZ, scalar: f32) -> PointXYZ {
+ PointXYZ {
+ x: point.x * scalar,
+ y: point.y * scalar,
+ z: point.z * scalar,
+ }
+}
+
+pub fn magnitude_squared(point: &PointXYZ) -> f32 {
+ (point.x.powi(2)) + (point.y.powi(2)) + (point.z.powi(2))
+}
+
+pub fn reflection_through_plane(
+ point: &PointXYZ,
+ normal: &PointXYZ,
+ point_on_plane: &PointXYZ,
+) -> PointXYZ {
+ PointXYZ {
+ x: point.x
+ - 2.0
+ * ((point.x - point_on_plane.x) * normal.x
+ + (point.y - point_on_plane.y) * normal.y
+ + (point.z - point_on_plane.z) * normal.z),
+ y: point.y
+ - 2.0
+ * ((point.x - point_on_plane.x) * normal.x
+ + (point.y - point_on_plane.y) * normal.y
+ + (point.z - point_on_plane.z) * normal.z),
+ z: point.z
+ - 2.0
+ * ((point.x - point_on_plane.x) * normal.x
+ + (point.y - point_on_plane.y) * normal.y
+ + (point.z - point_on_plane.z) * normal.z),
+ }
+}
+
+pub fn rotation_about_x(point: &PointXYZ, angle: f32) -> PointXYZ {
+ let c = f32::cos(angle);
+ let s = f32::sin(angle);
+ PointXYZ {
+ x: point.x,
+ y: point.y * c - point.z * s,
+ z: point.y * s + point.z * c,
+ }
+}
+
+pub fn closest_point_on_line(
+ point: &PointXYZ,
+ line_point: &PointXYZ,
+ line_direction: &PointXYZ,
+) -> PointXYZ {
+ PointXYZ {
+ x: line_point.x
+ + (line_point.x - point.x) * ((line_point.x - point.x).powi(2))
+ / ((line_direction.x * 2.0).powi(2))
+ + (line_direction.y * 2.0) * (point.z - line_point.z)
+ / ((line_direction.z * 2.0).powi(2)),
+ y: line_point.y
+ + (line_point.y - point.y) * ((line_point.y - point.y).powi(2))
+ / ((line_direction.y * 2.0).powi(2))
+ + (line_direction.x * 2.0) * (point.x - line_point.x)
+ / ((line_direction.x * 2.0).powi(2)),
+ z: line_point.z
+ + (line_point.z - point.z) * ((line_point.z - point.z).powi(2))
+ / ((line_direction.z * 2.0).powi(2))
+ + (line_direction.y * 2.0) * (point.y - line_point.y)
+ / ((line_direction.y * 2.0).powi(2)),
+ }
+}
+
+fn minus(point1: &PointXYZ, point2: &PointXYZ) -> PointXYZ {
+ PointXYZ {
+ x: point1.x - point2.x,
+ y: point1.y - point2.y,
+ z: point1.z - point2.z,
+ }
+}
+
+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 {
+ let point = PointXYZB {
x: rng.gen_range(min..max),
y: rng.gen_range(min..max),
z: rng.gen_range(min..max),
+ ..Default::default()
};
pointcloud.push(point);
}
pointcloud
}
+pub fn heavy_computing(point: &PointXYZ, iterations: u32) -> f32 {
+ let mut result = distance_to_origin(point);
+ for _ in 0..iterations {
+ result += dot_product(
+ point,
+ &PointXYZ {
+ x: 1.0,
+ y: 2.0,
+ z: 3.0,
+ },
+ );
+ result += cross_product(
+ point,
+ &PointXYZ {
+ x: 4.0,
+ y: 5.0,
+ z: 6.0,
+ },
+ )
+ .x;
+ result = result + (result * 10.0).sqrt();
+ let reflected_point = reflection_through_plane(
+ point,
+ &PointXYZ {
+ x: 7.0,
+ y: 8.0,
+ z: 9.0,
+ },
+ &PointXYZ {
+ x: 3.0,
+ y: 4.0,
+ z: 5.0,
+ },
+ );
+ let rotated_point = rotation_about_x(
+ &PointXYZ {
+ x: 10.0,
+ y: 11.0,
+ z: 12.0,
+ },
+ std::f32::consts::PI / 2.0,
+ );
+
+ result += magnitude_squared(&minus(&reflected_point, &rotated_point));
+ }
+ result
+}
+
#[cfg(feature = "derive")]
-fn roundtrip_vec(cloud: Vec) -> bool {
+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::>();
+ let total: Vec = internal_msg.try_into_vec().unwrap();
orig_len == total.len()
}
-fn roundtrip(cloud: Vec) -> bool {
+fn roundtrip(cloud: Vec) -> bool {
let orig_len = cloud.len();
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
let total = internal_msg
@@ -39,15 +181,13 @@ fn roundtrip(cloud: Vec) -> bool {
}
#[cfg(feature = "derive")]
-fn roundtrip_filter_vec(cloud: Vec) -> bool {
+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
- })
+ .filter(|point: &PointXYZ| distance_to_origin(point) < 69.9)
.fold(PointXYZ::default(), |acc, point| PointXYZ {
x: acc.x + point.x,
y: acc.y + point.y,
@@ -56,15 +196,13 @@ fn roundtrip_filter_vec(cloud: Vec) -> bool {
orig_len == total.x as usize
}
-fn roundtrip_filter(cloud: Vec) -> bool {
+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
- })
+ .filter(|point: &PointXYZ| distance_to_origin(point) < 69.9)
.fold(PointXYZ::default(), |acc, point| PointXYZ {
x: acc.x + point.x,
y: acc.y + point.y,
@@ -73,8 +211,52 @@ fn roundtrip_filter(cloud: Vec) -> bool {
orig_len == total.x as usize
}
+fn roundtrip_computing(cloud: Vec) -> bool {
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_iter()
+ .unwrap()
+ .map(|point: PointXYZ| heavy_computing(&point, 100))
+ .sum::();
+ total > 0.0
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_computing_par(cloud: Vec) -> bool {
+ let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
+ let total = internal_msg
+ .try_into_par_iter()
+ .unwrap()
+ .map(|point: PointXYZ| heavy_computing(&point, 100))
+ .sum::();
+ total > 0.0
+}
+
#[cfg(feature = "rayon")]
-fn roundtrip_par(cloud: Vec) -> bool {
+fn roundtrip_computing_par_par(cloud: Vec) -> bool {
+ let internal_msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter()).unwrap();
+ let total = internal_msg
+ .try_into_par_iter()
+ .unwrap()
+ .map(|point: PointXYZ| heavy_computing(&point, 100))
+ .sum::();
+ total > 0.0
+}
+
+#[cfg(feature = "derive")]
+fn roundtrip_computing_vec(cloud: Vec) -> bool {
+ let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
+ let total: f32 = internal_msg
+ .try_into_vec()
+ .unwrap()
+ .into_iter()
+ .map(|point: PointXYZ| heavy_computing(&point, 100))
+ .sum();
+ total > 0.0
+}
+
+#[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
@@ -85,15 +267,40 @@ fn roundtrip_par(cloud: Vec) -> bool {
}
#[cfg(feature = "rayon")]
-fn roundtrip_filter_par(cloud: Vec) -> bool {
+fn roundtrip_par_par(cloud: Vec) -> bool {
+ let orig_len = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter()).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
- })
+ .filter(|point: &PointXYZ| distance_to_origin(point) < 69.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
+}
+
+#[cfg(feature = "rayon")]
+fn roundtrip_filter_par_par(cloud: Vec) -> bool {
+ let orig_len: usize = cloud.len();
+ let internal_msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter()).unwrap();
+ let total = internal_msg
+ .try_into_par_iter()
+ .unwrap()
+ .filter(|point: &PointXYZ| distance_to_origin(point) < 69.9)
.reduce(PointXYZ::default, |acc, point| PointXYZ {
x: acc.x + point.x,
y: acc.y + point.y,
@@ -103,61 +310,385 @@ fn roundtrip_filter_par(cloud: Vec) -> bool {
}
fn roundtrip_benchmark(c: &mut Criterion) {
+ let cloud_points_16k = generate_random_pointcloud(16_000, f32::MIN / 2.0, f32::MAX / 2.0);
+ let cloud_points_60k = generate_random_pointcloud(60_000, f32::MIN / 2.0, f32::MAX / 2.0);
+ let cloud_points_120k = generate_random_pointcloud(120_000, f32::MIN / 2.0, f32::MAX / 2.0);
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| {
+ // 16k points (Velodyne with 16 beams)
+
+ // Moving memory
+ c.bench_function("16k iter", |b| {
+ b.iter(|| {
+ black_box(roundtrip(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("16k iter_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("16k iter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par_par(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("16k vec", |b| {
+ b.iter(|| {
+ black_box(roundtrip_vec(cloud_points_16k.clone()));
+ })
+ });
+
+ // Simple distance filter
+ c.bench_function("16k iter_filter", |b| {
+ b.iter(|| {
+ roundtrip_filter(black_box(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("16k filter_par", |b| {
+ b.iter(|| {
+ roundtrip_filter_par(black_box(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("16k filter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_filter_par_par(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("16k vec_filter", |b| {
+ b.iter(|| {
+ roundtrip_filter_vec(black_box(cloud_points_16k.clone()));
+ })
+ });
+
+ // Heavy computing
+ c.bench_function("16k iter_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing(black_box(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("16k iter_compute_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par(black_box(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("16k iter_compute_par_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par_par(black_box(cloud_points_16k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("16k vec_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing_vec(black_box(cloud_points_16k.clone()));
+ })
+ });
+
+ // 60k points (Ouster with 64 beams)
+
+ // Moving memory
+ c.bench_function("60k iter", |b| {
+ b.iter(|| {
+ black_box(roundtrip(cloud_points_60k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("60k iter_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par(cloud_points_60k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("60k iter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par_par(cloud_points_60k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("60k vec", |b| {
+ b.iter(|| {
+ black_box(roundtrip_vec(cloud_points_60k.clone()));
+ })
+ });
+
+ // 120k points (Ouster with 128 beams)
+
+ // Moving memory
+ c.bench_function("120k iter", |b| {
+ b.iter(|| {
+ black_box(roundtrip(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("120k iter_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("120k iter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par_par(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("120k vec", |b| {
+ b.iter(|| {
+ black_box(roundtrip_vec(cloud_points_120k.clone()));
+ })
+ });
+
+ // Simple distance filter
+ c.bench_function("120k iter_filter", |b| {
+ b.iter(|| {
+ roundtrip_filter(black_box(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("120k filter_par", |b| {
+ b.iter(|| {
+ roundtrip_filter_par(black_box(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("120k filter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_filter_par_par(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("120k vec_filter", |b| {
+ b.iter(|| {
+ roundtrip_filter_vec(black_box(cloud_points_120k.clone()));
+ })
+ });
+
+ // Heavy computing
+ c.bench_function("120k iter_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing(black_box(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("120k iter_compute_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par(black_box(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("120k iter_compute_par_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par_par(black_box(cloud_points_120k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("120k vec_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing_vec(black_box(cloud_points_120k.clone()));
+ })
+ });
+
+ // 500k points (just to show how it scales)
+
+ // Moving memory
+ c.bench_function("500k iter", |b| {
b.iter(|| {
black_box(roundtrip(cloud_points_500k.clone()));
})
});
#[cfg(feature = "rayon")]
- c.bench_function("roundtrip_par 500k", |b| {
+ c.bench_function("500k iter_par", |b| {
b.iter(|| {
black_box(roundtrip_par(cloud_points_500k.clone()));
})
});
+ #[cfg(feature = "rayon")]
+ c.bench_function("500k iter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par_par(cloud_points_500k.clone()));
+ })
+ });
+
#[cfg(feature = "derive")]
- c.bench_function("roundtrip_vec 500k", |b| {
+ c.bench_function("500k vec", |b| {
b.iter(|| {
black_box(roundtrip_vec(cloud_points_500k.clone()));
})
});
- c.bench_function("roundtrip_filter 500k", |b| {
+ // Simple distance filter
+ c.bench_function("500k iter_filter", |b| {
b.iter(|| {
roundtrip_filter(black_box(cloud_points_500k.clone()));
})
});
#[cfg(feature = "rayon")]
- c.bench_function("roundtrip_filter_par 500k", |b| {
+ c.bench_function("500k filter_par", |b| {
b.iter(|| {
roundtrip_filter_par(black_box(cloud_points_500k.clone()));
})
});
- c.bench_function("roundtrip_filter 1.5m", |b| {
+ #[cfg(feature = "rayon")]
+ c.bench_function("500k filter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_filter_par_par(cloud_points_500k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("500k vec_filter", |b| {
+ b.iter(|| {
+ roundtrip_filter_vec(black_box(cloud_points_500k.clone()));
+ })
+ });
+
+ // Heavy computing
+ c.bench_function("500k iter_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing(black_box(cloud_points_500k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("500k iter_compute_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par(black_box(cloud_points_500k.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("500k iter_compute_par_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par_par(black_box(cloud_points_500k.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("500k vec_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing_vec(black_box(cloud_points_500k.clone()));
+ })
+ });
+
+ // 1.5m points (scale of small localmaps in SLAM)
+
+ // Moving memory
+ c.bench_function("1.5m iter", |b| {
+ b.iter(|| {
+ black_box(roundtrip(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("1.5m iter_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("1.5m iter_par_par", |b| {
+ b.iter(|| {
+ black_box(roundtrip_par_par(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("1.5m vec", |b| {
+ b.iter(|| {
+ black_box(roundtrip_vec(cloud_points_1_5m.clone()));
+ })
+ });
+
+ // Simple distance filter
+ c.bench_function("1.5m iter_filter", |b| {
b.iter(|| {
roundtrip_filter(black_box(cloud_points_1_5m.clone()));
})
});
#[cfg(feature = "rayon")]
- c.bench_function("roundtrip_filter_par 1.5m", |b| {
+ c.bench_function("1.5m iter_par_filter", |b| {
b.iter(|| {
black_box(roundtrip_filter_par(cloud_points_1_5m.clone()));
})
});
+ #[cfg(feature = "rayon")]
+ c.bench_function("1.5m iter_par_par_filter", |b| {
+ b.iter(|| {
+ black_box(roundtrip_filter_par_par(cloud_points_1_5m.clone()));
+ })
+ });
+
#[cfg(feature = "derive")]
- c.bench_function("roundtrip_filter_vec 1.5m", |b| {
+ c.bench_function("1.5m vec_filter", |b| {
b.iter(|| {
roundtrip_filter_vec(black_box(cloud_points_1_5m.clone()));
})
});
+
+ // Heavy computing
+ c.bench_function("1.5m iter_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing(black_box(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("1.5m iter_compute_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par(black_box(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "rayon")]
+ c.bench_function("1.5m iter_compute_par_par", |b| {
+ b.iter(|| {
+ roundtrip_computing_par_par(black_box(cloud_points_1_5m.clone()));
+ })
+ });
+
+ #[cfg(feature = "derive")]
+ c.bench_function("1.5m vec_compute", |b| {
+ b.iter(|| {
+ roundtrip_computing_vec(black_box(cloud_points_1_5m.clone()));
+ })
+ });
}
criterion_group!(benches, roundtrip_benchmark);
diff --git a/examples/custom_enum_field_filter.rs b/examples/custom_enum_field_filter.rs
index 264422b..c86e50c 100644
--- a/examples/custom_enum_field_filter.rs
+++ b/examples/custom_enum_field_filter.rs
@@ -17,7 +17,8 @@ enum Label {
Car,
}
-// Define a custom point with an enum (not supported by PointCloud2)
+// Define a custom point with an enum.
+// This is normally not supported by PointCloud2 but we will explain the library how to handle it.
#[derive(Debug, PartialEq, Clone, Default)]
struct CustomPoint {
x: f32,
@@ -27,7 +28,7 @@ struct CustomPoint {
my_custom_label: Label,
}
-// For our convenience
+// Some convenience functions to convert between the enum and u8.
impl From for u8 {
fn from(label: Label) -> Self {
match label {
@@ -49,45 +50,53 @@ impl From for Label {
}
}
-// We implement the PointConvertible trait (needed for every custom point)
+impl CustomPoint {
+ fn new(x: f32, y: f32, z: f32, intensity: f32, my_custom_label: Label) -> Self {
+ Self {
+ x,
+ y,
+ z,
+ intensity,
+ my_custom_label,
+ }
+ }
+}
-// A ros_pointcloud2::Point takes as generic arguments:
-// - coordinate type
-// - dimension (xyz = 3)
-// - metadata dimension (intensity + my_custom_label = 2)
+// We implement the PointConvertible trait (needed for every custom point).
+// RPCL2Point is the internal representation. It takes the amount of fields as generic arguments.
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(),
- ],
- }
+ [
+ point.x.into(),
+ point.y.into(),
+ point.z.into(),
+ point.intensity.into(),
+ u8::from(point.my_custom_label).into(),
+ ]
+ .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
- }
+ Self::new(
+ point[0].get(),
+ point[1].get(),
+ point[2].get(),
+ point[3].get(),
+ point[4].get(),
+ )
}
}
+// Define wow we want to name the fields in the message.
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
+ ["x", "y", "z", "intensity", "my_custom_label"]
}
}
-// We implemented everything that is needed so we declare it as a PointConvertible
+// We implemented everything that is needed for PointConvertible so we declare it as a done.
#[cfg(not(feature = "derive"))]
impl PointConvertible<5> for CustomPoint {}
@@ -95,26 +104,28 @@ impl PointConvertible<5> for CustomPoint {}
// 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
+ FieldDatatype::U8 // Declare that we want to use u8 as the datatype for the label.
}
}
// 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.
+// u8 -> Label
impl FromBytes for Label {
- // u8 -> Label
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ // Technically, PointCloud2 supports big and little endian even though it is rarely used.
+ // 'be' stands for big endian and 'le' for little endian.
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
u8::from_be_bytes([bytes[0]]).into()
}
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
u8::from_le_bytes([bytes[0]]).into()
}
+}
- // Label -> u8
- fn bytes(label: &Self) -> Vec {
- u8::bytes(&u8::from(*label))
+// Label -> u8
+impl From for PointDataBuffer {
+ fn from(label: Label) -> Self {
+ [u8::from(label)].into()
}
}
diff --git a/rpcl2_derive/Cargo.toml b/rpcl2_derive/Cargo.toml
index 05371e7..07f2b25 100644
--- a/rpcl2_derive/Cargo.toml
+++ b/rpcl2_derive/Cargo.toml
@@ -7,6 +7,6 @@ edition = "2021"
proc-macro = true
[dependencies]
-syn = "2"
+syn = "1"
quote = "1"
-proc-macro2 = "1"
\ No newline at end of file
+proc-macro2 = "1"
diff --git a/rpcl2_derive/src/lib.rs b/rpcl2_derive/src/lib.rs
index a03e5bc..894bfd3 100644
--- a/rpcl2_derive/src/lib.rs
+++ b/rpcl2_derive/src/lib.rs
@@ -8,53 +8,91 @@ 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.insert("f32", std::mem::size_of::());
+ allowed_datatypes.insert("f64", std::mem::size_of::());
+ allowed_datatypes.insert("i32", std::mem::size_of::());
+ allowed_datatypes.insert("u8", std::mem::size_of::());
+ allowed_datatypes.insert("u16", std::mem::size_of::());
+ allowed_datatypes.insert("u32", std::mem::size_of::());
+ allowed_datatypes.insert("i8", std::mem::size_of::());
+ allowed_datatypes.insert("i16", std::mem::size_of::());
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;
+// Given a field, get the value of the `rpcl2` renaming attribute like
+// #[rpcl2(name = "new_name")]
+fn get_ros_fields_attribute(attrs: &[syn::Attribute]) -> Option {
+ for attr in attrs {
+ if attr.path.is_ident("rpcl2") {
+ let meta = attr.parse_meta().unwrap();
+ if let syn::Meta::List(meta_list) = meta {
+ for nested_meta in meta_list.nested {
+ if let syn::NestedMeta::Meta(meta) = nested_meta {
+ if let syn::Meta::NameValue(meta_name_value) = meta {
+ if meta_name_value.path.is_ident("name") {
+ return Some(meta_name_value.lit);
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ None
+}
+fn struct_field_rename_array(input: &DeriveInput) -> Vec {
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(),
+ syn::Data::Struct(ref data) => match data.fields {
+ syn::Fields::Named(ref fields) => &fields.named,
+ _ => panic!("StructNames can only be derived for structs with named fields"),
+ },
+ _ => panic!("StructNames can only be derived for structs"),
};
- let allowed_datatypes = get_allowed_types();
+ fields
+ .iter()
+ .map(|field| {
+ let field_name = field.ident.as_ref().unwrap();
+ let ros_fields_attr = get_ros_fields_attribute(&field.attrs);
+ match ros_fields_attr {
+ Some(ros_fields) => match ros_fields {
+ syn::Lit::Str(lit_str) => {
+ let val = lit_str.value();
+ if val.is_empty() {
+ panic!("Empty string literals are not allowed for the rpcl2 attribute");
+ }
+ val
+ }
+ _ => {
+ panic!("Only string literals are allowed for the rpcl2 attribute")
+ }
+ },
+ None => String::from(field_name.to_token_stream().to_string()),
+ }
+ })
+ .collect()
+}
- if fields.is_empty() {
- return syn::Error::new_spanned(input, "No fields found").to_compile_error().into();
- }
+/// This macro will implement the `Fields` trait for your struct so you can use your point for the PointCloud2 conversion.
+///
+/// Use the rename attribute if your struct field name should be different to the ROS field name.
+#[proc_macro_derive(Fields, attributes(rpcl2))]
+pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
+ let input = parse_macro_input!(input as DeriveInput);
+ let struct_name = &input.ident;
- 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_names = struct_field_rename_array(&input)
+ .into_iter()
+ .map(|field_name| {
+ quote! { #field_name }
+ });
- 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_names_len = field_names.len();
- let field_impl = quote! {
- impl Fields<#field_len_token> for #name {
- fn field_names_ordered() -> [&'static str; #field_len_token] {
+ let expanded = quote! {
+ impl Fields<#field_names_len> for #struct_name {
+ fn field_names_ordered() -> [&'static str; #field_names_len] {
[
#(#field_names,)*
]
@@ -62,7 +100,8 @@ pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
}
};
- TokenStream::from(field_impl)
+ // Return the generated implementation
+ expanded.into()
}
/// This macro will fully implement the `PointConvertible` trait for your struct so you can use your point for the PointCloud2 conversion.
@@ -70,35 +109,44 @@ pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
/// 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)]
+#[proc_macro_derive(PointConvertible)]
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(),
+ _ => {
+ 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();
+ 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();
+ 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_names = struct_field_rename_array(&input)
+ .into_iter()
+ .map(|field_name| {
+ quote! { #field_name }
+ });
let field_impl = quote! {
impl ros_pointcloud2::Fields<#field_len_token> for #name {
@@ -110,12 +158,16 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
}
};
- 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 field_names_get = fields
+ .iter()
+ .enumerate()
+ .map(|(idx, f)| {
+ let field_name = f.ident.as_ref().unwrap();
+ quote! { #field_name: point[#idx].get() }
+ })
+ .collect::>();
- let from_my_point = quote! {
+ let from_my_point = quote! {
impl From> for #name {
fn from(point: ros_pointcloud2::RPCL2Point<#field_len_token>) -> Self {
Self {
@@ -125,17 +177,18 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
}
};
- let field_names_into = fields.iter().map(|f| {
- let field_name = f.ident.as_ref().unwrap();
- quote! { point.#field_name.into() }
- }).collect::>();
+ 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,)* ]
- }
+ [ #(#field_names_into,)* ].into()
}
}
};
@@ -152,4 +205,4 @@ pub fn ros_point_derive(input: TokenStream) -> TokenStream {
});
TokenStream::from(out)
-}
\ No newline at end of file
+}
diff --git a/src/convert.rs b/src/convert.rs
index fc6edf3..88c8a10 100644
--- a/src/convert.rs
+++ b/src/convert.rs
@@ -1,3 +1,5 @@
+use std::str::FromStr;
+
use crate::*;
/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html).
@@ -13,9 +15,8 @@ pub enum FieldDatatype {
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.
+ /// While RGB is not officially supported by ROS, it is used in the tooling as a packed f32.
+ /// To make it easy to work with and avoid packing code, the [`ros_pointcloud2::points::RGB`] union is supported here and handled like a f32.
RGB,
}
@@ -35,11 +36,11 @@ impl FieldDatatype {
}
}
-impl TryFrom for FieldDatatype {
- type Error = MsgConversionError;
+impl FromStr for FieldDatatype {
+ type Err = MsgConversionError;
- fn try_from(value: String) -> Result {
- match value.to_lowercase().as_str() {
+ fn from_str(s: &str) -> Result {
+ match s {
"f32" => Ok(FieldDatatype::F32),
"f64" => Ok(FieldDatatype::F64),
"i32" => Ok(FieldDatatype::I32),
@@ -49,7 +50,7 @@ impl TryFrom for FieldDatatype {
"i8" => Ok(FieldDatatype::I8),
"i16" => Ok(FieldDatatype::I16),
"rgb" => Ok(FieldDatatype::RGB),
- _ => Err(MsgConversionError::UnsupportedFieldType(value)),
+ _ => Err(MsgConversionError::UnsupportedFieldType(s.into())),
}
}
}
@@ -148,6 +149,14 @@ impl From for u8 {
}
}
+impl TryFrom<&ros_types::PointFieldMsg> for FieldDatatype {
+ type Error = MsgConversionError;
+
+ fn try_from(value: &ros_types::PointFieldMsg) -> Result {
+ Self::try_from(value.datatype)
+ }
+}
+
/// 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.
///
@@ -175,190 +184,219 @@ pub trait Fields {
fn field_names_ordered() -> [&'static str; N];
}
+pub struct PointDataBuffer([u8; 8]);
+
+impl std::ops::Index for PointDataBuffer {
+ type Output = u8;
+
+ fn index(&self, index: usize) -> &Self::Output {
+ &self.0[index]
+ }
+}
+
+impl PointDataBuffer {
+ pub fn new(data: [u8; 8]) -> Self {
+ Self(data)
+ }
+
+ pub fn as_slice(&self) -> &[u8] {
+ &self.0
+ }
+
+ pub fn raw(self) -> [u8; 8] {
+ self.0
+ }
+
+ pub fn from_slice(data: &[u8]) -> Self {
+ let mut buffer = [0; 8];
+ data.iter().enumerate().for_each(|(i, &v)| buffer[i] = v);
+ Self(buffer)
+ }
+}
+
+impl From<&[u8]> for PointDataBuffer {
+ fn from(data: &[u8]) -> Self {
+ Self::from_slice(data)
+ }
+}
+
+impl From<[u8; N]> for PointDataBuffer {
+ fn from(data: [u8; N]) -> Self {
+ Self::from(data.as_slice())
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: i8) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: i16) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: u16) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: i32) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: u32) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: f32) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: f64) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: u8) -> Self {
+ x.to_le_bytes().into()
+ }
+}
+
+impl From for PointDataBuffer {
+ fn from(x: points::RGB) -> Self {
+ x.raw().to_le_bytes().into()
+ }
+}
+
/// This trait is used to convert a byte slice to a primitive type.
/// All PointField types are supported.
-pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype {
- fn from_be_bytes(bytes: &[u8]) -> Self;
- fn from_le_bytes(bytes: &[u8]) -> Self;
-
- fn bytes(_: &Self) -> Vec;
+pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype + Into {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self;
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self;
}
impl FromBytes for i8 {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([bytes[0]])
}
- #[inline]
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0]])
}
-
- #[inline]
- fn bytes(x: &i8) -> Vec {
- Vec::from(x.to_le_bytes())
- }
}
impl FromBytes for i16 {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
- #[inline]
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
-
- #[inline]
- fn bytes(x: &i16) -> Vec {
- Vec::from(x.to_le_bytes())
- }
}
impl FromBytes for u16 {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([bytes[0], bytes[1]])
}
- #[inline]
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0], bytes[1]])
}
-
- #[inline]
- fn bytes(x: &u16) -> Vec {
- Vec::from(x.to_le_bytes())
- }
}
impl FromBytes for u32 {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
- #[inline]
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
-
- #[inline]
- fn bytes(x: &u32) -> Vec {
- Vec::from(x.to_le_bytes())
- }
}
impl FromBytes for f32 {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
- #[inline]
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
-
- #[inline]
- fn bytes(x: &f32) -> Vec {
- Vec::from(x.to_le_bytes())
- }
}
impl FromBytes for points::RGB {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> 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 {
+ fn from_le_bytes(bytes: PointDataBuffer) -> 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 {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]])
}
-
- fn bytes(x: &i32) -> Vec {
- Vec::from(x.to_le_bytes())
- }
}
impl FromBytes for f64 {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
- #[inline]
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7],
])
}
-
- #[inline]
- fn bytes(x: &f64) -> Vec {
- Vec::from(x.to_le_bytes())
- }
}
impl FromBytes for u8 {
- #[inline]
- fn from_be_bytes(bytes: &[u8]) -> Self {
+ fn from_be_bytes(bytes: PointDataBuffer) -> Self {
Self::from_be_bytes([bytes[0]])
}
- #[inline]
- fn from_le_bytes(bytes: &[u8]) -> Self {
+ fn from_le_bytes(bytes: PointDataBuffer) -> Self {
Self::from_le_bytes([bytes[0]])
}
+}
- #[inline]
- fn bytes(x: &u8) -> Vec {
- Vec::from(x.to_le_bytes())
- }
+pub enum ByteSimilarity {
+ Equal,
+ Overlapping,
+ Different,
}
#[derive(Default, Clone, Debug, PartialEq, Copy)]
-pub enum Endianness {
+pub enum Endian {
Big,
-
#[default]
Little,
}
-#[cfg(test)]
-mod tests {
- use super::*;
-
- #[test]
- fn from_bytes() {
- i8::bytes(&1);
- u8::bytes(&1);
- i16::bytes(&1);
- u16::bytes(&1);
- i32::bytes(&1);
- u32::bytes(&1);
- f32::bytes(&1.0);
- f64::bytes(&1.0);
- }
+#[derive(Default, Clone, Debug, PartialEq, Copy)]
+pub enum Denseness {
+ #[default]
+ Dense,
+ Sparse,
}
diff --git a/src/iterator.rs b/src/iterator.rs
index 99135de..e8f0bdc 100644
--- a/src/iterator.rs
+++ b/src/iterator.rs
@@ -1,6 +1,6 @@
use crate::{
- convert::{Endianness, FieldDatatype},
- Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointMeta, RPCL2Point,
+ convert::{Endian, FieldDatatype},
+ Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, RPCL2Point,
};
/// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg.
@@ -162,7 +162,7 @@ struct ByteBufferView {
point_step_size: usize,
offsets: [usize; N],
meta: Vec<(String, FieldDatatype)>,
- endianness: Endianness,
+ endian: Endian,
}
impl ByteBufferView {
@@ -173,7 +173,7 @@ impl ByteBufferView {
end_point_idx: usize,
offsets: [usize; N],
meta: Vec<(String, FieldDatatype)>,
- endianness: Endianness,
+ endian: Endian,
) -> Self {
Self {
data: std::sync::Arc::<[u8]>::from(data),
@@ -182,7 +182,7 @@ impl ByteBufferView {
point_step_size,
offsets,
meta,
- endianness,
+ endian,
}
}
@@ -196,16 +196,16 @@ impl ByteBufferView {
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];
+ let mut meta = [PointData::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(
+ *p_meta = PointData::from_buffer(
&self.data,
offset + in_point_offset,
*meta_type,
- self.endianness,
+ self.endian,
);
});
@@ -221,7 +221,7 @@ impl ByteBufferView {
point_step_size: self.point_step_size,
offsets: self.offsets,
meta: self.meta.clone(),
- endianness: self.endianness,
+ endian: self.endian,
}
}
@@ -268,7 +268,7 @@ where
.into_iter()
.map(|(name, _)| (*name).to_owned())
.collect::>();
- return Err(MsgConversionError::FieldNotFound(names_not_found));
+ return Err(MsgConversionError::FieldsNotFound(names_not_found));
}
let ordered_fieldnames = C::field_names_ordered();
@@ -303,14 +303,8 @@ where
*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;
+ let cloud_length = cloud.dimensions.width as usize * cloud.dimensions.height as usize;
if point_step_size * cloud_length != cloud.data.len() {
return Err(MsgConversionError::DataLengthMismatch);
}
@@ -323,7 +317,7 @@ where
return Err(MsgConversionError::DataLengthMismatch);
}
- let cloud_length = cloud.width as usize * cloud.height as usize;
+ let cloud_length = cloud.dimensions.width as usize * cloud.dimensions.height as usize;
let data = ByteBufferView::new(
cloud.data,
@@ -332,7 +326,7 @@ where
cloud_length - 1,
offsets,
meta,
- endian,
+ cloud.endian,
);
Ok(Self {
diff --git a/src/lib.rs b/src/lib.rs
index 9956fc4..99142a2 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,39 +1,39 @@
-//! A library to work with the PointCloud2 message type from ROS.
+//! A PointCloud2 message conversion library.
//!
-//! ros_pointcloud2 mainly provides a [`ros_pointcloud2::PointCloud2Msg`] type that implements
-//! functions for conversion to and from iterators.
+//! The library provides the [`ros_pointcloud2::PointCloud2Msg`] type, which implements conversions to and from `Vec` and (parallel) iterators.
//!
+//! - [`ros_pointcloud2::PointCloud2Msg::try_from_vec`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_into_vec`]
//! - [`ros_pointcloud2::PointCloud2Msg::try_from_iter`]
//! - [`ros_pointcloud2::PointCloud2Msg::try_into_iter`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_into_par_iter`]
+//! - [`ros_pointcloud2::PointCloud2Msg::try_from_par_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 mostly does ownership transfers without copying the point data.
+//! The best choice depends on your use case and the point cloud size.
+//! A good rule of thumb for minimum latency is to use `_vec` per default.
//!
-//! 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.
+//! The `_iter` APIs bring more predictable performance and avoid memory allocation but are slower in general (see Performance section in the repository).
+//! If you do any processing on larger point clouds or heavy processing on any sized cloud, switch to `_par_iter`.
+//!
+//! For ROS interoperability, there are implementations for the `r2r`, `rclrs` (ros2_rust) and `rosrust` message types
+//! available with 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 [`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.
+//! Common point types like [`ros_pointcloud2::points::PointXYZ`] or
+//! [`ros_pointcloud2::points::PointXYZI`] are provided. You can easily add any additional custom type.
+//! See `examples/custom_enum_field_filter.rs` for an example.
//!
//! # Example PointXYZ
//! ```
//! use ros_pointcloud2::prelude::*;
//!
-//! // PointXYZ is predefined
//! let cloud_points = vec![
//! PointXYZ::new(9.0006, 42.0, -6.2),
//! PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
//! ];
+//! let cloud_copy = cloud_points.clone(); // For equality test later.
//!
-//! // For equality test later
-//! let cloud_copy = cloud_points.clone();
-//!
-//! // Give the Vec or anything that implements `IntoIterator`.
-//! let in_msg = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+//! let in_msg = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
//!
//! // Convert to your ROS crate message type, we will use r2r here.
//! // let msg: r2r::sensor_msgs::msg::PointCloud2 = in_msg.into();
@@ -42,15 +42,20 @@
//! // let in_msg: PointCloud2Msg = msg.into();
//!
//! let new_pcl = in_msg.try_into_iter().unwrap()
-//! .map(|point: PointXYZ| { // Define the data you want from the point
-//! // Some logic here
+//! .map(|point: PointXYZ| { // Define the data you want from the point.
+//! // Some logic here.
//!
//! point
//! })
-//! .collect::>(); // iterating points here O(n)
-//!
+//! .collect::>();
//! assert_eq!(new_pcl, cloud_copy);
//! ```
+//!
+//! # Features
+//! In addition to the ROS intregrations, the following features are available:
+//! - 'derive' (default): Helpful derive macros for custom point types. Also, derive enables direct copy with `_vec` endpoints.
+//! - 'rayon': Parallel iterator support for `_par_iter` functions. `try_from_par_iter` additionally needs the 'derive' feature to be enabled.
+//! - 'nalgebra': Predefined points offer `xyz()` getter functions for `nalgebra::Point3` types.
pub mod convert;
pub mod points;
@@ -59,20 +64,29 @@ pub mod ros_types;
pub mod iterator;
+use std::num::TryFromIntError;
+use std::str::FromStr;
+
use crate::convert::{FieldDatatype, FromBytes};
use crate::ros_types::{HeaderMsg, PointFieldMsg};
-use convert::Endianness;
pub use convert::Fields;
+use convert::{ByteSimilarity, Denseness, Endian};
-/// All errors that can occur while converting to or from the PointCloud2 message.
+/// All errors that can occur while converting to or from the message type.
+#[derive(Debug)]
pub enum MsgConversionError {
InvalidFieldFormat,
- NotEnoughFields,
- TooManyDimensions,
UnsupportedFieldType(String),
- NoPoints,
DataLengthMismatch,
- FieldNotFound(Vec),
+ FieldsNotFound(Vec),
+ UnsupportedFieldCount,
+ NumberConversion,
+}
+
+impl From for MsgConversionError {
+ fn from(_: TryFromIntError) -> Self {
+ MsgConversionError::NumberConversion
+ }
}
impl std::fmt::Display for MsgConversionError {
@@ -81,15 +95,6 @@ impl std::fmt::Display for MsgConversionError {
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,
@@ -97,202 +102,325 @@ impl std::fmt::Display for MsgConversionError {
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.")
+ write!(f, "The length of the byte buffer in the message does not match the expected length computed from the fields, indicating a corrupted or malformed message.")
+ }
+ MsgConversionError::FieldsNotFound(fields) => {
+ write!(f, "Some fields are not found in the message: {:?}", fields)
+ }
+ MsgConversionError::UnsupportedFieldCount => {
+ write!(
+ f,
+ "Only field_count 1 is supported for reading and writing."
+ )
}
- MsgConversionError::FieldNotFound(fields) => {
- write!(f, "There are fields missing in the message: {:?}", fields)
+ MsgConversionError::NumberConversion => {
+ write!(f, "The number is too large to be converted into a PointCloud2 supported datatype.")
}
}
}
}
-impl std::fmt::Debug for MsgConversionError {
- fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
- ::fmt(self, f)
+impl std::error::Error for MsgConversionError {
+ fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
+ None
}
}
-impl std::error::Error for MsgConversionError {}
-
#[cfg(feature = "derive")]
-fn system_endian() -> Endianness {
+fn system_endian() -> Endian {
if cfg!(target_endian = "big") {
- Endianness::Big
+ Endian::Big
} else if cfg!(target_endian = "little") {
- Endianness::Little
+ Endian::Little
} else {
- panic!("Unsupported endianness");
+ panic!("Unsupported Endian");
}
}
+/// A PointCloud2 message type.
+///
+/// This representation is a small abstraction of the ROS message description, since every ROS library generates its own messages.
+/// To assert consistency, the type should be build with the [`ros_pointcloud2::PointCloud2MsgBuilder`].
#[derive(Clone, Debug)]
pub struct PointCloud2Msg {
pub header: HeaderMsg,
- pub height: u32,
- pub width: u32,
+ pub dimensions: CloudDimensions,
pub fields: Vec,
- pub is_bigendian: bool,
+ pub endian: Endian,
pub point_step: u32,
pub row_step: u32,
pub data: Vec,
- pub is_dense: bool,
+ pub dense: Denseness,
}
-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
+#[derive(Clone, Debug)]
+pub struct CloudDimensionsBuilder(usize);
+
+impl CloudDimensionsBuilder {
+ pub fn new_with_width(width: usize) -> Self {
+ Self(width)
+ }
+
+ pub fn build(self) -> Result {
+ let width = match u32::try_from(self.0) {
+ Ok(w) => w,
+ Err(_) => return Err(MsgConversionError::NumberConversion),
+ };
+
+ Ok(CloudDimensions {
+ width,
+ height: if self.0 > 0 { 1 } else { 0 },
+ })
+ }
+}
+
+/// Creating a PointCloud2Msg with a builder pattern to avoid invalid states.
+#[derive(Clone, Debug, Default)]
+pub struct PointCloud2MsgBuilder {
+ header: HeaderMsg,
+ width: u32,
+ fields: Vec,
+ is_big_endian: bool,
+ point_step: u32,
+ row_step: u32,
+ data: Vec,
+ is_dense: bool,
+}
+
+impl PointCloud2MsgBuilder {
+ pub fn new() -> Self {
+ Self::default()
+ }
+
+ pub fn header(mut self, header: HeaderMsg) -> Self {
+ self.header = header;
+ self
+ }
+
+ pub fn width(mut self, width: u32) -> Self {
+ self.width = width;
+ self
+ }
+
+ pub fn fields(mut self, fields: Vec) -> Self {
+ self.fields = fields;
+ self
+ }
+
+ pub fn endian(mut self, is_big_endian: bool) -> Self {
+ self.is_big_endian = is_big_endian;
+ self
+ }
+
+ pub fn point_step(mut self, point_step: u32) -> Self {
+ self.point_step = point_step;
+ self
+ }
+
+ pub fn row_step(mut self, row_step: u32) -> Self {
+ self.row_step = row_step;
+ self
+ }
+
+ pub fn data(mut self, data: Vec) -> Self {
+ self.data = data;
+ self
+ }
+
+ pub fn dense(mut self, is_dense: bool) -> Self {
+ self.is_dense = is_dense;
+ self
+ }
+
+ pub fn build(self) -> Result {
+ if self.fields.is_empty() {
+ return Err(MsgConversionError::FieldsNotFound(vec![]));
+ }
+
+ if self.fields.iter().any(|f| f.count != 1) {
+ return Err(MsgConversionError::UnsupportedFieldCount);
+ }
+
+ let fields_size = self
+ .fields
+ .iter()
+ .map(FieldDatatype::try_from)
+ .collect::, _>>()?
+ .iter()
+ .map(|f| f.size() as u32)
+ .sum::<_>();
+
+ if self.point_step < fields_size {
+ return Err(MsgConversionError::InvalidFieldFormat);
+ }
+
+ if self.data.len() as u32 % self.point_step != 0 {
+ return Err(MsgConversionError::DataLengthMismatch);
}
+
+ Ok(PointCloud2Msg {
+ header: self.header,
+ dimensions: CloudDimensionsBuilder::new_with_width(self.width as usize).build()?,
+ fields: self.fields,
+ endian: if self.is_big_endian {
+ Endian::Big
+ } else {
+ Endian::Little
+ },
+ point_step: self.point_step,
+ row_step: self.row_step,
+ data: self.data,
+ dense: if self.is_dense {
+ Denseness::Dense
+ } else {
+ Denseness::Sparse
+ },
+ })
}
}
+#[derive(Clone, Debug, Default)]
+pub struct CloudDimensions {
+ pub width: u32,
+ pub height: u32,
+}
+
impl PointCloud2Msg {
#[cfg(feature = "derive")]
- fn prepare_direct_copy() -> Result
+ fn prepare_direct_copy() -> 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 field_names = C::field_names_ordered();
+ debug_assert!(field_names.len() == N);
- let mut offset: u32 = 0;
let layout = TypeLayoutInfo::try_from(C::type_layout())?;
+ debug_assert!(field_names.len() == layout.fields.len());
+
+ let mut offset = 0;
let mut fields: Vec = Vec::with_capacity(layout.fields.len());
for f in layout.fields.into_iter() {
+ let f_translated = field_names[fields.len()].to_string();
match f {
PointField::Field {
- name,
datatype,
size,
+ count,
} => {
fields.push(PointFieldMsg {
- name,
+ name: f_translated,
offset,
datatype,
..Default::default()
});
- offset += size; // assume field_count 1
+ offset += size * count;
}
PointField::Padding(size) => {
- offset += size; // assume field_count 1
+ offset += size;
}
}
}
- Ok(PointCloud2Msg {
- point_step: offset,
- fields,
- ..Default::default()
- })
+ Ok(PointCloud2MsgBuilder::new()
+ .fields(fields)
+ .point_step(offset))
}
#[cfg(feature = "derive")]
- fn assert_byte_similarity(&self) -> Result
+ 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 field_names = C::field_names_ordered();
+ debug_assert!(field_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()) {
+ debug_assert!(field_names.len() == layout.fields.len());
+
+ let mut offset: u32 = 0;
+ let mut field_counter = 0;
+ for (f, msg_f) in layout.fields.iter().zip(self.fields.iter()) {
match f {
PointField::Field {
- name,
datatype,
size,
+ count,
} => {
- if msg_f.name != name {
- return Err(MsgConversionError::FieldNotFound(vec![name.clone()]));
- }
-
- if msg_f.datatype != datatype {
- return Err(MsgConversionError::InvalidFieldFormat);
+ let f_translated = field_names[field_counter].to_string();
+ field_counter += 1;
+
+ if msg_f.name != f_translated
+ || msg_f.offset != offset
+ || msg_f.datatype != *datatype
+ || msg_f.count != 1
+ {
+ return Ok(ByteSimilarity::Different);
}
- if msg_f.offset != offset {
- return Err(MsgConversionError::DataLengthMismatch);
- }
-
- if msg_f.count != 1 {
- return Err(MsgConversionError::DataLengthMismatch);
- }
-
- offset += size; // assume field_count 1
+ offset += size * count;
}
PointField::Padding(size) => {
- offset += size; // assume field_count 1
+ offset += size;
}
}
}
- Ok(true)
+ Ok(if offset == self.point_step {
+ ByteSimilarity::Equal
+ } else {
+ ByteSimilarity::Overlapping
+ })
}
#[inline(always)]
- fn prepare() -> Result
+ 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 field_names = C::field_names_ordered();
+ debug_assert!(field_names.len() == N);
- let mut meta_offsets_acc = 0;
+ let mut meta_offsets_acc: u32 = 0;
let mut fields = vec![PointFieldMsg::default(); N];
- for ((meta_value, meta_name), field_val) in point
+ let field_count: u32 = 1;
+ for ((meta_value, field_name), field_val) in point
.fields
.into_iter()
- .zip(meta_names.into_iter())
+ .zip(field_names.into_iter())
.zip(fields.iter_mut())
{
let datatype_code = meta_value.datatype.into();
- FieldDatatype::try_from(datatype_code)?;
-
- let field_count = 1;
+ let _ = FieldDatatype::try_from(datatype_code)?;
*field_val = PointFieldMsg {
- name: meta_name.into(),
+ name: field_name.into(),
offset: meta_offsets_acc,
datatype: datatype_code,
count: 1,
};
- meta_offsets_acc += field_count * meta_value.datatype.size() as u32
+ meta_offsets_acc += field_count * meta_value.datatype.size() as u32;
}
- Ok(PointCloud2Msg {
- point_step: meta_offsets_acc,
- fields,
- ..Default::default()
- })
+ Ok(PointCloud2MsgBuilder::new()
+ .fields(fields)
+ .point_step(meta_offsets_acc))
}
/// 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::*;
@@ -311,9 +439,11 @@ impl PointCloud2Msg {
C: PointConvertible,
{
let mut cloud = Self::prepare::()?;
+ let mut cloud_width = 0;
+ let cloud_point_step = cloud.point_step;
- iterable.into_iter().for_each(|coords| {
- let point: RPCL2Point = coords.into();
+ iterable.into_iter().for_each(|pointdata| {
+ let point: RPCL2Point = pointdata.into();
point.fields.iter().for_each(|meta| {
let truncated_bytes = unsafe {
@@ -322,25 +452,51 @@ impl PointCloud2Msg {
cloud.data.extend_from_slice(truncated_bytes);
});
- cloud.width += 1;
+ cloud_width += 1;
});
- cloud.row_step = cloud.width * cloud.point_step;
+ cloud = cloud.width(cloud_width);
+ cloud = cloud.row_step(cloud_width * cloud_point_step);
+
+ cloud.build()
+ }
- Ok(cloud)
+ /// Create a PointCloud2Msg from a parallel iterator. Requires the `rayon` and `derive` feature to be enabled.
+ #[cfg(all(feature = "rayon", feature = "derive"))]
+ pub fn try_from_par_iter(
+ iterable: impl rayon::iter::ParallelIterator- ,
+ ) -> Result
+ where
+ C: PointConvertible + Send + Sync,
+ {
+ Self::try_from_vec(iterable.collect::>())
}
+ /// Create a PointCloud2Msg from a Vec of points.
+ /// Since the point type is known at compile time, the conversion is done by direct copy.
+ ///
+ /// # 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_vec(cloud_points).unwrap();
+ /// ```
#[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 => {
+ match (system_endian(), Endian::default()) {
+ (Endian::Big, Endian::Big) | (Endian::Little, Endian::Little) => {
let mut cloud = Self::prepare_direct_copy::()?;
+ let point_step = cloud.point_step;
- let bytes_total = vec.len() * cloud.point_step as usize;
+ let bytes_total = vec.len() * point_step as usize;
cloud.data.resize(bytes_total, u8::default());
let raw_data: *mut C = cloud.data.as_ptr() as *mut C;
unsafe {
@@ -351,38 +507,84 @@ impl PointCloud2Msg {
);
}
- cloud.width = vec.len() as u32;
- cloud.row_step = cloud.width * cloud.point_step;
-
- Ok(cloud)
+ Ok(cloud
+ .width(vec.len() as u32)
+ .row_step(vec.len() as u32 * point_step)
+ .build()?)
}
+ _ => Self::try_from_iter(vec),
}
}
+ /// Convert the PointCloud2Msg to a Vec of points.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZI::new(1.0, 2.0, 3.0, 0.5),
+ /// PointXYZI::new(4.0, 5.0, 6.0, 1.1),
+ /// ];
+ ///
+ /// let msg_out = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
+ /// let cloud_points_out: Vec = msg_out.try_into_vec().unwrap();
+ /// assert_eq!(1.0, cloud_points_out.get(0).unwrap().x);
+ /// ```
#[cfg(feature = "derive")]
pub fn try_into_vec(self) -> Result, MsgConversionError>
where
C: PointConvertible,
{
- self.assert_byte_similarity::()?;
+ match (system_endian(), self.endian) {
+ (Endian::Big, Endian::Big) | (Endian::Little, Endian::Little) => {
+ let bytematch = match self.assert_byte_similarity::()? {
+ ByteSimilarity::Equal => true,
+ ByteSimilarity::Overlapping => false,
+ ByteSimilarity::Different => return Ok(self.try_into_iter()?.collect()),
+ };
- 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);
+ let cloud_width = self.dimensions.width as usize;
+ let point_step = self.point_step as usize;
+ let mut vec = Vec::with_capacity(cloud_width);
+ if bytematch {
+ unsafe {
+ std::ptr::copy_nonoverlapping(
+ self.data.as_ptr(),
+ vec.as_mut_ptr() as *mut u8,
+ self.data.len(),
+ );
+ }
+ } else {
+ unsafe {
+ for i in 0..cloud_width {
+ let point_ptr = self.data.as_ptr().add(i * point_step) as *const C;
+ let point = point_ptr.read();
+ vec.push(point);
+ }
}
}
Ok(vec)
}
+ _ => Ok(self.try_into_iter()?.collect()), // Endianess does not match, read point by point since Endian is read at conversion time.
}
}
+ /// Convert the PointCloud2Msg to an iterator.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZI::new(1.0, 2.0, 3.0, 0.5),
+ /// PointXYZI::new(4.0, 5.0, 6.0, 1.1),
+ /// ];
+ ///
+ /// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+ /// let cloud_points_out = msg_out.try_into_iter().unwrap().collect::>();
+ /// ```
pub fn try_into_iter(
self,
) -> Result, MsgConversionError>
@@ -392,6 +594,21 @@ impl PointCloud2Msg {
iterator::PointCloudIterator::try_from(self)
}
+ /// Convert the PointCloud2Msg to a parallel iterator. Requires the `rayon` feature to be enabled.
+ ///
+ /// # Example
+ /// ```
+ /// use ros_pointcloud2::prelude::*;
+ ///
+ /// let cloud_points: Vec = vec![
+ /// PointXYZI::new(1.0, 2.0, 3.0, 0.5),
+ /// PointXYZI::new(4.0, 5.0, 6.0, 1.1),
+ /// ];
+ ///
+ /// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
+ /// let cloud_points_out = msg_out.try_into_par_iter().unwrap().collect::>();
+ /// assert_eq!(2, cloud_points_out.len());
+ /// ```
#[cfg(feature = "rayon")]
pub fn try_into_par_iter(
self,
@@ -409,26 +626,32 @@ impl PointCloud2Msg {
///
/// See the [`ros_pointcloud2::PointConvertible`] trait for more information.
pub struct RPCL2Point {
- pub fields: [PointMeta; N],
+ fields: [PointData; N],
}
impl Default for RPCL2Point {
fn default() -> Self {
Self {
- fields: [PointMeta::default(); N],
+ fields: [PointData::default(); N],
}
}
}
-impl From<[PointMeta; N]> for RPCL2Point {
- fn from(fields: [PointMeta; N]) -> Self {
+impl std::ops::Index for RPCL2Point {
+ type Output = PointData;
+
+ fn index(&self, index: usize) -> &Self::Output {
+ &self.fields[index]
+ }
+}
+
+impl From<[PointData; N]> for RPCL2Point {
+ fn from(fields: [PointData; 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.
+/// Trait to enable point conversions on the fly.
///
/// # Example
/// ```
@@ -444,19 +667,17 @@ impl From<[PointMeta; N]> for RPCL2Point {
///
/// impl From for RPCL2Point<4> {
/// fn from(point: MyPointXYZI) -> Self {
-/// RPCL2Point {
-/// fields: [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()],
-/// }
+/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
/// }
/// }
///
/// impl From> for MyPointXYZI {
/// fn from(point: RPCL2Point<4>) -> Self {
/// Self {
-/// x: point.fields[0].get(),
-/// y: point.fields[1].get(),
-/// z: point.fields[2].get(),
-/// intensity: point.fields[3].get(),
+/// x: point[0].get(),
+/// y: point[1].get(),
+/// z: point[2].get(),
+/// intensity: point[3].get(),
/// }
/// }
/// }
@@ -475,6 +696,61 @@ pub trait PointConvertible:
{
}
+/// 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 for RPCL2Point<4> {
+/// fn from(point: MyPointXYZI) -> Self {
+/// [point.x.into(), point.y.into(), point.z.into(), point.intensity.into()].into()
+/// }
+/// }
+///
+/// impl From> 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(feature = "derive")]
pub trait PointConvertible:
type_layout::TypeLayout + From> + Into> + Fields + 'static + Default
@@ -484,11 +760,7 @@ pub trait PointConvertible:
#[cfg(feature = "derive")]
enum PointField {
Padding(u32),
- Field {
- name: String,
- size: u32,
- datatype: u8,
- },
+ Field { size: u32, datatype: u8, count: u32 },
}
#[cfg(feature = "derive")]
@@ -502,16 +774,16 @@ impl TryFrom for PointField {
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)?;
+ type_layout::Field::Field { name: _, ty, size } => {
+ let typename: String = ty.into_owned().to_lowercase();
+ let datatype = FieldDatatype::from_str(typename.as_str())?;
Ok(Self::Field {
- name: name.into_owned(),
- size: size as u32,
+ size: size.try_into()?,
datatype: datatype.into(),
+ count: 1,
})
}
- type_layout::Field::Padding { size } => Ok(Self::Padding(size as u32)),
+ type_layout::Field::Padding { size } => Ok(Self::Padding(size.try_into()?)),
}
}
}
@@ -530,65 +802,54 @@ impl TryFrom for TypeLayoutInfo {
}
}
-/// Metadata representation for a point.
+/// Single data representation for a point.
///
-/// This struct is used to store meta data in a fixed size byte buffer along the with the
+/// This struct is used to store data fields in a fixed size byte buffer along the with the
/// datatype that is encoded so that it can be decoded later.
///
/// # Example
/// ```
-/// use ros_pointcloud2::PointMeta;
+/// use ros_pointcloud2::PointData;
///
/// let original_data: f64 = 1.0;
-/// let meta = PointMeta::new(original_data);
+/// let meta = PointData::new(original_data);
/// let my_data: f64 = meta.get();
/// ```
#[derive(Debug, Clone, Copy)]
-pub struct PointMeta {
+pub struct PointData {
bytes: [u8; std::mem::size_of::()],
- endianness: Endianness,
+ endian: Endian,
datatype: FieldDatatype,
}
-impl Default for PointMeta {
+impl Default for PointData {
fn default() -> Self {
Self {
bytes: [u8::default(); std::mem::size_of::()],
datatype: FieldDatatype::F32,
- endianness: Endianness::default(),
+ endian: Endian::default(),
}
}
}
-impl PointMeta {
- /// Create a new PointMeta from a value.
+impl PointData {
+ /// Create a new PointData from a value.
///
/// # Example
/// ```
- /// let meta = ros_pointcloud2::PointMeta::new(1.0);
+ /// let meta = ros_pointcloud2::PointData::new(1.0);
/// ```
#[inline(always)]
pub fn new(value: T) -> Self {
- let raw_bytes = T::bytes(&value);
- let mut bytes = [0; std::mem::size_of::()];
- for (byte, save_byte) in raw_bytes.into_iter().zip(bytes.iter_mut()) {
- *save_byte = byte;
- }
-
Self {
- bytes,
+ bytes: value.into().raw(),
datatype: T::field_datatype(),
..Default::default()
}
}
#[inline(always)]
- fn from_buffer(
- data: &[u8],
- offset: usize,
- datatype: FieldDatatype,
- endianness: Endianness,
- ) -> Self {
+ fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self {
debug_assert!(data.len() >= offset + datatype.size());
let bytes = [u8::default(); std::mem::size_of::()];
unsafe {
@@ -600,76 +861,92 @@ impl PointMeta {
Self {
bytes,
datatype,
- endianness,
+ endian,
}
}
- /// Get the numeric value from the PointMeta description.
+ /// Get the numeric value from the PointData description.
///
/// # Example
/// ```
/// let original_data: f64 = 1.0;
- /// let meta = ros_pointcloud2::PointMeta::new(original_data);
+ /// let meta = ros_pointcloud2::PointData::new(original_data);
/// let my_data: f64 = meta.get();
/// ```
pub fn get(&self) -> T {
- let size = T::field_datatype().size();
- let bytes = self
- .bytes
- .get(0..size)
- .expect("Exceeds bounds of f64, which is the largest type");
-
- match self.endianness {
- Endianness::Big => T::from_be_bytes(bytes),
- Endianness::Little => T::from_le_bytes(bytes),
+ match self.endian {
+ Endian::Big => T::from_be_bytes(convert::PointDataBuffer::new(self.bytes)),
+ Endian::Little => T::from_le_bytes(convert::PointDataBuffer::new(self.bytes)),
}
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: f32) -> Self {
Self::new(value)
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: f64) -> Self {
Self::new(value)
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: i32) -> Self {
Self::new(value)
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: u8) -> Self {
Self::new(value)
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: u16) -> Self {
Self::new(value)
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: u32) -> Self {
Self::new(value)
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: i8) -> Self {
Self::new(value)
}
}
-impl From for PointMeta {
+impl From for PointData {
fn from(value: i16) -> Self {
Self::new(value)
}
}
+
+#[cfg(test)]
+mod tests {
+ use super::*;
+ use rpcl2_derive::Fields;
+
+ #[allow(dead_code)]
+ #[derive(Fields)]
+ struct TestStruct {
+ field1: String,
+ #[rpcl2(name = "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"]);
+ }
+}
diff --git a/src/points.rs b/src/points.rs
index aa0b7aa..deb9068 100644
--- a/src/points.rs
+++ b/src/points.rs
@@ -149,11 +149,7 @@ impl Fields<3> for PointXYZ {
impl From> for PointXYZ {
fn from(point: RPCL2Point<3>) -> Self {
- Self::new(
- point.fields[0].get(),
- point.fields[1].get(),
- point.fields[2].get(),
- )
+ Self::new(point[0].get(), point[1].get(), point[2].get())
}
}
@@ -200,10 +196,10 @@ impl Fields<4> for PointXYZI {
impl From> for PointXYZI {
fn from(point: RPCL2Point<4>) -> Self {
Self::new(
- point.fields[0].get(),
- point.fields[1].get(),
- point.fields[2].get(),
- point.fields[3].get(),
+ point[0].get(),
+ point[1].get(),
+ point[2].get(),
+ point[3].get(),
)
}
}
@@ -257,10 +253,10 @@ impl Fields<4> for PointXYZL {
impl From> for PointXYZL {
fn from(point: RPCL2Point<4>) -> Self {
Self::new(
- point.fields[0].get(),
- point.fields[1].get(),
- point.fields[2].get(),
- point.fields[3].get(),
+ point[0].get(),
+ point[1].get(),
+ point[2].get(),
+ point[3].get(),
)
}
}
@@ -327,10 +323,10 @@ impl Fields<4> for PointXYZRGB {
impl From> for PointXYZRGB {
fn from(point: RPCL2Point<4>) -> Self {
Self {
- x: point.fields[0].get(),
- y: point.fields[1].get(),
- z: point.fields[2].get(),
- rgb: point.fields[3].get(),
+ x: point[0].get(),
+ y: point[1].get(),
+ z: point[2].get(),
+ rgb: point[3].get(),
}
}
}
@@ -399,11 +395,11 @@ impl Fields<5> for PointXYZRGBA {
impl From> for PointXYZRGBA {
fn from(point: RPCL2Point<5>) -> Self {
Self {
- x: point.fields[0].get(),
- y: point.fields[1].get(),
- z: point.fields[2].get(),
- rgb: point.fields[3].get::().into(),
- a: point.fields[4].get(),
+ x: point[0].get(),
+ y: point[1].get(),
+ z: point[2].get(),
+ rgb: point[3].get::().into(),
+ a: point[4].get(),
}
}
}
@@ -489,13 +485,13 @@ impl Fields<7> for PointXYZRGBNormal {
impl From> for PointXYZRGBNormal {
fn from(point: RPCL2Point<7>) -> Self {
Self {
- x: point.fields[0].get(),
- y: point.fields[1].get(),
- z: point.fields[2].get(),
- rgb: point.fields[3].get::().into(),
- normal_x: point.fields[4].get(),
- normal_y: point.fields[5].get(),
- normal_z: point.fields[6].get(),
+ x: point[0].get(),
+ y: point[1].get(),
+ z: point[2].get(),
+ rgb: point[3].get::().into(),
+ normal_x: point[4].get(),
+ normal_y: point[5].get(),
+ normal_z: point[6].get(),
}
}
}
@@ -571,13 +567,13 @@ impl Fields<7> for PointXYZINormal {
impl From> for PointXYZINormal {
fn from(point: RPCL2Point<7>) -> Self {
Self::new(
- point.fields[0].get(),
- point.fields[1].get(),
- point.fields[2].get(),
- point.fields[3].get(),
- point.fields[4].get(),
- point.fields[5].get(),
- point.fields[6].get(),
+ point[0].get(),
+ point[1].get(),
+ point[2].get(),
+ point[3].get(),
+ point[4].get(),
+ point[5].get(),
+ point[6].get(),
)
}
}
@@ -654,11 +650,11 @@ impl Fields<5> for PointXYZRGBL {
impl From> for PointXYZRGBL {
fn from(point: RPCL2Point<5>) -> Self {
Self {
- x: point.fields[0].get(),
- y: point.fields[1].get(),
- z: point.fields[2].get(),
- rgb: point.fields[3].get::().into(),
- label: point.fields[4].get(),
+ x: point[0].get(),
+ y: point[1].get(),
+ z: point[2].get(),
+ rgb: point[3].get::().into(),
+ label: point[4].get(),
}
}
}
@@ -722,12 +718,12 @@ impl Fields<6> for PointXYZNormal {
impl From> for PointXYZNormal {
fn from(point: RPCL2Point<6>) -> Self {
Self::new(
- point.fields[0].get(),
- point.fields[1].get(),
- point.fields[2].get(),
- point.fields[3].get(),
- point.fields[4].get(),
- point.fields[5].get(),
+ point[0].get(),
+ point[1].get(),
+ point[2].get(),
+ point[3].get(),
+ point[4].get(),
+ point[5].get(),
)
}
}
diff --git a/src/prelude.rs b/src/prelude.rs
index 4097dbc..9cbecf2 100644
--- a/src/prelude.rs
+++ b/src/prelude.rs
@@ -12,4 +12,4 @@ pub use type_layout::TypeLayout;
#[cfg(feature = "derive")]
pub use rpcl2_derive::*;
-pub use crate::convert::{FieldDatatype, FromBytes, GetFieldDatatype};
+pub use crate::convert::{FieldDatatype, FromBytes, GetFieldDatatype, PointDataBuffer};
diff --git a/src/ros_types.rs b/src/ros_types.rs
index a17dd53..d88fb00 100644
--- a/src/ros_types.rs
+++ b/src/ros_types.rs
@@ -46,8 +46,10 @@ impl From for crate::PointCloud2Msg {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ dimensions: crate::CloudDimensions {
+ width: msg.width,
+ height: msg.height,
+ },
fields: msg
.fields
.into_iter()
@@ -58,11 +60,19 @@ impl From for crate::PointCloud2Msg {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ endian: if msg.is_bigendian {
+ crate::Endian::Big
+ } else {
+ crate::Endian::Little
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ dense: if msg.is_dense {
+ crate::convert::Denseness::Dense
+ } else {
+ crate::convert::Denseness::Sparse
+ },
}
}
}
@@ -78,8 +88,8 @@ impl From for r2r::sensor_msgs::msg::PointCloud2 {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ height: msg.dimensions.height,
+ width: msg.dimensions.width,
fields: msg
.fields
.into_iter()
@@ -90,11 +100,17 @@ impl From for r2r::sensor_msgs::msg::PointCloud2 {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ is_bigendian: match msg.endian {
+ crate::Endian::Big => true,
+ crate::Endian::Little => false,
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ is_dense: match msg.dense {
+ crate::convert::Denseness::Dense => true,
+ crate::convert::Denseness::Sparse => false,
+ },
}
}
}
@@ -111,8 +127,10 @@ impl From for crate::PointCloud2Msg {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ dimensions: crate::CloudDimensions {
+ width: msg.width,
+ height: msg.height,
+ },
fields: msg
.fields
.into_iter()
@@ -123,11 +141,19 @@ impl From for crate::PointCloud2Msg {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ endian: if msg.is_bigendian {
+ crate::Endian::Big
+ } else {
+ crate::Endian::Little
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ dense: if msg.is_dense {
+ crate::convert::Denseness::Dense
+ } else {
+ crate::convert::Denseness::Sparse
+ },
}
}
}
@@ -144,8 +170,8 @@ impl From for rosrust_msg::sensor_msgs::PointCloud2 {
},
frame_id: msg.header.frame_id,
},
- height: msg.height,
- width: msg.width,
+ height: msg.dimensions.height,
+ width: msg.dimensions.width,
fields: msg
.fields
.into_iter()
@@ -156,11 +182,19 @@ impl From for rosrust_msg::sensor_msgs::PointCloud2 {
count: field.count,
})
.collect(),
- is_bigendian: msg.is_bigendian,
+ is_bigendian: if msg.endian == crate::Endian::Big {
+ true
+ } else {
+ false
+ },
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
- is_dense: msg.is_dense,
+ is_dense: if msg.dense == crate::convert::Denseness::Dense {
+ true
+ } else {
+ false
+ },
}
}
}
diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs
index ffbc5e5..4edd9ac 100644
--- a/tests/e2e_test.rs
+++ b/tests/e2e_test.rs
@@ -78,7 +78,7 @@ fn write_cloud_from_vec() {
#[test]
#[cfg(feature = "derive")]
fn custom_xyz_f32() {
- #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
+ #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
#[repr(C)]
struct CustomPoint {
x: f32,
@@ -138,7 +138,7 @@ fn custom_xyzi_f32() {
},
];
- #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
+ #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
#[repr(C)]
struct CustomPointXYZI {
x: f32,
@@ -153,7 +153,7 @@ fn custom_xyzi_f32() {
#[test]
#[cfg(feature = "derive")]
fn custom_rgba_f32() {
- #[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
+ #[derive(Debug, PartialEq, Clone, Default, PointConvertible, TypeLayout)]
#[repr(C)]
struct CustomPoint {
x: f32,
@@ -392,9 +392,9 @@ fn write_less_than_available() {
impl From> for CustomPoint {
fn from(point: RPCL2Point<3>) -> Self {
Self {
- x: point.fields[0].get(),
- y: point.fields[1].get(),
- z: point.fields[2].get(),
+ x: point[0].get(),
+ y: point[1].get(),
+ z: point[2].get(),
dummy: 0.0,
}
}
