Skip to content

Commit

Permalink
merge
Browse files Browse the repository at this point in the history
  • Loading branch information
stelzo committed Jun 24, 2024
2 parents 4c0db66 + 43f3d4e commit e6b7649
Show file tree
Hide file tree
Showing 9 changed files with 546 additions and 421 deletions.
21 changes: 21 additions & 0 deletions .github/workflows/rclrs_jazzy.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
name: rclrs_jazzy

on:
push:
branches:
- rclrs
pull_request:
branches:
- rclrs
workflow_dispatch:

env:
CARGO_TERM_COLOR: always

jobs:
tests_jazzy:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_rclrs_jazzy --tag rclrs_jazzy
- run: docker run rclrs_jazzy cargo test --features derive,nalgebra,rayon
13 changes: 7 additions & 6 deletions Cargo.toml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[package]
name = "ros_pointcloud2"
version = "0.5.0-rc.1"
version = "0.5.0"
edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"]
description = "Customizable conversions for working with sensor_msgs/PointCloud2."
Expand Down Expand Up @@ -32,11 +32,11 @@ rust-version = "1.63"
[dependencies]
rosrust_msg = { version = "0.1", optional = true }
rosrust = { version = "0.9.11", optional = true }
r2r = { version = "0.8.4", optional = true }
r2r = { version = "0.9", optional = true }
rayon = { version = "1", optional = true }
nalgebra = { version = "0.32.5", optional = true, default-features = false }
nalgebra = { version = "0.33", optional = true, default-features = false }
rpcl2-derive = { version = "0.3", optional = true }
type-layout = { version = "0.2", package = "type-layout-syn2", optional = true }
memoffset = { version = "0.9", optional = true }

sensor_msgs = { version = "*", optional = true }
std_msgs = { version = "*", optional = true }
Expand All @@ -45,17 +45,18 @@ builtin_interfaces = { version = "*", optional = true }
[dev-dependencies]
rand = "0.8"
criterion = { version = "0.5", features = ["html_reports"] }
pretty_assertions = "1.0"

[features]
rclrs_msg = ["dep:sensor_msgs", "dep:std_msgs", "dep:builtin_interfaces"]
rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
r2r_msg = ["dep:r2r"]
rayon = ["dep:rayon"]
derive = ["dep:rpcl2-derive", "dep:type-layout"]
derive = ["dep:rpcl2-derive", "dep:memoffset"]
nalgebra = ["dep:nalgebra"]
std = ["nalgebra/std"]

default = ["std", "derive", "rclrs_msg"]
default = ["std", "rclrs_msg"]

[package.metadata.docs.rs]
features = ["derive", "nalgebra", "rayon"]
Expand Down
43 changes: 16 additions & 27 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@

ros_pointcloud2 uses its own type for the message `PointCloud2Msg` to keep the library framework agnostic. ROS1 and ROS2 are supported with feature flags.

Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.0-rc.1/) for a complete guide.
Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.0/) for a complete guide.

## Quickstart

Expand Down Expand Up @@ -57,9 +57,9 @@ You can use `rosrust` and `r2r` by enabling the respective feature:

```toml
[dependencies]
ros_pointcloud2 = { version = "*", features = ["r2r_msg", "derive"]}
ros_pointcloud2 = { version = "*", features = ["r2r_msg"]}
# or
ros_pointcloud2 = { version = "*", features = ["rosrust_msg", "derive"]}
ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]}
```

### rclrs (ros2_rust)
Expand All @@ -68,7 +68,7 @@ Features do not work properly with `rcrls` because the messages are linked exter

```toml
[dependencies]
ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.0-rc.1_rclrs" }
ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.0_rclrs" }
```

Also, indicate the following dependencies to your linker inside the `package.xml` of your package.
Expand All @@ -84,32 +84,21 @@ Please open an issue or PR if you need other integrations.
## Performance

This library offers a speed up when compared to PointCloudLibrary (PCL) conversions but the specific factor depends heavily on the use case and system.
The `_vec` conversions are on average ~6x faster than PCL while the single core iteration `_iter` functions are around ~2x faster.
Parallelization with `_par_iter` gives a ~9x speed up compared to an OpenMP accelerated PCL pipeline.

The results are measured on an Intel i7-14700 with benchmarks from [this repository](https://github.com/stelzo/ros_pcl_conv_bench).
See [this repository](https://github.com/stelzo/ros_pcl_conv_bench) for a detailed benchmark.

For minimizing the conversion overhead in general, always use the functions that best fit your use case.

## `no_std` Environments

The `_iter` conversions are compatible with `#[no_std]` environments if an allocator is provided. This is due to the fact that names for point fields do not have a maximum length, and PointCloud2 data vectors can have arbitrary sizes. Use `default-features = false` to disable std for this crate. Only `nalgebra` can be added as an additional feature in this case.

## License

Licensed under either of

- Apache License, Version 2.0, ([LICENSE-APACHE](LICENSE-APACHE) or http://www.apache.org/licenses/LICENSE-2.0)
- MIT license ([LICENSE-MIT](LICENSE-MIT) or http://opensource.org/licenses/MIT)

at your option.

### type-layout

For compatibility reasons, a patched version of `type-layout` is included in this repository. The original crate can be found [here](https://crates.io/crates/type-layout). After the patch is applied on the original `type-layout` crate ([PR](https://github.com/LPGhatguy/type-layout/pull/9)), the local dependency will be changed to the original crate.
### License

`type-layout` is licensed under MIT or Apache-2.0 and Copyright by Lucien Greathouse. The changes are highlighted in the diff of the PR.
<sup>
Licensed under either of <a href="LICENSE-APACHE">Apache License, Version
2.0</a> or <a href="LICENSE-MIT">MIT license</a> at your option.
</sup>

### Contribution
<br>

Unless you explicitly state otherwise, any contribution intentionally submitted for inclusion in the work by you, as defined in the Apache-2.0 license, shall be dual licensed as above, without any additional terms or conditions.
<sub>
Unless you explicitly state otherwise, any contribution intentionally submitted
for inclusion in this crate by you, as defined in the Apache-2.0 license, shall
be dual licensed as above, without any additional terms or conditions.
</sub>
34 changes: 21 additions & 13 deletions src/iterator.rs
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
//! Iterator implementations for [`PointCloud2Msg`] including a parallel iterator for rayon.
use crate::{
Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData,
Endian, FieldDatatype, MsgConversionError, PointCloud2Msg, PointConvertible, PointData,
RPCL2Point,
};

Expand All @@ -27,12 +27,12 @@ use alloc::vec::Vec;
///
pub struct PointCloudIterator<const N: usize, C>
where
C: Fields<N>,
C: PointConvertible<N>,
{
iteration: usize,
iteration_back: usize,
data: ByteBufferView<N>,
phantom_c: core::marker::PhantomData<C>, // internally used for pdata names array
_phantom: core::marker::PhantomData<C>,
}

#[cfg(feature = "rayon")]
Expand Down Expand Up @@ -250,7 +250,7 @@ impl<const N: usize> ByteBufferView<N> {

impl<const N: usize, C> TryFrom<PointCloud2Msg> for PointCloudIterator<N, C>
where
C: Fields<N>,
C: PointConvertible<N>,
{
type Error = MsgConversionError;

Expand All @@ -262,8 +262,10 @@ where
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];

let not_found_fieldnames = C::field_names_ordered()
.into_iter()
let fields_only = crate::ordered_field_names::<N, C>();

let not_found_fieldnames = fields_only
.iter()
.map(|name| {
let found = cloud.fields.iter().any(|field| field.name == *name);
(name, found)
Expand All @@ -279,9 +281,8 @@ where
return Err(MsgConversionError::FieldsNotFound(names_not_found));
}

let ordered_fieldnames = C::field_names_ordered();
for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) {
if ordered_fieldnames.contains(&field.name.as_str()) {
if fields_only.contains(&field.name) {
*with_offset = (
field.name.clone(),
field.datatype.try_into()?,
Expand Down Expand Up @@ -318,9 +319,16 @@ where
return Err(MsgConversionError::DataLengthMismatch);
}

let last_offset = offsets.last().expect("Dimensionality is 0.");
let last_offset = match offsets.last() {
Some(offset) => *offset,
None => return Err(MsgConversionError::DataLengthMismatch),
};

let last_pdata = match pdata.last() {
Some(pdata) => pdata,
None => return Err(MsgConversionError::DataLengthMismatch),
};

let last_pdata = pdata.last().expect("Dimensionality is 0.");
let size_with_last_pdata = last_offset + last_pdata.1.size();
if size_with_last_pdata > point_step_size {
return Err(MsgConversionError::DataLengthMismatch);
Expand All @@ -342,14 +350,14 @@ where
iteration: 0,
iteration_back: cloud_length - 1,
data,
phantom_c: core::marker::PhantomData,
_phantom: core::marker::PhantomData,
})
}
}

impl<const N: usize, C> PointCloudIterator<N, C>
where
C: Fields<N>,
C: PointConvertible<N>,
{
#[inline]
#[must_use]
Expand All @@ -358,7 +366,7 @@ where
iteration: 0,
iteration_back: data.len() - 1,
data,
phantom_c: core::marker::PhantomData,
_phantom: core::marker::PhantomData,
}
}

Expand Down
Loading

0 comments on commit e6b7649

Please sign in to comment.