API changes for v1 (#14)
* reduce to functions * fix msg not found * impl par iterator * clippy * pre endian * derive as feat only * custom conversions * tidy * test with all feats * indent
This commit is contained in:
stelzo
GitHub
parent
a0ca014e13
commit
89a1dbbfdd
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
37
Cargo.toml
37
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"]
|
||||
r2r_msg = ["dep:r2r"]
|
||||
rayon = ["dep:rayon"]
|
||||
derive = ["dep:rpcl2_derive", "dep:type-layout"]
|
||||
nalgebra = ["dep:nalgebra"]
|
||||
|
||||
default = ["rayon", "derive", "nalgebra"]
|
||||
72
README.md
72
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/).
|
||||
|
||||
<p align="center">
|
||||
<h3 align="center">ROS PointCloud2</h3>
|
||||
<p align="center">Customizable conversions to and from the PointCloud2 ROS message.</p>
|
||||
<p align="center">A complete and versatile abstraction of PointCloud2.</p>
|
||||
<p align="center"><a href="https://crates.io/crates/ros_pointcloud2"><img src="https://img.shields.io/crates/v/ros_pointcloud2.svg" alt=""></a> <a href="https://github.com/stelzo/ros_pointcloud2/tree/main/tests"><img src="https://github.com/stelzo/ros_pointcloud2/actions/workflows/tests.yml/badge.svg" alt=""></a>
|
||||
</p>
|
||||
</p>
|
||||
|
||||
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::<Vec<PointXYZ>>(); // iterating points here O(n)
|
||||
|
||||
assert_eq!(new_cloud_points, cloud_copy);
|
||||
point
|
||||
})
|
||||
.collect::<Vec<_>>();
|
||||
```
|
||||
|
||||
## 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
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
|
|
@ -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/)
|
||||
|
|
|
|||
|
|
@ -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<PointXYZ> {
|
||||
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<PointXYZ>) -> bool {
|
||||
let orig_len = cloud.len();
|
||||
let internal_msg = PointCloud2Msg::try_from_vec(cloud).unwrap();
|
||||
let total = internal_msg
|
||||
.try_into_iter()
|
||||
.unwrap()
|
||||
.collect::<Vec<PointXYZ>>();
|
||||
orig_len == total.len()
|
||||
}
|
||||
|
||||
fn roundtrip(cloud: Vec<PointXYZ>) -> bool {
|
||||
let orig_len = cloud.len();
|
||||
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
|
||||
let total = internal_msg
|
||||
.try_into_iter()
|
||||
.unwrap()
|
||||
.collect::<Vec<PointXYZ>>();
|
||||
orig_len == total.len()
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
fn roundtrip_filter_vec(cloud: Vec<PointXYZ>) -> 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<PointXYZ>) -> 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<PointXYZ>) -> 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::<Vec<PointXYZ>>();
|
||||
orig_len != total.len()
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> 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);
|
||||
|
|
@ -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<Label> for u8 {
|
||||
fn from(label: Label) -> Self {
|
||||
match label {
|
||||
Label::Human => 0,
|
||||
Label::Deer => 1,
|
||||
Label::Car => 2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<u8> 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<CustomPoint> 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<RPCL2Point<5>> 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> {
|
||||
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::<Vec<_>>();
|
||||
|
||||
println!("Filtered cloud: {:?}", out);
|
||||
|
||||
assert_eq!(
|
||||
vec![CustomPoint::new(1.0, 2.0, 3.0, 4.0, Label::Deer),],
|
||||
out
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
@ -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<PointXYZ> {
|
||||
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<PointXYZ>) -> bool {
|
||||
let orig_len = cloud.len();
|
||||
let internal_msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
|
||||
let total = internal_msg
|
||||
.try_into_iter()
|
||||
.unwrap()
|
||||
.collect::<Vec<PointXYZ>>();
|
||||
orig_len == total.len()
|
||||
}
|
||||
|
||||
fn roundtrip_filter(cloud: Vec<PointXYZ>) -> 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<PointXYZ>) -> 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::<Vec<PointXYZ>>();
|
||||
orig_len != total.len()
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
fn roundtrip_filter_par(cloud: Vec<PointXYZ>) -> 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<PointXYZ>) -> 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<F>(pcl_size: usize, func: F) -> Duration
|
||||
where
|
||||
F: Fn(Vec<PointXYZ>) -> 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);
|
||||
}
|
||||
|
|
@ -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::<Vec<_>>();
|
||||
|
||||
println!("Filtered cloud: {:?}", out);
|
||||
|
||||
assert_eq!(vec![PointXYZ::new(1.0, 1.0, 1.0),], out);
|
||||
}
|
||||
|
|
@ -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"
|
||||
|
|
@ -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::<Vec<_>>();
|
||||
|
||||
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::<Vec<_>>();
|
||||
|
||||
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::<Vec<_>>();
|
||||
|
||||
let from_my_point = quote! {
|
||||
impl From<ros_pointcloud2::RPCL2Point<#field_len_token>> 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::<Vec<_>>();
|
||||
|
||||
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)
|
||||
}
|
||||
137
src/convert.rs
137
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::<u8>(),
|
||||
FieldDatatype::U16 => std::mem::size_of::<u16>(),
|
||||
FieldDatatype::U32 => std::mem::size_of::<u32>(),
|
||||
FieldDatatype::I8 => std::mem::size_of::<i8>(),
|
||||
FieldDatatype::I16 => std::mem::size_of::<i16>(),
|
||||
FieldDatatype::I32 => std::mem::size_of::<i32>(),
|
||||
FieldDatatype::F32 => std::mem::size_of::<f32>(),
|
||||
FieldDatatype::F64 => std::mem::size_of::<f64>(),
|
||||
FieldDatatype::RGB => std::mem::size_of::<f32>(), // packed in f32
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl TryFrom<String> for FieldDatatype {
|
||||
type Error = MsgConversionError;
|
||||
|
||||
fn try_from(value: String) -> Result<Self, Self::Error> {
|
||||
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<u8> for FieldDatatype {
|
||||
type Error = ConversionError;
|
||||
type Error = MsgConversionError;
|
||||
|
||||
fn try_from(value: u8) -> Result<Self, Self::Error> {
|
||||
match value {
|
||||
|
|
@ -95,7 +127,7 @@ impl TryFrom<u8> 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<FieldDatatype> 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<usize>,
|
||||
fields: &[PointFieldMsg],
|
||||
xyz_field_type: &FieldDatatype,
|
||||
) -> Result<PointFieldMsg, ConversionError> {
|
||||
match coord {
|
||||
Some(y_idx) => {
|
||||
let field = &fields[y_idx];
|
||||
if field.datatype != u8::from(*xyz_field_type) {
|
||||
return Err(ConversionError::InvalidFieldFormat);
|
||||
}
|
||||
Ok(field.clone())
|
||||
}
|
||||
None => Err(ConversionError::NotEnoughFields),
|
||||
}
|
||||
}
|
||||
|
||||
/// Matching field names from each meta data per point to the PointField name.
|
||||
/// 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<const METADIM: usize> {
|
||||
fn meta_names() -> [&'static str; METADIM];
|
||||
pub trait Fields<const N: usize> {
|
||||
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<u8> {
|
||||
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<T: FromBytes, const SIZE: usize>(
|
||||
start_idx: usize,
|
||||
data: &[u8],
|
||||
endian: &Endianness,
|
||||
) -> T {
|
||||
match endian {
|
||||
Endianness::Big => T::from_be_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
|
||||
Endianness::Little => T::from_le_bytes(load_bytes::<SIZE>(start_idx, data).as_slice()),
|
||||
}
|
||||
}
|
||||
|
||||
/// Note: check if the data slice is long enough to load the bytes beforehand! Uses unsafe indexing.
|
||||
#[inline(always)]
|
||||
fn load_bytes<const S: usize>(start_idx: usize, data: &[u8]) -> [u8; S] {
|
||||
let mut target = [u8::default(); S];
|
||||
|
||||
debug_assert!(target.len() == S);
|
||||
debug_assert!(data.len() >= start_idx + S);
|
||||
|
||||
let source = unsafe { data.get_unchecked(start_idx..start_idx + S) };
|
||||
target
|
||||
.iter_mut()
|
||||
.zip(source.iter())
|
||||
.for_each(|(target, source)| {
|
||||
*target = *source;
|
||||
});
|
||||
target
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
|
|
|||
|
|
@ -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<const N: usize, C>
|
||||
where
|
||||
C: Fields<N>,
|
||||
{
|
||||
iteration: usize,
|
||||
iteration_back: usize,
|
||||
data: ByteBufferView<N>,
|
||||
phantom_c: std::marker::PhantomData<C>, // internally used for meta names array
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
impl<const N: usize, C> ExactSizeIterator for PointCloudIterator<N, C>
|
||||
where
|
||||
C: PointConvertible<N> + Send + Sync,
|
||||
{
|
||||
fn len(&self) -> usize {
|
||||
self.data.len()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
impl<const N: usize, C> DoubleEndedIterator for PointCloudIterator<N, C>
|
||||
where
|
||||
C: PointConvertible<N> + Send + Sync,
|
||||
{
|
||||
fn next_back(&mut self) -> Option<Self::Item> {
|
||||
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<const N: usize, C> rayon::iter::ParallelIterator for PointCloudIterator<N, C>
|
||||
where
|
||||
C: PointConvertible<N> + Send + Sync,
|
||||
{
|
||||
type Item = C;
|
||||
|
||||
fn drive_unindexed<Co>(self, consumer: Co) -> Co::Result
|
||||
where
|
||||
Co: rayon::iter::plumbing::UnindexedConsumer<Self::Item>,
|
||||
{
|
||||
rayon::iter::plumbing::bridge(self, consumer)
|
||||
}
|
||||
|
||||
fn opt_len(&self) -> Option<usize> {
|
||||
Some(self.data.len())
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
impl<const N: usize, C> rayon::iter::IndexedParallelIterator for PointCloudIterator<N, C>
|
||||
where
|
||||
C: PointConvertible<N> + Send + Sync,
|
||||
{
|
||||
fn len(&self) -> usize {
|
||||
self.data.len()
|
||||
}
|
||||
|
||||
fn drive<Co>(self, consumer: Co) -> Co::Result
|
||||
where
|
||||
Co: rayon::iter::plumbing::Consumer<Self::Item>,
|
||||
{
|
||||
rayon::iter::plumbing::bridge(self, consumer)
|
||||
}
|
||||
|
||||
fn with_producer<CB: rayon::iter::plumbing::ProducerCallback<Self::Item>>(
|
||||
self,
|
||||
callback: CB,
|
||||
) -> CB::Output {
|
||||
callback.callback(RayonProducer::from(self))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
struct RayonProducer<const N: usize, C: PointConvertible<N>> {
|
||||
iter: PointCloudIterator<N, C>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
impl<const N: usize, C> rayon::iter::plumbing::Producer for RayonProducer<N, C>
|
||||
where
|
||||
C: PointConvertible<N> + Send + Sync,
|
||||
{
|
||||
type Item = C;
|
||||
type IntoIter = PointCloudIterator<N, C>;
|
||||
|
||||
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<const N: usize, C> From<PointCloudIterator<N, C>> for RayonProducer<N, C>
|
||||
where
|
||||
C: PointConvertible<N> + Send + Sync,
|
||||
{
|
||||
fn from(iterator: PointCloudIterator<N, C>) -> Self {
|
||||
Self { iter: iterator }
|
||||
}
|
||||
}
|
||||
|
||||
/// Implementation of the iterator trait.
|
||||
impl<const N: usize, C> Iterator for PointCloudIterator<N, C>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
type Item = C;
|
||||
|
||||
fn size_hint(&self) -> (usize, Option<usize>) {
|
||||
let buf_len = self.data.len();
|
||||
(buf_len, Some(buf_len))
|
||||
}
|
||||
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
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<const N: usize> {
|
||||
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<const N: usize> ByteBufferView<N> {
|
||||
fn new(
|
||||
data: Vec<u8>,
|
||||
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<N> {
|
||||
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<const N: usize, C> TryFrom<PointCloud2Msg> for PointCloudIterator<N, C>
|
||||
where
|
||||
C: Fields<N>,
|
||||
{
|
||||
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<Self, Self::Error> {
|
||||
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::<Vec<_>>();
|
||||
|
||||
if !not_found_fieldnames.is_empty() {
|
||||
let names_not_found = not_found_fieldnames
|
||||
.into_iter()
|
||||
.map(|(name, _)| (*name).to_owned())
|
||||
.collect::<Vec<String>>();
|
||||
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<const N: usize, C> PointCloudIterator<N, C>
|
||||
where
|
||||
C: Fields<N>,
|
||||
{
|
||||
#[inline]
|
||||
fn from_byte_buffer_view(data: ByteBufferView<N>) -> 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());
|
||||
}
|
||||
}
|
||||
611
src/lib.rs
611
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::<Vec<PointXYZ>>(); // iterating points here O(n)
|
||||
//! .collect::<Vec<_>>(); // 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::<T>().
|
||||
/// Use it for your custom Reader and Writer implementations.
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// writer::Writer, size_of, pcl_utils::PointXYZ,
|
||||
/// };
|
||||
///
|
||||
/// type MyWriterXYZ = Writer<f32, { size_of!(f32) }, 3, 0, PointXYZ>;
|
||||
/// ```
|
||||
#[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<String>),
|
||||
}
|
||||
|
||||
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 {
|
||||
<MsgConversionError as std::fmt::Display>::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<PointFieldMsg>,
|
||||
pub is_bigendian: bool,
|
||||
pub point_step: u32,
|
||||
pub row_step: u32,
|
||||
pub data: Vec<u8>,
|
||||
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<const N: usize, C>() -> Result<Self, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
let point: RPCL2Point<N> = 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<PointFieldMsg> = 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<const N: usize, C>(&self) -> Result<bool, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
let point: RPCL2Point<N> = 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<const N: usize, C>() -> Result<Self, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
let point: RPCL2Point<N> = 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<PointXYZ> = 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<const N: usize, C>(
|
||||
iterable: impl IntoIterator<Item = C>,
|
||||
) -> Result<Self, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
let mut cloud = Self::prepare::<N, C>()?;
|
||||
|
||||
iterable.into_iter().for_each(|coords| {
|
||||
let point: RPCL2Point<N> = 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<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
match system_endian() {
|
||||
Endianness::Big => Self::try_from_iter(vec),
|
||||
Endianness::Little => {
|
||||
let mut cloud = Self::prepare_direct_copy::<N, C>()?;
|
||||
|
||||
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<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
self.assert_byte_similarity::<N, C>()?;
|
||||
|
||||
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<const N: usize, C>(
|
||||
self,
|
||||
) -> Result<impl Iterator<Item = C>, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N>,
|
||||
{
|
||||
iterator::PointCloudIterator::try_from(self)
|
||||
}
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
pub fn try_into_par_iter<const N: usize, C>(
|
||||
self,
|
||||
) -> Result<impl rayon::iter::ParallelIterator<Item = C>, MsgConversionError>
|
||||
where
|
||||
C: PointConvertible<N> + 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<MyPointXYZI> for Point<f32, 3, 1> {
|
||||
/// fn from(point: MyPointXYZI) -> Self {
|
||||
/// Point {
|
||||
/// coords: [point.x, point.y, point.z],
|
||||
/// meta: [point.intensity.into()],
|
||||
/// }
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl From<Point<f32, 3, 1>> for MyPointXYZI {
|
||||
/// fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
/// Self {
|
||||
/// x: point.coords[0],
|
||||
/// y: point.coords[1],
|
||||
/// z: point.coords[2],
|
||||
/// intensity: point.meta[0].get(),
|
||||
/// }
|
||||
/// }
|
||||
/// }
|
||||
/// ```
|
||||
pub struct Point<T, const DIM: usize, const METADIM: usize> {
|
||||
pub coords: [T; DIM],
|
||||
pub meta: [PointMeta; METADIM],
|
||||
/// See the [`ros_pointcloud2::PointConvertible`] trait for more information.
|
||||
pub struct RPCL2Point<const N: usize> {
|
||||
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<const N: usize> Default for RPCL2Point<N> {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
fields: [PointMeta::default(); N],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<const N: usize> From<[PointMeta; N]> for RPCL2Point<N> {
|
||||
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<T, const DIM: usize, const METADIM: usize> {
|
|||
/// pub intensity: f32,
|
||||
/// }
|
||||
///
|
||||
/// impl From<MyPointXYZI> for Point<f32, 3, 1> {
|
||||
/// impl From<MyPointXYZI> 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<Point<f32, 3, 1>> for MyPointXYZI {
|
||||
/// fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
/// impl From<RPCL2Point<4>> 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<f32, {size_of!(f32)}, 3, 1> for MyPointXYZI {}
|
||||
/// impl PointConvertible<4> for MyPointXYZI {}
|
||||
/// ```
|
||||
pub trait PointConvertible<T, const SIZE: usize, const DIM: usize, const METADIM: usize>:
|
||||
From<Point<T, DIM, METADIM>> + Into<Point<T, DIM, METADIM>> + MetaNames<METADIM> + Clone + 'static
|
||||
where
|
||||
T: FromBytes,
|
||||
#[cfg(not(feature = "derive"))]
|
||||
pub trait PointConvertible<const N: usize>:
|
||||
From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Clone + 'static + Default
|
||||
{
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
pub trait PointConvertible<const N: usize>:
|
||||
type_layout::TypeLayout + From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + 'static + Default
|
||||
{
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
enum PointField {
|
||||
Padding(u32),
|
||||
Field {
|
||||
name: String,
|
||||
size: u32,
|
||||
datatype: u8,
|
||||
},
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
struct TypeLayoutInfo {
|
||||
fields: Vec<PointField>,
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
impl TryFrom<type_layout::Field> for PointField {
|
||||
type Error = MsgConversionError;
|
||||
|
||||
fn try_from(f: type_layout::Field) -> Result<Self, Self::Error> {
|
||||
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<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
|
||||
type Error = MsgConversionError;
|
||||
|
||||
fn try_from(t: type_layout::TypeLayoutInfo) -> Result<Self, Self::Error> {
|
||||
let fields: Vec<PointField> = t
|
||||
.fields
|
||||
.into_iter()
|
||||
.map(PointField::try_from)
|
||||
.collect::<Result<Vec<_>, _>>()?;
|
||||
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::<f64>()],
|
||||
endianness: Endianness,
|
||||
datatype: FieldDatatype,
|
||||
}
|
||||
|
||||
|
|
@ -228,6 +555,7 @@ impl Default for PointMeta {
|
|||
Self {
|
||||
bytes: [u8::default(); std::mem::size_of::<f64>()],
|
||||
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::<f64>()]; // 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::<f64>()];
|
||||
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),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
422
src/pcl_utils.rs
422
src/pcl_utils.rs
|
|
@ -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<Point<f32, 3, 0>> for PointXYZ {
|
||||
fn from(point: Point<f32, 3, 0>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZ> for Point<f32, 3, 0> {
|
||||
fn from(point: PointXYZ) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<0> for PointXYZ {
|
||||
fn meta_names() -> [&'static str; 0] {
|
||||
[]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for PointXYZ {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates and an intensity value.
|
||||
#[derive(Clone, Debug, PartialEq, Copy)]
|
||||
pub struct PointXYZI {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub intensity: f32,
|
||||
}
|
||||
|
||||
impl From<PointXYZI> for Point<f32, 3, 1> {
|
||||
fn from(point: PointXYZI) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [point.intensity.into()],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<Point<f32, 3, 1>> for PointXYZI {
|
||||
fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
intensity: point.meta[0].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<1> for PointXYZI {
|
||||
fn meta_names() -> [&'static str; 1] {
|
||||
["intensity"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 1> for PointXYZI {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates and an RGB color value.
|
||||
#[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<Point<f32, 3, 1>> for PointXYZRGB {
|
||||
fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
let rgb = point.meta[0].get::<f32>();
|
||||
let rgb_unpacked = unpack_rgb(rgb);
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
r: rgb_unpacked[0],
|
||||
g: rgb_unpacked[1],
|
||||
b: rgb_unpacked[2],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZRGB> for Point<f32, 3, 1> {
|
||||
fn from(point: PointXYZRGB) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [pack_rgb(point.r, point.g, point.b).into()],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<1> for PointXYZRGB {
|
||||
fn meta_names() -> [&'static str; 1] {
|
||||
["rgb"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 1> for PointXYZRGB {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates and an RGBA color value.
|
||||
/// 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<Point<f32, 3, 2>> for PointXYZRGBA {
|
||||
fn from(point: Point<f32, 3, 2>) -> Self {
|
||||
let rgb = point.meta[0].get::<f32>();
|
||||
let rgb_unpacked = unpack_rgb(rgb);
|
||||
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<PointXYZRGBA> for Point<f32, 3, 2> {
|
||||
fn from(point: PointXYZRGBA) -> Self {
|
||||
let rgb = pack_rgb(point.r, point.g, point.b);
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [rgb.into(), point.a.into()],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<2> for PointXYZRGBA {
|
||||
fn meta_names() -> [&'static str; 2] {
|
||||
["rgb", "a"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 2> for PointXYZRGBA {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates, an RGB color value and a normal vector.
|
||||
#[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<Point<f32, 3, 4>> for PointXYZRGBNormal {
|
||||
fn from(point: Point<f32, 3, 4>) -> Self {
|
||||
let rgb = point.meta[0].get::<f32>();
|
||||
let rgb_unpacked = unpack_rgb(rgb);
|
||||
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<PointXYZRGBNormal> for Point<f32, 3, 4> {
|
||||
fn from(point: PointXYZRGBNormal) -> Self {
|
||||
let rgb = pack_rgb(point.r, point.g, point.b);
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [
|
||||
rgb.into(),
|
||||
point.normal_x.into(),
|
||||
point.normal_y.into(),
|
||||
point.normal_z.into(),
|
||||
],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<4> for PointXYZRGBNormal {
|
||||
fn meta_names() -> [&'static str; 4] {
|
||||
["rgb", "normal_x", "normal_y", "normal_z"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 4> for PointXYZRGBNormal {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates, an intensity value and a normal vector.
|
||||
#[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<PointXYZINormal> for Point<f32, 3, 4> {
|
||||
fn from(point: PointXYZINormal) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [
|
||||
point.intensity.into(),
|
||||
point.normal_x.into(),
|
||||
point.normal_y.into(),
|
||||
point.normal_z.into(),
|
||||
],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<Point<f32, 3, 4>> for PointXYZINormal {
|
||||
fn from(point: Point<f32, 3, 4>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
intensity: point.meta[0].get(),
|
||||
normal_x: point.meta[1].get(),
|
||||
normal_y: point.meta[2].get(),
|
||||
normal_z: point.meta[3].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<4> for PointXYZINormal {
|
||||
fn meta_names() -> [&'static str; 4] {
|
||||
["intensity", "normal_x", "normal_y", "normal_z"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 4> for PointXYZINormal {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates and a label.
|
||||
#[derive(Clone, Debug, PartialEq, Copy)]
|
||||
pub struct PointXYZL {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub label: u32,
|
||||
}
|
||||
|
||||
impl From<Point<f32, 3, 1>> for PointXYZL {
|
||||
fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
label: point.meta[0].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZL> for Point<f32, 3, 1> {
|
||||
fn from(point: PointXYZL) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [point.label.into()],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<1> for PointXYZL {
|
||||
fn meta_names() -> [&'static str; 1] {
|
||||
["label"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 1> for PointXYZL {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates and a label.
|
||||
#[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<Point<f32, 3, 2>> for PointXYZRGBL {
|
||||
fn from(point: Point<f32, 3, 2>) -> Self {
|
||||
let rgb = point.meta[0].get::<f32>();
|
||||
let rgb_unpacked = unpack_rgb(rgb);
|
||||
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<PointXYZRGBL> for Point<f32, 3, 2> {
|
||||
fn from(point: PointXYZRGBL) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [
|
||||
pack_rgb(point.r, point.g, point.b).into(),
|
||||
point.label.into(),
|
||||
],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<2> for PointXYZRGBL {
|
||||
fn meta_names() -> [&'static str; 2] {
|
||||
["rgb", "label"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 2> for PointXYZRGBL {}
|
||||
|
||||
/// Predefined point type commonly used in ROS with PCL.
|
||||
/// This is a 3D point with x, y, z coordinates and a normal vector.
|
||||
#[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<Point<f32, 3, 3>> for PointXYZNormal {
|
||||
fn from(point: Point<f32, 3, 3>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
normal_x: point.meta[0].get(),
|
||||
normal_y: point.meta[1].get(),
|
||||
normal_z: point.meta[2].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZNormal> for Point<f32, 3, 3> {
|
||||
fn from(point: PointXYZNormal) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [
|
||||
point.normal_x.into(),
|
||||
point.normal_y.into(),
|
||||
point.normal_z.into(),
|
||||
],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<3> for PointXYZNormal {
|
||||
fn meta_names() -> [&'static str; 3] {
|
||||
["normal_x", "normal_y", "normal_z"]
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 3> for PointXYZNormal {}
|
||||
|
|
@ -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<RGB> for f32 {
|
||||
fn from(rgb: RGB) -> Self {
|
||||
unsafe { rgb.packed }
|
||||
}
|
||||
}
|
||||
|
||||
impl From<f32> 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<nalgebra::Point3<f32>> for PointXYZ {
|
||||
fn from(point: nalgebra::Point3<f32>) -> Self {
|
||||
Self {
|
||||
x: point.x,
|
||||
y: point.y,
|
||||
z: point.z,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
impl From<PointXYZ> for nalgebra::Point3<f32> {
|
||||
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<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZ {}
|
||||
unsafe impl Sync for PointXYZ {}
|
||||
|
||||
impl Fields<3> for PointXYZ {
|
||||
fn field_names_ordered() -> [&'static str; 3] {
|
||||
["x", "y", "z"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<3>> for PointXYZ {
|
||||
fn from(point: RPCL2Point<3>) -> Self {
|
||||
Self::new(
|
||||
point.fields[0].get(),
|
||||
point.fields[1].get(),
|
||||
point.fields[2].get(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZ> for RPCL2Point<3> {
|
||||
fn from(point: PointXYZ) -> Self {
|
||||
[point.x.into(), point.y.into(), point.z.into()].into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<3> 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
pub struct PointXYZI {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub intensity: f32,
|
||||
}
|
||||
|
||||
impl PointXYZI {
|
||||
pub fn new(x: f32, y: f32, z: f32, intensity: f32) -> Self {
|
||||
Self { x, y, z, intensity }
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZI {}
|
||||
unsafe impl Sync for PointXYZI {}
|
||||
|
||||
impl Fields<4> for PointXYZI {
|
||||
fn field_names_ordered() -> [&'static str; 4] {
|
||||
["x", "y", "z", "intensity"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<4>> for PointXYZI {
|
||||
fn from(point: RPCL2Point<4>) -> Self {
|
||||
Self::new(
|
||||
point.fields[0].get(),
|
||||
point.fields[1].get(),
|
||||
point.fields[2].get(),
|
||||
point.fields[3].get(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZI> for RPCL2Point<4> {
|
||||
fn from(point: PointXYZI) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
point.intensity.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<4> for PointXYZI {}
|
||||
|
||||
/// 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
pub struct PointXYZL {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub label: u32,
|
||||
}
|
||||
|
||||
impl PointXYZL {
|
||||
pub fn new(x: f32, y: f32, z: f32, label: u32) -> Self {
|
||||
Self { x, y, z, label }
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZL {}
|
||||
unsafe impl Sync for PointXYZL {}
|
||||
|
||||
impl Fields<4> for PointXYZL {
|
||||
fn field_names_ordered() -> [&'static str; 4] {
|
||||
["x", "y", "z", "label"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<4>> for PointXYZL {
|
||||
fn from(point: RPCL2Point<4>) -> Self {
|
||||
Self::new(
|
||||
point.fields[0].get(),
|
||||
point.fields[1].get(),
|
||||
point.fields[2].get(),
|
||||
point.fields[3].get(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZL> for RPCL2Point<4> {
|
||||
fn from(point: PointXYZL) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
point.label.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<4> for PointXYZL {}
|
||||
|
||||
/// 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
pub struct PointXYZRGB {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub rgb: RGB,
|
||||
}
|
||||
|
||||
impl PointXYZRGB {
|
||||
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8) -> Self {
|
||||
let rgb = RGB::new(r, g, b);
|
||||
Self { x, y, z, rgb }
|
||||
}
|
||||
|
||||
pub fn r(&self) -> u8 {
|
||||
self.rgb.r()
|
||||
}
|
||||
|
||||
pub fn g(&self) -> u8 {
|
||||
self.rgb.g()
|
||||
}
|
||||
|
||||
pub fn b(&self) -> u8 {
|
||||
self.rgb.b()
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZRGB {}
|
||||
unsafe impl Sync for PointXYZRGB {}
|
||||
|
||||
impl Fields<4> for PointXYZRGB {
|
||||
fn field_names_ordered() -> [&'static str; 4] {
|
||||
["x", "y", "z", "rgb"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<4>> for PointXYZRGB {
|
||||
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(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZRGB> for RPCL2Point<4> {
|
||||
fn from(point: PointXYZRGB) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
f32::from(point.rgb).into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<4> 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
pub struct PointXYZRGBA {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub rgb: RGB,
|
||||
pub a: u8,
|
||||
}
|
||||
|
||||
impl PointXYZRGBA {
|
||||
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, a: u8) -> Self {
|
||||
let rgb = RGB::new(r, g, b);
|
||||
Self { x, y, z, rgb, a }
|
||||
}
|
||||
|
||||
pub fn r(&self) -> u8 {
|
||||
self.rgb.r()
|
||||
}
|
||||
|
||||
pub fn g(&self) -> u8 {
|
||||
self.rgb.g()
|
||||
}
|
||||
|
||||
pub fn b(&self) -> u8 {
|
||||
self.rgb.b()
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZRGBA {}
|
||||
unsafe impl Sync for PointXYZRGBA {}
|
||||
|
||||
impl Fields<5> for PointXYZRGBA {
|
||||
fn field_names_ordered() -> [&'static str; 5] {
|
||||
["x", "y", "z", "rgb", "a"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<5>> for PointXYZRGBA {
|
||||
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::<f32>().into(),
|
||||
a: point.fields[4].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZRGBA> for RPCL2Point<5> {
|
||||
fn from(point: PointXYZRGBA) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
f32::from(point.rgb).into(),
|
||||
point.a.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<5> 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
pub struct PointXYZRGBNormal {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub rgb: RGB,
|
||||
pub normal_x: f32,
|
||||
pub normal_y: f32,
|
||||
pub normal_z: f32,
|
||||
}
|
||||
|
||||
impl PointXYZRGBNormal {
|
||||
pub fn new(
|
||||
x: f32,
|
||||
y: f32,
|
||||
z: f32,
|
||||
rgb: RGB,
|
||||
normal_x: f32,
|
||||
normal_y: f32,
|
||||
normal_z: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
rgb,
|
||||
normal_x,
|
||||
normal_y,
|
||||
normal_z,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn r(&self) -> u8 {
|
||||
self.rgb.r()
|
||||
}
|
||||
|
||||
pub fn g(&self) -> u8 {
|
||||
self.rgb.g()
|
||||
}
|
||||
|
||||
pub fn b(&self) -> u8 {
|
||||
self.rgb.b()
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZRGBNormal {}
|
||||
unsafe impl Sync for PointXYZRGBNormal {}
|
||||
|
||||
impl Fields<7> for PointXYZRGBNormal {
|
||||
fn field_names_ordered() -> [&'static str; 7] {
|
||||
["x", "y", "z", "rgb", "normal_x", "normal_y", "normal_z"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<7>> for PointXYZRGBNormal {
|
||||
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::<f32>().into(),
|
||||
normal_x: point.fields[4].get(),
|
||||
normal_y: point.fields[5].get(),
|
||||
normal_z: point.fields[6].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZRGBNormal> for RPCL2Point<7> {
|
||||
fn from(point: PointXYZRGBNormal) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
f32::from(point.rgb).into(),
|
||||
point.normal_x.into(),
|
||||
point.normal_y.into(),
|
||||
point.normal_z.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<7> 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
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 PointXYZINormal {
|
||||
pub fn new(
|
||||
x: f32,
|
||||
y: f32,
|
||||
z: f32,
|
||||
intensity: f32,
|
||||
normal_x: f32,
|
||||
normal_y: f32,
|
||||
normal_z: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
intensity,
|
||||
normal_x,
|
||||
normal_y,
|
||||
normal_z,
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZINormal {}
|
||||
unsafe impl Sync for PointXYZINormal {}
|
||||
|
||||
impl Fields<7> for PointXYZINormal {
|
||||
fn field_names_ordered() -> [&'static str; 7] {
|
||||
["x", "y", "z", "i", "normal_x", "normal_y", "normal_z"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<7>> for PointXYZINormal {
|
||||
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(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZINormal> for RPCL2Point<7> {
|
||||
fn from(point: PointXYZINormal) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
point.intensity.into(),
|
||||
point.normal_x.into(),
|
||||
point.normal_y.into(),
|
||||
point.normal_z.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<7> 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
pub struct PointXYZRGBL {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
pub rgb: RGB,
|
||||
pub label: u32,
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZRGBL {}
|
||||
unsafe impl Sync for PointXYZRGBL {}
|
||||
|
||||
impl PointXYZRGBL {
|
||||
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, label: u32) -> Self {
|
||||
let rgb = RGB::new(r, g, b);
|
||||
Self {
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
rgb,
|
||||
label,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn r(&self) -> u8 {
|
||||
self.rgb.r()
|
||||
}
|
||||
|
||||
pub fn g(&self) -> u8 {
|
||||
self.rgb.g()
|
||||
}
|
||||
|
||||
pub fn b(&self) -> u8 {
|
||||
self.rgb.b()
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
impl Fields<5> for PointXYZRGBL {
|
||||
fn field_names_ordered() -> [&'static str; 5] {
|
||||
["x", "y", "z", "rgb", "label"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<5>> 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::<f32>().into(),
|
||||
label: point.fields[4].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZRGBL> for RPCL2Point<5> {
|
||||
fn from(point: PointXYZRGBL) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
f32::from(point.rgb).into(),
|
||||
point.label.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<5> 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, Default)]
|
||||
#[cfg_attr(feature = "derive", derive(TypeLayout))]
|
||||
#[repr(C)]
|
||||
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 PointXYZNormal {
|
||||
pub fn new(x: f32, y: f32, z: f32, normal_x: f32, normal_y: f32, normal_z: f32) -> Self {
|
||||
Self {
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
normal_x,
|
||||
normal_y,
|
||||
normal_z,
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "nalgebra")]
|
||||
pub fn xyz(&self) -> nalgebra::Point3<f32> {
|
||||
nalgebra::Point3::new(self.x, self.y, self.z)
|
||||
}
|
||||
}
|
||||
|
||||
unsafe impl Send for PointXYZNormal {}
|
||||
unsafe impl Sync for PointXYZNormal {}
|
||||
|
||||
impl Fields<6> for PointXYZNormal {
|
||||
fn field_names_ordered() -> [&'static str; 6] {
|
||||
["x", "y", "z", "normal_x", "normal_y", "normal_z"]
|
||||
}
|
||||
}
|
||||
|
||||
impl From<RPCL2Point<6>> for PointXYZNormal {
|
||||
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(),
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<PointXYZNormal> for RPCL2Point<6> {
|
||||
fn from(point: PointXYZNormal) -> Self {
|
||||
[
|
||||
point.x.into(),
|
||||
point.y.into(),
|
||||
point.z.into(),
|
||||
point.normal_x.into(),
|
||||
point.normal_y.into(),
|
||||
point.normal_z.into(),
|
||||
]
|
||||
.into()
|
||||
}
|
||||
}
|
||||
|
||||
impl PointConvertible<6> for PointXYZNormal {}
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
/// Re-export commonly used types and traits.
|
||||
pub use crate::points::*;
|
||||
|
||||
#[cfg(feature = "rayon")]
|
||||
pub use rayon::prelude::*;
|
||||
|
||||
pub use crate::{Fields, MsgConversionError, PointCloud2Msg, PointConvertible, RPCL2Point};
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
pub use type_layout::TypeLayout;
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
pub use rpcl2_derive::*;
|
||||
|
||||
pub use crate::convert::{FieldDatatype, FromBytes, GetFieldDatatype};
|
||||
422
src/reader.rs
422
src/reader.rs
|
|
@ -1,422 +0,0 @@
|
|||
use crate::{
|
||||
pcl_utils::*,
|
||||
Point,
|
||||
PointCloud2Msg,
|
||||
PointConvertible,
|
||||
ConversionError,
|
||||
MetaNames,
|
||||
PointMeta,
|
||||
convert::{
|
||||
FromBytes,
|
||||
FieldDatatype,
|
||||
load_loadable,
|
||||
Endianness,
|
||||
check_coord,
|
||||
},
|
||||
};
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderF32<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderF64<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderI8<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderI16<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderI32<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderU8<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderU16<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Reader that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type ReaderU32<const DIM: usize, const METADIM: usize, C> =
|
||||
Reader<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`).
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZ = ReaderF32<3, 0, PointXYZ>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`).
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZI = ReaderF32<3, 1, PointXYZI>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`).
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZNormal = ReaderF32<3, 3, PointXYZNormal>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb (see `PointXYZRGB`).
|
||||
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZRGB = ReaderF32<3, 1, PointXYZRGB>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb and intensity (see `PointXYZRGBL`).
|
||||
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZRGBL = ReaderF32<3, 2, PointXYZRGBL>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and unpacked rgb with additional alpha channel (see `PointXYZRGBA`).
|
||||
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZRGBA = ReaderF32<3, 2, PointXYZRGBA>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and normal and unpacked rgb (see `PointXYZRGBNormal`).
|
||||
/// Inside the message, the rgb field gets packed into a single u32 which is ROS standard.
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZRGBNormal = ReaderF32<3, 4, PointXYZRGBNormal>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
|
||||
/// Every additional meta data is ignored.
|
||||
pub type ReaderXYZINormal = ReaderF32<3, 4, PointXYZINormal>;
|
||||
|
||||
/// Provides the message as an Iterator over xyz coordinates and intensity with additional label (see `PointXYZL`).
|
||||
/// Every additional meta data is ignored.
|
||||
/// The label is typically used for semantic segmentation.
|
||||
pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>;
|
||||
|
||||
/// The Reader provides a an iterator abstraction of the PointCloud2Msg.
|
||||
///
|
||||
/// The iterator is defined at compile time, so the point has to be described via template arguments.
|
||||
///
|
||||
/// # Predefined Readers
|
||||
/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
|
||||
///
|
||||
/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
|
||||
/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
|
||||
///
|
||||
/// ROS1 with rosrust:
|
||||
/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
|
||||
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
|
||||
///
|
||||
/// ROS2 with r2r:
|
||||
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
|
||||
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
|
||||
///
|
||||
/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
|
||||
///
|
||||
/// ## Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
/// };
|
||||
///
|
||||
///
|
||||
/// let cloud_points: Vec<PointXYZ> = vec![
|
||||
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
|
||||
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
|
||||
/// ];
|
||||
///
|
||||
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
|
||||
/// let convert = ReaderXYZ::try_from(msg).unwrap();
|
||||
/// // ^^^^^^^^^ conversion from PointCloud2Msg to Reader that implements Iterator
|
||||
///
|
||||
/// convert.for_each(|point: PointXYZ| {
|
||||
/// // do something with the point
|
||||
/// });
|
||||
/// ```
|
||||
/// # Fully Custom Reader
|
||||
/// When the predefined types are not enough (like sensor specific metadata), you can describe your Reader with the following template arguments:
|
||||
/// - T: The coordinate type, e.g. f32
|
||||
/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
|
||||
/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
|
||||
/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
|
||||
/// Afterwards, implement the PointConvertible trait for your custom point type.
|
||||
///
|
||||
/// ## Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
|
||||
/// };
|
||||
///
|
||||
/// type Xyz = f32; // coordinate type
|
||||
/// const XYZ_S: usize = size_of!(Xyz);
|
||||
/// const DIM: usize = 3; // helper constant for dimensionality
|
||||
/// const METADIM: usize = 1; // helper constant for meta data fields
|
||||
///
|
||||
/// #[derive(Debug, PartialEq, Clone)]
|
||||
/// struct CustomPoint {
|
||||
/// x: f32,
|
||||
/// y: f32,
|
||||
/// z: f32,
|
||||
/// i: u8,
|
||||
/// }
|
||||
///
|
||||
/// impl From<Point<f32, 3, 1>> for CustomPoint {
|
||||
/// fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
/// Self {
|
||||
/// x: point.coords[0],
|
||||
/// y: point.coords[1],
|
||||
/// z: point.coords[2],
|
||||
/// i: point.meta[0].get(),
|
||||
/// }
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
///impl From<CustomPoint> for Point<f32, 3, 1> {
|
||||
/// fn from(point: CustomPoint) -> Self {
|
||||
/// Point {
|
||||
/// coords: [point.x, point.y, point.z],
|
||||
/// meta: [
|
||||
/// point.i.into(),
|
||||
/// ],
|
||||
/// }
|
||||
/// }
|
||||
///}
|
||||
///
|
||||
/// impl MetaNames<METADIM> for CustomPoint {
|
||||
/// fn meta_names() -> [&'static str; METADIM] {
|
||||
/// ["intensity"]
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
|
||||
///
|
||||
/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
|
||||
/// // ^^^^^^^^ your custom Reader
|
||||
/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
|
||||
/// ```
|
||||
pub struct Reader<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
|
||||
where
|
||||
T: FromBytes,
|
||||
C: MetaNames<METADIM>,
|
||||
{
|
||||
iteration: usize,
|
||||
data: Vec<u8>,
|
||||
point_step_size: usize,
|
||||
cloud_length: usize,
|
||||
offsets: Vec<usize>,
|
||||
meta: Vec<(String, FieldDatatype)>,
|
||||
endianness: Endianness,
|
||||
phantom_t: std::marker::PhantomData<T>, // internally used for byte and datatype conversions
|
||||
phantom_c: std::marker::PhantomData<C>, // internally used for meta names array
|
||||
}
|
||||
|
||||
/// The iterator implementation for the Reader struct.
|
||||
/// The iterator is fallible because the data is read from a byte buffer inside the PointCloud2 message, which is inherently unsafe.
|
||||
///
|
||||
/// See ConversionError for possible errors that can occur during iteration.
|
||||
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> Iterator
|
||||
for Reader<T, SIZE, DIM, METADIM, C>
|
||||
where
|
||||
T: FromBytes,
|
||||
C: PointConvertible<T, SIZE, DIM, METADIM>,
|
||||
{
|
||||
type Item = C;
|
||||
|
||||
/// The size_hint is the length of the remaining elements and the maximum length of the iterator.
|
||||
///
|
||||
/// PointCloud2 messages contain the length of the cloud, so we can prepare coming operations.
|
||||
/// This hint is used inside common iterator functions like `collect<Vec<_>>`, for example.
|
||||
fn size_hint(&self) -> (usize, Option<usize>) {
|
||||
(self.cloud_length - self.iteration, Some(self.cloud_length))
|
||||
}
|
||||
|
||||
/// Get the data from the byte buffer and convert it to the predefined point.
|
||||
/// It must keep track of the current iteration and the length of the cloud so it has to mutate self.
|
||||
///
|
||||
/// The current point is then converted into the custom type. If the conversion fails, an error is returned.
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
if self.iteration >= self.cloud_length {
|
||||
return None; // iteration finished
|
||||
}
|
||||
|
||||
let mut xyz = [T::default(); DIM];
|
||||
xyz.iter_mut()
|
||||
.zip(self.offsets.iter())
|
||||
.for_each(|(p_xyz, in_point_offset)| {
|
||||
*p_xyz = load_loadable::<T, SIZE>(
|
||||
(self.iteration * self.point_step_size) + in_point_offset,
|
||||
&self.data,
|
||||
&self.endianness,
|
||||
);
|
||||
});
|
||||
|
||||
debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch");
|
||||
debug_assert!(
|
||||
self.offsets.len() == DIM + METADIM,
|
||||
"Offset length mismatch"
|
||||
);
|
||||
|
||||
let mut meta = [PointMeta::default(); METADIM];
|
||||
meta.iter_mut()
|
||||
.zip(self.offsets.iter().skip(DIM))
|
||||
.zip(self.meta.iter())
|
||||
.for_each(|((p_meta, in_point_offset), (_, meta_type))| {
|
||||
let start = (self.iteration * self.point_step_size) + in_point_offset;
|
||||
*p_meta = PointMeta::from_buffer(&self.data, start, meta_type);
|
||||
});
|
||||
|
||||
self.iteration += 1;
|
||||
|
||||
Some(C::from(Point { coords: xyz, meta }))
|
||||
}
|
||||
}
|
||||
|
||||
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryFrom<PointCloud2Msg>
|
||||
for Reader<T, SIZE, DIM, METADIM, C>
|
||||
where
|
||||
T: FromBytes,
|
||||
C: MetaNames<METADIM>,
|
||||
{
|
||||
type Error = ConversionError;
|
||||
|
||||
/// Convert a PointCloud2Msg into a Reader.
|
||||
/// Converting a PointCloud2Msg into a Reader is a fallible operation since the message can contain only a subset of the required fields.
|
||||
///
|
||||
/// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
|
||||
/// It therefore has effectively a constant time complexity O(1) for practical purposes.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
/// };
|
||||
///
|
||||
/// let cloud_points: Vec<PointXYZ> = vec![
|
||||
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
|
||||
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
|
||||
/// ];
|
||||
///
|
||||
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
|
||||
///
|
||||
/// let convert = ReaderXYZ::try_from(msg).unwrap();
|
||||
/// // ^^^^^^^^ conversion from PointCloud2Msg to Reader
|
||||
/// ```
|
||||
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
|
||||
if cloud.fields.len() < DIM {
|
||||
return Err(ConversionError::NotEnoughFields);
|
||||
}
|
||||
|
||||
let xyz_field_type = T::field_datatype();
|
||||
|
||||
let mut has_x: Option<usize> = None;
|
||||
let mut has_y: Option<usize> = None;
|
||||
let mut has_z: Option<usize> = None;
|
||||
|
||||
let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); METADIM];
|
||||
|
||||
let lower_meta_names = C::meta_names()
|
||||
.iter()
|
||||
.map(|x| x.to_lowercase())
|
||||
.collect::<Vec<String>>();
|
||||
|
||||
for (idx, field) in cloud.fields.iter().enumerate() {
|
||||
let lower_field_name = field.name.to_lowercase();
|
||||
match lower_field_name.as_str() {
|
||||
"x" => has_x = Some(idx),
|
||||
"y" => has_y = Some(idx),
|
||||
"z" => has_z = Some(idx),
|
||||
_ => {
|
||||
if lower_meta_names.contains(&lower_field_name) {
|
||||
let meta_idx = idx - DIM;
|
||||
debug_assert!(
|
||||
meta_idx < meta_with_offsets.len(),
|
||||
"Meta data length mismatch"
|
||||
);
|
||||
meta_with_offsets[meta_idx].0 = field.name.clone();
|
||||
meta_with_offsets[meta_idx].1 = field.datatype.try_into()?;
|
||||
meta_with_offsets[meta_idx].2 = field.offset as usize;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
|
||||
|
||||
debug_assert!(
|
||||
meta_with_offsets.len() == METADIM,
|
||||
"Meta data length mismatch"
|
||||
);
|
||||
|
||||
let mut meta_offsets = [usize::default(); METADIM];
|
||||
let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM];
|
||||
|
||||
meta_with_offsets
|
||||
.into_iter()
|
||||
.zip(meta.iter_mut())
|
||||
.zip(meta_offsets.iter_mut())
|
||||
.for_each(|(((name, datatype, offset), meta), meta_offset)| {
|
||||
*meta = (name, datatype);
|
||||
*meta_offset = offset;
|
||||
});
|
||||
|
||||
let x_field = check_coord(has_x, &cloud.fields, &xyz_field_type)?;
|
||||
let y_field = check_coord(has_y, &cloud.fields, &xyz_field_type)?;
|
||||
|
||||
let mut offsets = vec![x_field.offset as usize, y_field.offset as usize];
|
||||
|
||||
let z_field = check_coord(has_z, &cloud.fields, &xyz_field_type);
|
||||
match z_field {
|
||||
Ok(z_field) => {
|
||||
offsets.push(z_field.offset as usize);
|
||||
}
|
||||
Err(err) => match err {
|
||||
ConversionError::NotEnoughFields => {
|
||||
if DIM == 3 {
|
||||
return Err(ConversionError::NotEnoughFields);
|
||||
}
|
||||
}
|
||||
_ => return Err(err),
|
||||
},
|
||||
}
|
||||
|
||||
let endian = if cloud.is_bigendian {
|
||||
Endianness::Big
|
||||
} else {
|
||||
Endianness::Little
|
||||
};
|
||||
|
||||
if offsets.len() != DIM {
|
||||
return Err(ConversionError::NotEnoughFields);
|
||||
}
|
||||
|
||||
offsets.extend(meta_offsets);
|
||||
|
||||
if offsets.len() != DIM + METADIM {
|
||||
return Err(ConversionError::NotEnoughFields);
|
||||
}
|
||||
|
||||
let point_step_size = cloud.point_step as usize;
|
||||
let cloud_length = cloud.width as usize * cloud.height as usize;
|
||||
if point_step_size * cloud_length != cloud.data.len() {
|
||||
return Err(ConversionError::DataLengthMismatch);
|
||||
}
|
||||
|
||||
let last_offset = offsets.last().expect("Dimensionality is 0.");
|
||||
|
||||
if let Some(last_meta) = meta.last() {
|
||||
let size_with_last_meta = last_offset + last_meta.1.size();
|
||||
if size_with_last_meta > point_step_size {
|
||||
return Err(ConversionError::DataLengthMismatch);
|
||||
}
|
||||
} else if last_offset + xyz_field_type.size() > point_step_size {
|
||||
return Err(ConversionError::DataLengthMismatch);
|
||||
}
|
||||
|
||||
Ok(Self {
|
||||
iteration: 0,
|
||||
data: cloud.data,
|
||||
point_step_size,
|
||||
cloud_length: cloud.width as usize * cloud.height as usize,
|
||||
offsets,
|
||||
meta,
|
||||
endianness: endian,
|
||||
phantom_t: std::marker::PhantomData,
|
||||
phantom_c: std::marker::PhantomData,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
|
@ -15,7 +15,7 @@ pub struct HeaderMsg {
|
|||
pub frame_id: String,
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default)]
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct PointFieldMsg {
|
||||
pub name: String,
|
||||
pub offset: u32,
|
||||
|
|
@ -23,21 +23,19 @@ pub struct PointFieldMsg {
|
|||
pub count: u32,
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct PointCloud2Msg {
|
||||
pub header: HeaderMsg,
|
||||
pub height: u32,
|
||||
pub width: u32,
|
||||
pub fields: Vec<PointFieldMsg>,
|
||||
pub is_bigendian: bool,
|
||||
pub point_step: u32,
|
||||
pub row_step: u32,
|
||||
pub data: Vec<u8>,
|
||||
pub is_dense: bool,
|
||||
impl Default for PointFieldMsg {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
name: String::new(),
|
||||
offset: 0,
|
||||
datatype: 0,
|
||||
count: 1,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature = "r2r_msg")]
|
||||
impl From<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
|
||||
impl From<r2r::sensor_msgs::msg::PointCloud2> for crate::PointCloud2Msg {
|
||||
fn from(msg: r2r::sensor_msgs::msg::PointCloud2) -> Self {
|
||||
Self {
|
||||
header: HeaderMsg {
|
||||
|
|
@ -70,8 +68,8 @@ impl From<r2r::sensor_msgs::msg::PointCloud2> for PointCloud2Msg {
|
|||
}
|
||||
|
||||
#[cfg(feature = "r2r_msg")]
|
||||
impl From<PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
|
||||
fn from(msg: PointCloud2Msg) -> Self {
|
||||
impl From<crate::PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
|
||||
fn from(msg: crate::PointCloud2Msg) -> Self {
|
||||
r2r::sensor_msgs::msg::PointCloud2 {
|
||||
header: r2r::std_msgs::msg::Header {
|
||||
stamp: r2r::builtin_interfaces::msg::Time {
|
||||
|
|
@ -102,7 +100,7 @@ impl From<PointCloud2Msg> for r2r::sensor_msgs::msg::PointCloud2 {
|
|||
}
|
||||
|
||||
#[cfg(feature = "rosrust_msg")]
|
||||
impl From<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
|
||||
impl From<rosrust_msg::sensor_msgs::PointCloud2> for crate::PointCloud2Msg {
|
||||
fn from(msg: rosrust_msg::sensor_msgs::PointCloud2) -> Self {
|
||||
Self {
|
||||
header: HeaderMsg {
|
||||
|
|
@ -135,8 +133,8 @@ impl From<rosrust_msg::sensor_msgs::PointCloud2> for PointCloud2Msg {
|
|||
}
|
||||
|
||||
#[cfg(feature = "rosrust_msg")]
|
||||
impl From<PointCloud2Msg> for rosrust_msg::sensor_msgs::PointCloud2 {
|
||||
fn from(msg: PointCloud2Msg) -> Self {
|
||||
impl From<crate::PointCloud2Msg> for rosrust_msg::sensor_msgs::PointCloud2 {
|
||||
fn from(msg: crate::PointCloud2Msg) -> Self {
|
||||
rosrust_msg::sensor_msgs::PointCloud2 {
|
||||
header: rosrust_msg::std_msgs::Header {
|
||||
seq: msg.header.seq,
|
||||
|
|
|
|||
346
src/writer.rs
346
src/writer.rs
|
|
@ -1,346 +0,0 @@
|
|||
use crate::{
|
||||
pcl_utils::*,
|
||||
Point,
|
||||
PointCloud2Msg,
|
||||
PointConvertible,
|
||||
ConversionError,
|
||||
ros_types::PointFieldMsg,
|
||||
convert::{
|
||||
FromBytes,
|
||||
FieldDatatype,
|
||||
},
|
||||
};
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as f32. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterF32<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as f64. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterF64<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<f64, { std::mem::size_of::<f64>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as i8. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterI8<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<i8, { std::mem::size_of::<i8>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as i16. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterI16<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<i16, { std::mem::size_of::<i16>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as i32. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterI32<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<i32, { std::mem::size_of::<i32>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as u8. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterU8<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<u8, { std::mem::size_of::<u8>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as u16. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterU16<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<u16, { std::mem::size_of::<u16>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Convenience type for a Writer that reads coordinates as u32. Specify the number of dimensions, metadata dimensions and C, the point type.
|
||||
pub type WriterU32<const DIM: usize, const METADIM: usize, C> =
|
||||
Writer<u32, { std::mem::size_of::<u32>() }, DIM, METADIM, C>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`).
|
||||
pub type WriterXYZ = WriterF32<3, 0, PointXYZ>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`).
|
||||
pub type WriterXYZI = WriterF32<3, 1, PointXYZI>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`).
|
||||
pub type WriterXYZNormal = WriterF32<3, 3, PointXYZNormal>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`).
|
||||
pub type WriterXYZRGB = WriterF32<3, 1, PointXYZRGB>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`).
|
||||
pub type WriterXYZRGBL = WriterF32<3, 2, PointXYZRGBL>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels and alpha channel (see `PointXYZRGBA`).
|
||||
pub type WriterXYZRGBA = WriterF32<3, 2, PointXYZRGBA>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`).
|
||||
pub type WriterXYZRGBNormal = WriterF32<3, 4, PointXYZRGBNormal>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`).
|
||||
pub type WriterXYZINormal = WriterF32<3, 4, PointXYZINormal>;
|
||||
|
||||
/// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`).
|
||||
pub type WriterXYZL = WriterF32<3, 1, PointXYZL>;
|
||||
|
||||
/// The Write creates a PointCloud2Msg out of your point iterator.
|
||||
///
|
||||
/// The iterator is defined at compile time, so the Point has to be described via template arguments.
|
||||
///
|
||||
/// # Predefined Readers
|
||||
/// For the most common use cases, there are already predefined types. Examples are `ReaderXYZ` for xyz coordinates or `ReaderXYZI` for xyz coordinates and intensity.
|
||||
///
|
||||
/// When using within a ROS node, the PointCloud2 created by the ROS crate must be converted first.
|
||||
/// The cost of this operation is low, as it mostly moves ownership without iterating over the point data.
|
||||
///
|
||||
/// ROS1 with rosrust:
|
||||
/// let msg: rosrust_msg::sensor_msgs::PointCloud2; // inside the callback
|
||||
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
|
||||
///
|
||||
/// ROS2 with r2r:
|
||||
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
|
||||
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
|
||||
///
|
||||
/// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags.
|
||||
///
|
||||
/// ## Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
/// };
|
||||
///
|
||||
///
|
||||
/// let cloud_points: Vec<PointXYZ> = vec![
|
||||
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
|
||||
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
|
||||
/// ];
|
||||
///
|
||||
/// let msg: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
|
||||
/// // ^^^^^^^^^ creating PointCloud2Msg from an iterator
|
||||
/// ```
|
||||
/// # Fully Custom Writer
|
||||
/// When the predefined types are not enough (like sensor specific metadata), you can describe your Writer with the following template arguments:
|
||||
/// - T: The coordinate type, e.g. f32
|
||||
/// - SIZE: The size of the coordinate type in bytes, e.g. 4 for f32. Use the ros_pointcloud2::size_of! macro for this.
|
||||
/// - DIM: The dimensionality of the point, e.g. 3 for xyz coordinates.
|
||||
/// - METADIM: The number of additional meta data fields per point that are not for dimensionality. Intensity for example is a meta data field.
|
||||
/// Afterwards, implement the PointConvertible trait for your custom point type.
|
||||
///
|
||||
/// ## Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// reader::Reader, writer::Writer, PointConvertible, Point, size_of, MetaNames, PointMeta,
|
||||
/// };
|
||||
///
|
||||
/// type Xyz = f32; // coordinate type
|
||||
/// const XYZ_S: usize = size_of!(Xyz);
|
||||
/// const DIM: usize = 3; // helper constant for dimensionality
|
||||
/// const METADIM: usize = 1; // helper constant for meta data fields
|
||||
///
|
||||
/// #[derive(Debug, PartialEq, Clone)]
|
||||
/// struct CustomPoint {
|
||||
/// x: f32,
|
||||
/// y: f32,
|
||||
/// z: f32,
|
||||
/// i: u8,
|
||||
/// }
|
||||
///
|
||||
/// impl From<Point<f32, 3, 1>> for CustomPoint {
|
||||
/// fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
/// Self {
|
||||
/// x: point.coords[0],
|
||||
/// y: point.coords[1],
|
||||
/// z: point.coords[2],
|
||||
/// i: point.meta[0].get(),
|
||||
/// }
|
||||
/// }
|
||||
/// }
|
||||
///
|
||||
///impl From<CustomPoint> for Point<f32, 3, 1> {
|
||||
/// fn from(point: CustomPoint) -> Self {
|
||||
/// Point {
|
||||
/// coords: [point.x, point.y, point.z],
|
||||
/// meta: [
|
||||
/// point.i.into(),
|
||||
/// ],
|
||||
/// }
|
||||
/// }
|
||||
///}
|
||||
///
|
||||
/// impl MetaNames<METADIM> for CustomPoint {
|
||||
/// fn meta_names() -> [&'static str; METADIM] {
|
||||
/// ["intensity"]
|
||||
/// }
|
||||
/// }
|
||||
/// impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
|
||||
///
|
||||
/// type MyReader = Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
|
||||
/// type MyWriter = Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
|
||||
/// // ^^^^^^^^ your custom Writer
|
||||
/// ```
|
||||
pub struct Writer<T: FromBytes, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
|
||||
where
|
||||
T: FromBytes,
|
||||
C: PointConvertible<T, SIZE, DIM, METADIM>,
|
||||
{
|
||||
coordinates: Box<dyn Iterator<Item = C>>,
|
||||
phantom_t: std::marker::PhantomData<T>,
|
||||
}
|
||||
|
||||
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C> TryInto<PointCloud2Msg>
|
||||
for Writer<T, SIZE, DIM, METADIM, C>
|
||||
where
|
||||
T: FromBytes,
|
||||
C: PointConvertible<T, SIZE, DIM, METADIM>,
|
||||
{
|
||||
type Error = ConversionError;
|
||||
|
||||
/// Writes the points to a PointCloud2Msg.
|
||||
///
|
||||
/// First use the `from` method for initializing the Writer.
|
||||
/// Then use the `try_into` method to do the actual conversion.
|
||||
///
|
||||
/// The operation is O(n) in time complexity where n is the number of points in the point cloud.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
/// };
|
||||
///
|
||||
/// let cloud_points: Vec<PointXYZ> = vec![
|
||||
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
|
||||
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
|
||||
/// ];
|
||||
/// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
|
||||
/// // ^^^^^^^^ ROS message conversion
|
||||
/// ```
|
||||
fn try_into(mut self) -> Result<PointCloud2Msg, Self::Error> {
|
||||
if DIM > 3 {
|
||||
return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time?
|
||||
}
|
||||
|
||||
let mut fields = Vec::with_capacity(METADIM + DIM); // TODO check if we can preallocate the size on the stack
|
||||
|
||||
if DIM > 1 {
|
||||
fields.push(PointFieldMsg {
|
||||
name: "x".into(),
|
||||
offset: 0,
|
||||
datatype: T::field_datatype().into(),
|
||||
count: 1,
|
||||
});
|
||||
|
||||
fields.push(PointFieldMsg {
|
||||
name: "y".into(),
|
||||
offset: SIZE as u32,
|
||||
datatype: T::field_datatype().into(),
|
||||
count: 1,
|
||||
});
|
||||
}
|
||||
|
||||
if DIM == 3 {
|
||||
fields.push(PointFieldMsg {
|
||||
name: "z".into(),
|
||||
offset: 2 * SIZE as u32,
|
||||
datatype: T::field_datatype().into(),
|
||||
count: 1,
|
||||
});
|
||||
}
|
||||
|
||||
let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?;
|
||||
let point: Point<T, DIM, METADIM> = first_point.clone().into();
|
||||
let meta_names = C::meta_names();
|
||||
|
||||
let mut meta_offsets_acc = DIM as u32 * SIZE as u32;
|
||||
for (meta_value, meta_name) in point.meta.into_iter().zip(meta_names.into_iter()) {
|
||||
let datatype_code = meta_value.datatype.into();
|
||||
if FieldDatatype::try_from(datatype_code).is_err() {
|
||||
return Err(ConversionError::UnsupportedFieldType);
|
||||
}
|
||||
|
||||
fields.push(PointFieldMsg {
|
||||
name: meta_name.into(),
|
||||
offset: meta_offsets_acc,
|
||||
datatype: datatype_code,
|
||||
count: 1,
|
||||
});
|
||||
meta_offsets_acc += meta_value.datatype.size() as u32
|
||||
}
|
||||
|
||||
let mut cloud = PointCloud2Msg {
|
||||
point_step: fields.iter().fold(Default::default(), |acc, field| {
|
||||
let field_type: FieldDatatype = field
|
||||
.datatype
|
||||
.try_into()
|
||||
.expect("Unsupported field but checked before.");
|
||||
let field_size = field_type.size();
|
||||
acc + field.count * field_size as u32
|
||||
}),
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
// actual point -> byte conversion -- O(n)
|
||||
add_point_to_byte_buffer(first_point, &mut cloud)?;
|
||||
for coords in self.coordinates {
|
||||
add_point_to_byte_buffer(coords, &mut cloud)?;
|
||||
}
|
||||
|
||||
cloud.fields = fields;
|
||||
cloud.height = 1; // everything is in one row (unstructured)
|
||||
cloud.is_bigendian = false; // ROS default
|
||||
cloud.is_dense = true; // ROS default
|
||||
cloud.row_step = cloud.width * cloud.point_step; // Note: redundant but defined in PointCloud2 message
|
||||
|
||||
Ok(cloud)
|
||||
}
|
||||
}
|
||||
|
||||
impl<T, const SIZE: usize, const DIM: usize, const METADIM: usize, C>
|
||||
Writer<T, SIZE, DIM, METADIM, C>
|
||||
where
|
||||
T: FromBytes,
|
||||
C: PointConvertible<T, SIZE, DIM, METADIM>,
|
||||
{
|
||||
/// Create a Writer from any iterator that iterates over a template-defined point to a ROS message type.
|
||||
/// First use the `from` method for initializing the Writer.
|
||||
/// Then use the `try_into` method to do the actual conversion.
|
||||
///
|
||||
/// The operation is O(n) in time complexity where n is the number of points in the point cloud.
|
||||
///
|
||||
/// # Example
|
||||
/// ```
|
||||
/// use ros_pointcloud2::{
|
||||
/// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
/// };
|
||||
///
|
||||
/// let cloud_points: Vec<PointXYZ> = vec![
|
||||
/// PointXYZ { x: 1.0, y: 2.0, z: 3.0 },
|
||||
/// PointXYZ { x: 4.0, y: 5.0, z: 6.0 },
|
||||
/// ];
|
||||
// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap();
|
||||
/// // ^^^^ Writer creation
|
||||
/// ```
|
||||
pub fn from(coordinates: impl IntoIterator<Item = C> + 'static) -> Self {
|
||||
Self {
|
||||
coordinates: Box::new(coordinates.into_iter()),
|
||||
phantom_t: std::marker::PhantomData,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn add_point_to_byte_buffer<
|
||||
T: FromBytes,
|
||||
const SIZE: usize,
|
||||
const DIM: usize,
|
||||
const METADIM: usize,
|
||||
C: PointConvertible<T, SIZE, DIM, METADIM>,
|
||||
>(
|
||||
coords: C,
|
||||
cloud: &mut PointCloud2Msg,
|
||||
) -> Result<bool, ConversionError> {
|
||||
let point: Point<T, DIM, METADIM> = coords.into();
|
||||
|
||||
// (x, y, z...)
|
||||
point
|
||||
.coords
|
||||
.iter()
|
||||
.for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice()));
|
||||
|
||||
// meta data description
|
||||
point.meta.iter().for_each(|meta| {
|
||||
let truncated_bytes = &meta.bytes[0..meta.datatype.size()];
|
||||
cloud.data.extend_from_slice(truncated_bytes);
|
||||
});
|
||||
|
||||
cloud.width += 1;
|
||||
|
||||
Ok(true)
|
||||
}
|
||||
|
|
@ -1,80 +1,92 @@
|
|||
use ros_pointcloud2::pcl_utils::*;
|
||||
use ros_pointcloud2::reader::*;
|
||||
use ros_pointcloud2::ros_types::PointCloud2Msg;
|
||||
use ros_pointcloud2::writer::*;
|
||||
use ros_pointcloud2::*;
|
||||
use ros_pointcloud2::prelude::*;
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
use std::fmt::Debug;
|
||||
|
||||
macro_rules! convert_from_into {
|
||||
($reader:ty, $writer:ty, $point:ty, $cloud:expr) => {
|
||||
convert_from_into_in_out_cloud!(
|
||||
$reader,
|
||||
$writer,
|
||||
$point,
|
||||
$cloud.clone(),
|
||||
$point,
|
||||
$cloud,
|
||||
$point
|
||||
);
|
||||
($point:ty, $cloud:expr) => {
|
||||
convert_from_into_in_out_cloud!($cloud, $point, $cloud, $point);
|
||||
};
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
macro_rules! convert_from_into_vec {
|
||||
($point:ty, $cloud:expr) => {
|
||||
convert_from_into_in_out_cloud_vec!($cloud, $point, $cloud, $point);
|
||||
};
|
||||
}
|
||||
|
||||
macro_rules! convert_from_into_in_out_cloud {
|
||||
($reader:ty, $writer:ty, $point:ty, $in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
|
||||
let msg: Result<PointCloud2Msg, _> = <$writer>::from($in_cloud).try_into();
|
||||
assert!(msg.is_ok());
|
||||
let to_p_type = <$reader>::try_from(msg.unwrap());
|
||||
($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
|
||||
let msg = PointCloud2Msg::try_from_iter($in_cloud.clone().into_iter());
|
||||
assert!(msg.is_ok(), "{:?}", msg);
|
||||
let msg = msg.unwrap();
|
||||
let to_p_type = msg.try_into_iter();
|
||||
assert!(to_p_type.is_ok());
|
||||
let to_p_type = to_p_type.unwrap();
|
||||
let back_to_type = to_p_type.collect::<Vec<$out_point>>();
|
||||
assert_eq!($out_cloud, back_to_type);
|
||||
let orig_cloud: Vec<$out_point> = $out_cloud.iter().cloned().collect();
|
||||
assert_eq!(orig_cloud, back_to_type);
|
||||
};
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
macro_rules! convert_from_into_in_out_cloud_vec {
|
||||
($in_cloud:expr, $in_point:ty, $out_cloud:expr, $out_point:ty) => {
|
||||
let msg = PointCloud2Msg::try_from_vec($in_cloud.clone());
|
||||
assert!(msg.is_ok(), "{:?}", msg);
|
||||
let msg = msg.unwrap();
|
||||
let to_p_type = msg.try_into_iter();
|
||||
assert!(to_p_type.is_ok());
|
||||
let to_p_type = to_p_type.unwrap();
|
||||
let back_to_type = to_p_type.collect::<Vec<$out_point>>();
|
||||
let orig_cloud: Vec<$out_point> = $out_cloud.iter().cloned().collect();
|
||||
assert_eq!(orig_cloud, back_to_type);
|
||||
};
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn custom_xyz_f32() {
|
||||
const DIM: usize = 3;
|
||||
const METADIM: usize = 0;
|
||||
fn write_cloud() {
|
||||
let cloud = vec![
|
||||
PointXYZ::new(0.0, 1.0, 5.0),
|
||||
PointXYZ::new(1.0, 1.5, 5.0),
|
||||
PointXYZ::new(1.3, 1.6, 5.7),
|
||||
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
||||
];
|
||||
|
||||
#[derive(Debug, PartialEq, Clone)]
|
||||
let msg = PointCloud2Msg::try_from_iter(cloud);
|
||||
assert!(msg.is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn write_cloud_from_vec() {
|
||||
let cloud = vec![
|
||||
PointXYZ::new(0.0, 1.0, 5.0),
|
||||
PointXYZ::new(1.0, 1.5, 5.0),
|
||||
PointXYZ::new(1.3, 1.6, 5.7),
|
||||
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
||||
];
|
||||
|
||||
let msg = PointCloud2Msg::try_from_vec(cloud);
|
||||
assert!(msg.is_ok());
|
||||
|
||||
let msg = msg.unwrap();
|
||||
println!("{:?}", msg);
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn custom_xyz_f32() {
|
||||
#[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
|
||||
#[repr(C)]
|
||||
struct CustomPoint {
|
||||
x: f32,
|
||||
y: f32,
|
||||
z: f32,
|
||||
}
|
||||
|
||||
type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
|
||||
type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
|
||||
|
||||
impl From<Point<f32, 3, 0>> for CustomPoint {
|
||||
fn from(point: Point<f32, 3, 0>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<CustomPoint> for Point<f32, 3, 0> {
|
||||
fn from(point: CustomPoint) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<METADIM> for CustomPoint {
|
||||
fn meta_names() -> [&'static str; METADIM] {
|
||||
[]
|
||||
}
|
||||
}
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
|
||||
|
||||
convert_from_into!(
|
||||
MyReader,
|
||||
MyWriter,
|
||||
CustomPoint,
|
||||
vec![
|
||||
CustomPoint {
|
||||
|
|
@ -97,84 +109,52 @@ fn custom_xyz_f32() {
|
|||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn custom_xyzi_f32() {
|
||||
type Xyz = f32;
|
||||
const XYZ_S: usize = std::mem::size_of::<Xyz>();
|
||||
const DIM: usize = 3;
|
||||
const METADIM: usize = 1;
|
||||
#[derive(Debug, PartialEq, Clone)]
|
||||
struct CustomPoint {
|
||||
x: f32,
|
||||
y: f32,
|
||||
z: f32,
|
||||
i: u8,
|
||||
}
|
||||
|
||||
impl From<Point<f32, 3, 1>> for CustomPoint {
|
||||
fn from(point: Point<f32, 3, 1>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
i: point.meta[0].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<CustomPoint> for Point<f32, 3, 1> {
|
||||
fn from(point: CustomPoint) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [point.i.into()],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<METADIM> for CustomPoint {
|
||||
fn meta_names() -> [&'static str; METADIM] {
|
||||
["intensity"]
|
||||
}
|
||||
}
|
||||
|
||||
type MyReader = reader::Reader<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
|
||||
type MyWriter = writer::Writer<Xyz, XYZ_S, DIM, METADIM, CustomPoint>;
|
||||
|
||||
impl PointConvertible<Xyz, XYZ_S, DIM, METADIM> for CustomPoint {}
|
||||
|
||||
let cloud = vec![
|
||||
CustomPoint {
|
||||
let cloud: Vec<CustomPointXYZI> = vec![
|
||||
CustomPointXYZI {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
i: 0,
|
||||
},
|
||||
CustomPoint {
|
||||
CustomPointXYZI {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
i: 1,
|
||||
},
|
||||
CustomPoint {
|
||||
CustomPointXYZI {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
i: 2,
|
||||
},
|
||||
CustomPoint {
|
||||
CustomPointXYZI {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
i: u8::MAX,
|
||||
},
|
||||
];
|
||||
convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
|
||||
|
||||
#[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
|
||||
#[repr(C)]
|
||||
struct CustomPointXYZI {
|
||||
x: f32,
|
||||
y: f32,
|
||||
z: f32,
|
||||
i: u8,
|
||||
}
|
||||
|
||||
convert_from_into!(CustomPointXYZI, cloud);
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn custom_rgba_f32() {
|
||||
const DIM: usize = 3;
|
||||
const METADIM: usize = 4;
|
||||
#[derive(Debug, PartialEq, Clone)]
|
||||
#[derive(Debug, PartialEq, Clone, Default, RosFull, TypeLayout)]
|
||||
#[repr(C)]
|
||||
struct CustomPoint {
|
||||
x: f32,
|
||||
y: f32,
|
||||
|
|
@ -184,43 +164,7 @@ fn custom_rgba_f32() {
|
|||
b: u8,
|
||||
a: u8,
|
||||
}
|
||||
type MyReader = reader::Reader<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
|
||||
type MyWriter = writer::Writer<f32, { std::mem::size_of::<f32>() }, DIM, METADIM, CustomPoint>;
|
||||
|
||||
impl From<Point<f32, 3, 4>> for CustomPoint {
|
||||
fn from(point: Point<f32, 3, 4>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
r: point.meta[0].get(),
|
||||
g: point.meta[1].get(),
|
||||
b: point.meta[2].get(),
|
||||
a: point.meta[3].get(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<CustomPoint> for Point<f32, 3, 4> {
|
||||
fn from(point: CustomPoint) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [
|
||||
point.r.into(),
|
||||
point.g.into(),
|
||||
point.b.into(),
|
||||
point.a.into(),
|
||||
],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<METADIM> for CustomPoint {
|
||||
fn meta_names() -> [&'static str; METADIM] {
|
||||
["r", "g", "b", "a"]
|
||||
}
|
||||
}
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, DIM, METADIM> for CustomPoint {}
|
||||
let cloud = vec![
|
||||
CustomPoint {
|
||||
x: 0.0,
|
||||
|
|
@ -259,80 +203,38 @@ fn custom_rgba_f32() {
|
|||
a: u8::MAX,
|
||||
},
|
||||
];
|
||||
convert_from_into!(MyReader, MyWriter, CustomPoint, cloud);
|
||||
convert_from_into!(CustomPoint, cloud);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn converterxyz() {
|
||||
let cloud = vec![
|
||||
PointXYZ {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
},
|
||||
PointXYZ {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
},
|
||||
PointXYZ {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
},
|
||||
PointXYZ {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
},
|
||||
PointXYZ::new(0.0, 1.0, 5.0),
|
||||
PointXYZ::new(1.0, 1.5, 5.0),
|
||||
PointXYZ::new(1.3, 1.6, 5.7),
|
||||
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
||||
];
|
||||
|
||||
convert_from_into!(ReaderXYZ, WriterXYZ, PointXYZ, cloud);
|
||||
convert_from_into!(PointXYZ, cloud);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn converterxyzrgba() {
|
||||
convert_from_into!(
|
||||
ReaderXYZRGBA,
|
||||
WriterXYZRGBA,
|
||||
PointXYZRGBA,
|
||||
vec![
|
||||
PointXYZRGBA {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
r: 0,
|
||||
g: 0,
|
||||
b: 0,
|
||||
a: 0,
|
||||
},
|
||||
PointXYZRGBA {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
r: 1,
|
||||
g: 1,
|
||||
b: 1,
|
||||
a: 1,
|
||||
},
|
||||
PointXYZRGBA {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
r: 2,
|
||||
g: 2,
|
||||
b: 2,
|
||||
a: 2,
|
||||
},
|
||||
PointXYZRGBA {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
r: u8::MAX,
|
||||
g: u8::MAX,
|
||||
b: u8::MAX,
|
||||
a: u8::MAX,
|
||||
},
|
||||
PointXYZRGBA::new(0.0, 1.0, 5.0, 0, 0, 0, 0),
|
||||
PointXYZRGBA::new(1.0, 1.5, 5.0, 1, 1, 1, 1),
|
||||
PointXYZRGBA::new(1.3, 1.6, 5.7, 2, 2, 2, 2),
|
||||
PointXYZRGBA::new(
|
||||
f32::MAX,
|
||||
f32::MIN,
|
||||
f32::MAX,
|
||||
u8::MAX,
|
||||
u8::MAX,
|
||||
u8::MAX,
|
||||
u8::MAX
|
||||
),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -340,46 +242,11 @@ fn converterxyzrgba() {
|
|||
#[test]
|
||||
fn converterxyzinormal() {
|
||||
convert_from_into!(
|
||||
ReaderXYZINormal,
|
||||
WriterXYZINormal,
|
||||
PointXYZINormal,
|
||||
vec![
|
||||
PointXYZINormal {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
intensity: 0.0,
|
||||
normal_x: 0.0,
|
||||
normal_y: 0.0,
|
||||
normal_z: 0.0,
|
||||
},
|
||||
PointXYZINormal {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
intensity: 1.0,
|
||||
normal_x: 1.0,
|
||||
normal_y: 1.0,
|
||||
normal_z: 1.0,
|
||||
},
|
||||
PointXYZINormal {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
intensity: 2.0,
|
||||
normal_x: 2.0,
|
||||
normal_y: 2.0,
|
||||
normal_z: 2.0,
|
||||
},
|
||||
PointXYZINormal {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
intensity: f32::MAX,
|
||||
normal_x: f32::MAX,
|
||||
normal_y: f32::MAX,
|
||||
normal_z: f32::MAX,
|
||||
},
|
||||
PointXYZINormal::new(0.0, 1.0, 5.0, 0.0, 0.0, 0.0, 0.0),
|
||||
PointXYZINormal::new(1.0, 1.5, 5.0, 1.0, 1.0, 1.0, 1.0),
|
||||
PointXYZINormal::new(1.3, 1.6, 5.7, 2.0, 2.0, 2.0, 2.0),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -387,54 +254,20 @@ fn converterxyzinormal() {
|
|||
#[test]
|
||||
fn converterxyzrgbnormal() {
|
||||
convert_from_into!(
|
||||
ReaderXYZRGBNormal,
|
||||
WriterXYZRGBNormal,
|
||||
PointXYZRGBNormal,
|
||||
vec![
|
||||
PointXYZRGBNormal {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
r: 0,
|
||||
g: 0,
|
||||
b: 0,
|
||||
normal_x: 0.0,
|
||||
normal_y: 0.0,
|
||||
normal_z: 0.0,
|
||||
},
|
||||
PointXYZRGBNormal {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
r: 1,
|
||||
g: 1,
|
||||
b: 1,
|
||||
normal_x: 1.0,
|
||||
normal_y: 1.0,
|
||||
normal_z: 1.0,
|
||||
},
|
||||
PointXYZRGBNormal {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
r: 2,
|
||||
g: 2,
|
||||
b: 2,
|
||||
normal_x: 2.0,
|
||||
normal_y: 2.0,
|
||||
normal_z: 2.0,
|
||||
},
|
||||
PointXYZRGBNormal {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
r: u8::MAX,
|
||||
g: u8::MAX,
|
||||
b: u8::MAX,
|
||||
normal_x: f32::MAX,
|
||||
normal_y: f32::MAX,
|
||||
normal_z: f32::MAX,
|
||||
},
|
||||
PointXYZRGBNormal::new(0.0, 1.0, 5.0, RGB::new(0, 0, 0), 0.0, 0.0, 0.0),
|
||||
PointXYZRGBNormal::new(1.0, 1.5, 5.0, RGB::new(1, 1, 1), 1.0, 1.0, 1.0),
|
||||
PointXYZRGBNormal::new(1.3, 1.6, 5.7, RGB::new(2, 2, 2), 2.0, 2.0, 2.0),
|
||||
PointXYZRGBNormal::new(
|
||||
f32::MAX,
|
||||
f32::MIN,
|
||||
f32::MAX,
|
||||
RGB::new(u8::MAX, u8::MAX, u8::MAX),
|
||||
f32::MAX,
|
||||
f32::MAX,
|
||||
f32::MAX,
|
||||
),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -442,42 +275,12 @@ fn converterxyzrgbnormal() {
|
|||
#[test]
|
||||
fn converterxyznormal() {
|
||||
convert_from_into!(
|
||||
ReaderXYZNormal,
|
||||
WriterXYZNormal,
|
||||
PointXYZNormal,
|
||||
vec![
|
||||
PointXYZNormal {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
normal_x: 0.0,
|
||||
normal_y: 0.0,
|
||||
normal_z: 0.0,
|
||||
},
|
||||
PointXYZNormal {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
normal_x: 1.0,
|
||||
normal_y: 1.0,
|
||||
normal_z: 1.0,
|
||||
},
|
||||
PointXYZNormal {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
normal_x: 2.0,
|
||||
normal_y: 2.0,
|
||||
normal_z: 2.0,
|
||||
},
|
||||
PointXYZNormal {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
normal_x: f32::MAX,
|
||||
normal_y: f32::MAX,
|
||||
normal_z: f32::MAX,
|
||||
},
|
||||
PointXYZNormal::new(0.0, 1.0, 5.0, 0.0, 0.0, 0.0),
|
||||
PointXYZNormal::new(1.0, 1.5, 5.0, 1.0, 1.0, 1.0),
|
||||
PointXYZNormal::new(1.3, 1.6, 5.7, 2.0, 2.0, 2.0),
|
||||
PointXYZNormal::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX, f32::MAX, f32::MAX),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -485,46 +288,20 @@ fn converterxyznormal() {
|
|||
#[test]
|
||||
fn converterxyzrgbl() {
|
||||
convert_from_into!(
|
||||
ReaderXYZRGBL,
|
||||
WriterXYZRGBL,
|
||||
PointXYZRGBL,
|
||||
vec![
|
||||
PointXYZRGBL {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
r: 0,
|
||||
g: 0,
|
||||
b: 0,
|
||||
label: 0,
|
||||
},
|
||||
PointXYZRGBL {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
r: 1,
|
||||
g: 1,
|
||||
b: 1,
|
||||
label: 1,
|
||||
},
|
||||
PointXYZRGBL {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
r: 2,
|
||||
g: 2,
|
||||
b: 2,
|
||||
label: 2,
|
||||
},
|
||||
PointXYZRGBL {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
r: u8::MAX,
|
||||
g: u8::MAX,
|
||||
b: u8::MAX,
|
||||
label: u32::MAX,
|
||||
},
|
||||
PointXYZRGBL::new(0.0, 1.0, 5.0, 0, 0, 0, 0),
|
||||
PointXYZRGBL::new(1.0, 1.5, 5.0, 1, 1, 1, 1),
|
||||
PointXYZRGBL::new(1.3, 1.6, 5.7, 2, 2, 2, 2),
|
||||
PointXYZRGBL::new(
|
||||
f32::MAX,
|
||||
f32::MIN,
|
||||
f32::MAX,
|
||||
u8::MAX,
|
||||
u8::MAX,
|
||||
u8::MAX,
|
||||
u32::MAX
|
||||
),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -532,42 +309,25 @@ fn converterxyzrgbl() {
|
|||
#[test]
|
||||
fn converterxyzrgb() {
|
||||
convert_from_into!(
|
||||
ReaderXYZRGB,
|
||||
WriterXYZRGB,
|
||||
PointXYZRGB,
|
||||
vec![
|
||||
PointXYZRGB {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
r: 0,
|
||||
g: 0,
|
||||
b: 0,
|
||||
},
|
||||
PointXYZRGB {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
r: 1,
|
||||
g: 1,
|
||||
b: 1,
|
||||
},
|
||||
PointXYZRGB {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
r: 2,
|
||||
g: 2,
|
||||
b: 2,
|
||||
},
|
||||
PointXYZRGB {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
r: u8::MAX,
|
||||
g: u8::MAX,
|
||||
b: u8::MAX,
|
||||
},
|
||||
PointXYZRGB::new(0.0, 1.0, 5.0, 0, 0, 0),
|
||||
PointXYZRGB::new(1.0, 1.5, 5.0, 1, 1, 1),
|
||||
PointXYZRGB::new(1.3, 1.6, 5.7, 2, 2, 2),
|
||||
PointXYZRGB::new(f32::MAX, f32::MIN, f32::MAX, u8::MAX, u8::MAX, u8::MAX),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[cfg(feature = "derive")]
|
||||
fn converterxyzrgb_from_vec() {
|
||||
convert_from_into_vec!(
|
||||
PointXYZRGB,
|
||||
vec![
|
||||
PointXYZRGB::new(0.0, 1.0, 5.0, 0, 0, 0),
|
||||
PointXYZRGB::new(1.3, 1.6, 5.7, 2, 2, 2),
|
||||
PointXYZRGB::new(f32::MAX, f32::MIN, f32::MAX, u8::MAX, u8::MAX, u8::MAX),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -575,34 +335,12 @@ fn converterxyzrgb() {
|
|||
#[test]
|
||||
fn converterxyzl() {
|
||||
convert_from_into!(
|
||||
ReaderXYZL,
|
||||
WriterXYZL,
|
||||
PointXYZL,
|
||||
vec![
|
||||
PointXYZL {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
label: 0,
|
||||
},
|
||||
PointXYZL {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
label: 1,
|
||||
},
|
||||
PointXYZL {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
label: 2,
|
||||
},
|
||||
PointXYZL {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
label: u32::MAX,
|
||||
},
|
||||
PointXYZL::new(0.0, 1.0, 5.0, 0),
|
||||
PointXYZL::new(1.0, 1.5, 5.0, 1),
|
||||
PointXYZL::new(1.3, 1.6, 5.7, 2),
|
||||
PointXYZL::new(f32::MAX, f32::MIN, f32::MAX, u32::MAX),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -610,34 +348,12 @@ fn converterxyzl() {
|
|||
#[test]
|
||||
fn converterxyzi() {
|
||||
convert_from_into!(
|
||||
ReaderXYZI,
|
||||
WriterXYZI,
|
||||
PointXYZI,
|
||||
vec![
|
||||
PointXYZI {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
intensity: 0.0,
|
||||
},
|
||||
PointXYZI {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
intensity: 1.0,
|
||||
},
|
||||
PointXYZI {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
intensity: 2.0,
|
||||
},
|
||||
PointXYZI {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
intensity: f32::MAX,
|
||||
},
|
||||
PointXYZI::new(0.0, 1.0, 5.0, 0.0),
|
||||
PointXYZI::new(1.0, 1.5, 5.0, 1.0),
|
||||
PointXYZI::new(1.3, 1.6, 5.7, 2.0),
|
||||
PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
|
||||
]
|
||||
);
|
||||
}
|
||||
|
|
@ -645,72 +361,27 @@ fn converterxyzi() {
|
|||
#[test]
|
||||
fn write_xyzi_read_xyz() {
|
||||
let write_cloud = vec![
|
||||
PointXYZI {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
intensity: 0.0,
|
||||
},
|
||||
PointXYZI {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
intensity: 1.0,
|
||||
},
|
||||
PointXYZI {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
intensity: 2.0,
|
||||
},
|
||||
PointXYZI {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
intensity: f32::MAX,
|
||||
},
|
||||
PointXYZI::new(0.0, 1.0, 5.0, 0.0),
|
||||
PointXYZI::new(1.0, 1.5, 5.0, 1.0),
|
||||
PointXYZI::new(1.3, 1.6, 5.7, 2.0),
|
||||
PointXYZI::new(f32::MAX, f32::MIN, f32::MAX, f32::MAX),
|
||||
];
|
||||
|
||||
let read_cloud = vec![
|
||||
PointXYZ {
|
||||
x: 0.0,
|
||||
y: 1.0,
|
||||
z: 5.0,
|
||||
},
|
||||
PointXYZ {
|
||||
x: 1.0,
|
||||
y: 1.5,
|
||||
z: 5.0,
|
||||
},
|
||||
PointXYZ {
|
||||
x: 1.3,
|
||||
y: 1.6,
|
||||
z: 5.7,
|
||||
},
|
||||
PointXYZ {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
},
|
||||
let read_cloud = [
|
||||
PointXYZ::new(0.0, 1.0, 5.0),
|
||||
PointXYZ::new(1.0, 1.5, 5.0),
|
||||
PointXYZ::new(1.3, 1.6, 5.7),
|
||||
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
|
||||
];
|
||||
|
||||
convert_from_into_in_out_cloud!(
|
||||
ReaderXYZ,
|
||||
WriterXYZI,
|
||||
PointXYZI,
|
||||
write_cloud,
|
||||
PointXYZI,
|
||||
read_cloud,
|
||||
PointXYZ
|
||||
);
|
||||
convert_from_into_in_out_cloud!(write_cloud, PointXYZI, read_cloud, PointXYZ);
|
||||
}
|
||||
|
||||
#[cfg(feature = "derive")]
|
||||
#[test]
|
||||
fn write_less_than_available() {
|
||||
const DIM: usize = 3;
|
||||
const METADIM: usize = 0;
|
||||
|
||||
#[derive(Debug, PartialEq, Clone)]
|
||||
#[derive(Debug, PartialEq, Clone, Default, TypeLayout)]
|
||||
#[repr(C)]
|
||||
struct CustomPoint {
|
||||
x: f32,
|
||||
y: f32,
|
||||
|
|
@ -718,35 +389,30 @@ fn write_less_than_available() {
|
|||
dummy: f32,
|
||||
}
|
||||
|
||||
type MyReader = ReaderF32<DIM, METADIM, CustomPoint>;
|
||||
type MyWriter = WriterF32<DIM, METADIM, CustomPoint>;
|
||||
|
||||
impl From<Point<f32, 3, 0>> for CustomPoint {
|
||||
fn from(point: Point<f32, 3, 0>) -> Self {
|
||||
impl From<RPCL2Point<3>> for CustomPoint {
|
||||
fn from(point: RPCL2Point<3>) -> Self {
|
||||
Self {
|
||||
x: point.coords[0],
|
||||
y: point.coords[1],
|
||||
z: point.coords[2],
|
||||
x: point.fields[0].get(),
|
||||
y: point.fields[1].get(),
|
||||
z: point.fields[2].get(),
|
||||
dummy: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<CustomPoint> for Point<f32, 3, 0> {
|
||||
impl From<CustomPoint> for RPCL2Point<3> {
|
||||
fn from(point: CustomPoint) -> Self {
|
||||
Point {
|
||||
coords: [point.x, point.y, point.z],
|
||||
meta: [],
|
||||
}
|
||||
[point.x.into(), point.y.into(), point.z.into()].into()
|
||||
}
|
||||
}
|
||||
|
||||
impl MetaNames<METADIM> for CustomPoint {
|
||||
fn meta_names() -> [&'static str; METADIM] {
|
||||
[]
|
||||
impl Fields<3> for CustomPoint {
|
||||
fn field_names_ordered() -> [&'static str; 3] {
|
||||
["x", "y", "z"]
|
||||
}
|
||||
}
|
||||
impl PointConvertible<f32, { std::mem::size_of::<f32>() }, 3, 0> for CustomPoint {}
|
||||
|
||||
impl PointConvertible<3> for CustomPoint {}
|
||||
|
||||
let write_cloud = vec![
|
||||
CustomPoint {
|
||||
|
|
@ -769,7 +435,7 @@ fn write_less_than_available() {
|
|||
},
|
||||
];
|
||||
|
||||
let read_cloud = vec![
|
||||
let read_cloud = [
|
||||
CustomPoint {
|
||||
x: 1.0,
|
||||
y: 2.0,
|
||||
|
|
@ -790,62 +456,5 @@ fn write_less_than_available() {
|
|||
},
|
||||
];
|
||||
|
||||
convert_from_into_in_out_cloud!(
|
||||
MyReader,
|
||||
MyWriter,
|
||||
CustomPoint,
|
||||
write_cloud,
|
||||
CustomPoint,
|
||||
read_cloud,
|
||||
CustomPoint
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn readme() {
|
||||
use ros_pointcloud2::{
|
||||
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
};
|
||||
|
||||
// Your points (here using the predefined type PointXYZ).
|
||||
let cloud_points = vec![
|
||||
PointXYZ {
|
||||
x: 1337.0,
|
||||
y: 42.0,
|
||||
z: 69.0,
|
||||
},
|
||||
PointXYZ {
|
||||
x: f32::MAX,
|
||||
y: f32::MIN,
|
||||
z: f32::MAX,
|
||||
},
|
||||
];
|
||||
|
||||
// For equality test later
|
||||
let cloud_copy = cloud_points.clone();
|
||||
|
||||
// Vector -> Writer -> Message
|
||||
let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points)
|
||||
.try_into() // iterating points here O(n)
|
||||
.unwrap();
|
||||
|
||||
// Convert to your ROS crate message type, we will use r2r here.
|
||||
// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
|
||||
|
||||
// Publish ...
|
||||
|
||||
// ... now incoming from a topic.
|
||||
// let internal_msg: PointCloud2Msg = msg.into();
|
||||
|
||||
// Message -> Reader. The Reader implements the Iterator trait.
|
||||
let reader = ReaderXYZ::try_from(internal_msg).unwrap();
|
||||
let new_cloud_points = reader
|
||||
.map(|point: PointXYZ| {
|
||||
// Some logic here
|
||||
|
||||
point
|
||||
})
|
||||
.collect::<Vec<PointXYZ>>();
|
||||
|
||||
assert_eq!(new_cloud_points, cloud_copy);
|
||||
convert_from_into_in_out_cloud!(write_cloud, CustomPoint, read_cloud, CustomPoint);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,9 +1,7 @@
|
|||
#[cfg(feature = "r2r_msg")]
|
||||
#[test]
|
||||
fn convertxyz_r2r_msg() {
|
||||
use ros_pointcloud2::{
|
||||
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
};
|
||||
use ros_pointcloud2::{points::PointXYZ, PointCloud2Msg};
|
||||
|
||||
use r2r::sensor_msgs::msg::PointCloud2;
|
||||
|
||||
|
|
@ -25,10 +23,10 @@ fn convertxyz_r2r_msg() {
|
|||
},
|
||||
];
|
||||
let copy = cloud.clone();
|
||||
let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
|
||||
let internal_cloud = PointCloud2Msg::try_from_iter(cloud).unwrap();
|
||||
let r2r_msg_cloud: PointCloud2 = internal_cloud.into();
|
||||
let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into();
|
||||
let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap();
|
||||
let to_convert = convert_back_internal.try_into_iter().unwrap();
|
||||
let back_to_type = to_convert.collect::<Vec<PointXYZ>>();
|
||||
assert_eq!(copy, back_to_type);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,9 +1,7 @@
|
|||
#[cfg(feature = "rosrust_msg")]
|
||||
#[test]
|
||||
fn convertxyz_rosrust_msg() {
|
||||
use ros_pointcloud2::{
|
||||
pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg,
|
||||
};
|
||||
use ros_pointcloud2::{points::PointXYZ, PointCloud2Msg};
|
||||
|
||||
let cloud = vec![
|
||||
PointXYZ {
|
||||
|
|
@ -23,10 +21,10 @@ fn convertxyz_rosrust_msg() {
|
|||
},
|
||||
];
|
||||
let copy = cloud.clone();
|
||||
let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap();
|
||||
let internal_cloud = PointCloud2Msg::try_from_iter(cloud).unwrap();
|
||||
let rosrust_msg_cloud: rosrust_msg::sensor_msgs::PointCloud2 = internal_cloud.into();
|
||||
let convert_back_internal: PointCloud2Msg = rosrust_msg_cloud.into();
|
||||
let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap();
|
||||
let to_convert = convert_back_internal.try_into_iter().unwrap();
|
||||
let back_to_type = to_convert.collect::<Vec<PointXYZ>>();
|
||||
assert_eq!(copy, back_to_type);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue