From bc447afe6017f8551114962ffe0c59c3c0f1f4e3 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 25 Mar 2024 12:35:46 +0100 Subject: [PATCH 01/11] standalone rclrs testing --- .github/workflows/r2r_galactic.yml | 6 +++++ .github/workflows/r2r_humble.yml | 6 +++++ .github/workflows/r2r_iron.yml | 6 +++++ .github/workflows/rclrs_humble.yml | 21 +++++++++++++++++ .github/workflows/rosrust_noetic.yml | 6 +++++ .github/workflows/tests.yml | 6 +++++ tests/Dockerfile_rclrs_humble | 34 ++++++++++++++++++++++++++++ tests/rclrs_test.bash | 25 ++++++++++++++++++++ 8 files changed, 110 insertions(+) create mode 100644 .github/workflows/rclrs_humble.yml create mode 100644 tests/Dockerfile_rclrs_humble create mode 100644 tests/rclrs_test.bash diff --git a/.github/workflows/r2r_galactic.yml b/.github/workflows/r2r_galactic.yml index 99180aa..f1718f5 100644 --- a/.github/workflows/r2r_galactic.yml +++ b/.github/workflows/r2r_galactic.yml @@ -2,7 +2,13 @@ name: r2r_galactic on: push: + branches: + - "*" + - "!rclrs" pull_request: + branches: + - "*" + - "!rclrs" workflow_dispatch: env: diff --git a/.github/workflows/r2r_humble.yml b/.github/workflows/r2r_humble.yml index 2f8721d..25cfe90 100644 --- a/.github/workflows/r2r_humble.yml +++ b/.github/workflows/r2r_humble.yml @@ -2,7 +2,13 @@ name: r2r_humble on: push: + branches: + - "*" + - "!rclrs" pull_request: + branches: + - "*" + - "!rclrs" workflow_dispatch: env: diff --git a/.github/workflows/r2r_iron.yml b/.github/workflows/r2r_iron.yml index a773884..fa435be 100644 --- a/.github/workflows/r2r_iron.yml +++ b/.github/workflows/r2r_iron.yml @@ -2,7 +2,13 @@ name: r2r_iron on: push: + branches: + - "*" + - "!rclrs" pull_request: + branches: + - "*" + - "!rclrs" workflow_dispatch: env: diff --git a/.github/workflows/rclrs_humble.yml b/.github/workflows/rclrs_humble.yml new file mode 100644 index 0000000..7ac4349 --- /dev/null +++ b/.github/workflows/rclrs_humble.yml @@ -0,0 +1,21 @@ +name: rclrs_humble + +on: + push: + branches: + - rclrs + pull_request: + branches: + - rclrs + workflow_dispatch: + +env: + CARGO_TERM_COLOR: always + +jobs: + tests_humble: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble + - run: docker run rclrs_humble cargo test diff --git a/.github/workflows/rosrust_noetic.yml b/.github/workflows/rosrust_noetic.yml index a5acf78..f262078 100644 --- a/.github/workflows/rosrust_noetic.yml +++ b/.github/workflows/rosrust_noetic.yml @@ -2,7 +2,13 @@ name: rosrust_noetic on: push: + branches: + - "*" + - "!rclrs" pull_request: + branches: + - "*" + - "!rclrs" workflow_dispatch: jobs: diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 8066730..c28dde0 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -2,7 +2,13 @@ name: Tests on: push: + branches: + - "*" + - "!rclrs" pull_request: + branches: + - "*" + - "!rclrs" workflow_dispatch: env: diff --git a/tests/Dockerfile_rclrs_humble b/tests/Dockerfile_rclrs_humble new file mode 100644 index 0000000..d5ee37b --- /dev/null +++ b/tests/Dockerfile_rclrs_humble @@ -0,0 +1,34 @@ +# syntax=docker/dockerfile:1 +FROM ros:humble + +# Update default packages +RUN apt-get update + +# Get Ubuntu packages +RUN apt-get install -y \ + build-essential \ + curl \ + libclang-dev \ + git \ + python3-pip \ + python3-vcstool + +# Get Rust +RUN curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | bash -s -- -y +RUN echo 'source $HOME/.cargo/env' >> $HOME/.bashrc +RUN . $HOME/.cargo/env && cargo install --debug cargo-ament-build +RUN pip install git+https://github.com/colcon/colcon-cargo.git +RUN pip install git+https://github.com/colcon/colcon-ros-cargo.git + +WORKDIR /ros2_rust_build +RUN git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust +RUN vcs import src < src/ros2_rust/ros2_rust_humble.repos + +WORKDIR /ros2_rust_build/src/ros_pointcloud2_tests +COPY . . + +WORKDIR /ros2_rust_build +RUN . $HOME/.cargo/env && . /opt/ros/humble/setup.sh && colcon build + +RUN chmod +x /ros2_rust_build/src/ros_pointcloud2_tests/tests/rclrs_test.bash +ENTRYPOINT [ "/ros2_rust_build/src/ros_pointcloud2_tests/tests/rclrs_test.bash" ] diff --git a/tests/rclrs_test.bash b/tests/rclrs_test.bash new file mode 100644 index 0000000..4afaab4 --- /dev/null +++ b/tests/rclrs_test.bash @@ -0,0 +1,25 @@ +#!/bin/bash + +. "$HOME/.cargo/env" + +# run rustup to test with latest rust version +rustup update + +if [ -e "/opt/ros/iron/setup.bash" ]; then + . "/opt/ros/iron/setup.bash" + . "/ros2_rust_build/install/local_setup.bash" +fi + +if [ -e "/opt/ros/humble/setup.bash" ]; then + . "/opt/ros/humble/setup.bash" + . "/ros2_rust_build/install/local_setup.bash" +fi + +if [ -e "/opt/ros/galactic/setup.bash" ]; then + . "/opt/ros/galactic/setup.bash" + . "/ros2_rust_build/install/local_setup.bash" +fi + +cd /ros2_rust_build/src/ros_pointcloud2_tests/ || exit + +"$@" From c7bae0d3775f30f4af5a83ecf4df6a52df090ef8 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 25 Mar 2024 12:46:57 +0100 Subject: [PATCH 02/11] except rclrs branch --- .github/workflows/r2r_galactic.yml | 10 ++++------ .github/workflows/r2r_humble.yml | 10 ++++------ .github/workflows/r2r_iron.yml | 10 ++++------ .github/workflows/rosrust_noetic.yml | 10 ++++------ .github/workflows/tests.yml | 10 ++++------ 5 files changed, 20 insertions(+), 30 deletions(-) diff --git a/.github/workflows/r2r_galactic.yml b/.github/workflows/r2r_galactic.yml index f1718f5..5c5f3d1 100644 --- a/.github/workflows/r2r_galactic.yml +++ b/.github/workflows/r2r_galactic.yml @@ -2,13 +2,11 @@ name: r2r_galactic on: push: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs pull_request: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs workflow_dispatch: env: diff --git a/.github/workflows/r2r_humble.yml b/.github/workflows/r2r_humble.yml index 25cfe90..5373957 100644 --- a/.github/workflows/r2r_humble.yml +++ b/.github/workflows/r2r_humble.yml @@ -2,13 +2,11 @@ name: r2r_humble on: push: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs pull_request: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs workflow_dispatch: env: diff --git a/.github/workflows/r2r_iron.yml b/.github/workflows/r2r_iron.yml index fa435be..5de642d 100644 --- a/.github/workflows/r2r_iron.yml +++ b/.github/workflows/r2r_iron.yml @@ -2,13 +2,11 @@ name: r2r_iron on: push: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs pull_request: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs workflow_dispatch: env: diff --git a/.github/workflows/rosrust_noetic.yml b/.github/workflows/rosrust_noetic.yml index f262078..4662df4 100644 --- a/.github/workflows/rosrust_noetic.yml +++ b/.github/workflows/rosrust_noetic.yml @@ -2,13 +2,11 @@ name: rosrust_noetic on: push: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs pull_request: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs workflow_dispatch: jobs: diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index c28dde0..7f940b7 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -2,13 +2,11 @@ name: Tests on: push: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs pull_request: - branches: - - "*" - - "!rclrs" + branches-ignore: + - rclrs workflow_dispatch: env: From 11868f5f451ede623f1508df79ac871fa2fd86ab Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 25 Mar 2024 12:51:23 +0100 Subject: [PATCH 03/11] clippy --- .github/workflows/rclrs_humble.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/rclrs_humble.yml b/.github/workflows/rclrs_humble.yml index 7ac4349..5a1f759 100644 --- a/.github/workflows/rclrs_humble.yml +++ b/.github/workflows/rclrs_humble.yml @@ -19,3 +19,4 @@ jobs: - 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 clippy --all-targets -- -D warnings From e231047056d21ee78a3049c0922097c83acd3ee9 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 25 Mar 2024 13:04:15 +0100 Subject: [PATCH 04/11] iron rclrs --- .github/workflows/rclrs_humble.yml | 1 - .github/workflows/rclrs_iron.yml | 21 ++++++++++++++++++ tests/Dockerfile_rclrs_iron | 34 ++++++++++++++++++++++++++++++ tests/rclrs_test.bash | 5 ----- 4 files changed, 55 insertions(+), 6 deletions(-) create mode 100644 .github/workflows/rclrs_iron.yml create mode 100644 tests/Dockerfile_rclrs_iron diff --git a/.github/workflows/rclrs_humble.yml b/.github/workflows/rclrs_humble.yml index 5a1f759..7ac4349 100644 --- a/.github/workflows/rclrs_humble.yml +++ b/.github/workflows/rclrs_humble.yml @@ -19,4 +19,3 @@ jobs: - 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 clippy --all-targets -- -D warnings diff --git a/.github/workflows/rclrs_iron.yml b/.github/workflows/rclrs_iron.yml new file mode 100644 index 0000000..d9ac7cf --- /dev/null +++ b/.github/workflows/rclrs_iron.yml @@ -0,0 +1,21 @@ +name: rclrs_iron + +on: + push: + branches: + - rclrs + pull_request: + branches: + - rclrs + workflow_dispatch: + +env: + CARGO_TERM_COLOR: always + +jobs: + tests_iron: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - run: docker build . --file ./tests/Dockerfile_rclrs_iron --tag rclrs_iron + - run: docker run rclrs_iron cargo test diff --git a/tests/Dockerfile_rclrs_iron b/tests/Dockerfile_rclrs_iron new file mode 100644 index 0000000..e6a3457 --- /dev/null +++ b/tests/Dockerfile_rclrs_iron @@ -0,0 +1,34 @@ +# syntax=docker/dockerfile:1 +FROM ros:iron + +# Update default packages +RUN apt-get update + +# Get Ubuntu packages +RUN apt-get install -y \ + build-essential \ + curl \ + libclang-dev \ + git \ + python3-pip \ + python3-vcstool + +# Get Rust +RUN curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | bash -s -- -y +RUN echo 'source $HOME/.cargo/env' >> $HOME/.bashrc +RUN . $HOME/.cargo/env && cargo install --debug cargo-ament-build +RUN pip install git+https://github.com/colcon/colcon-cargo.git +RUN pip install git+https://github.com/colcon/colcon-ros-cargo.git + +WORKDIR /ros2_rust_build +RUN git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust +RUN vcs import src < src/ros2_rust/ros2_rust_iron.repos + +WORKDIR /ros2_rust_build/src/ros_pointcloud2_tests +COPY . . + +WORKDIR /ros2_rust_build +RUN . $HOME/.cargo/env && . /opt/ros/iron/setup.sh && colcon build + +RUN chmod +x /ros2_rust_build/src/ros_pointcloud2_tests/tests/rclrs_test.bash +ENTRYPOINT [ "/ros2_rust_build/src/ros_pointcloud2_tests/tests/rclrs_test.bash" ] diff --git a/tests/rclrs_test.bash b/tests/rclrs_test.bash index 4afaab4..0dbc711 100644 --- a/tests/rclrs_test.bash +++ b/tests/rclrs_test.bash @@ -15,11 +15,6 @@ if [ -e "/opt/ros/humble/setup.bash" ]; then . "/ros2_rust_build/install/local_setup.bash" fi -if [ -e "/opt/ros/galactic/setup.bash" ]; then - . "/opt/ros/galactic/setup.bash" - . "/ros2_rust_build/install/local_setup.bash" -fi - cd /ros2_rust_build/src/ros_pointcloud2_tests/ || exit "$@" From 0105a76f533ba655db46893784cc8bff69286696 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 25 Mar 2024 15:13:48 +0100 Subject: [PATCH 05/11] bump version --- Cargo.toml | 2 +- README.md | 53 ++++++++++++++++++++++++++++++++++++----------------- 2 files changed, 37 insertions(+), 18 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index a97e861..f5de2b8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "ros_pointcloud2" -version = "0.3.2" +version = "0.4.0" edition = "2021" authors = ["Christopher Sieh "] description = "Customizable conversions for working with sensor_msgs/PointCloud2." diff --git a/README.md b/README.md index aba9a92..01d5766 100644 --- a/README.md +++ b/README.md @@ -61,7 +61,42 @@ let new_cloud_points = convert assert_eq!(new_cloud_points, cloud_copy); ``` -To use `ros_pointcloud2` in your favorite ROS crate, you can either use this crate's features (see Integration section below) or implement the `Into` and `From` traits for `PointCloud2Msg`. +## Integrations + +There are currently 3 integrations for common ROS crates. +- [rosrust_msg](https://github.com/adnanademovic/rosrust) + - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml) +- [r2r_msg](https://github.com/sequenceplanner/r2r) + - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_galactic.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_galactic.yml) + - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_humble.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_humble.yml) + - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_iron.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_iron.yml) +- [rclrs_msg](https://github.com/ros2-rust/ros2_rust) + - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_humble.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_humble.yml) + - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rclrs_iron.yml/badge.svg)](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"]} +# or +ros_pointcloud2 = { version = "*", features = ["rosrust_msg"]} +``` + +### rclrs (ros2_rust) +Features do not work properly with `rcrls` because the messages are linked externally. You need to use tags instead: +```toml +[dependencies] +ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.4.0_rclrs" } +``` +Also, indicate the following dependencies to your linker inside the `package.xml` of your package. +```xml +std_msgs +sensor_msgs +builtin_interfaces +``` + +### Others +To use `ros_pointcloud2` somewhere else, you can also implement the `Into` and `From` traits for `PointCloud2Msg`. Try to avoid cloning the `data: Vec` field. ```rust @@ -82,22 +117,6 @@ impl From for PointCloud2Msg { } ``` -## Integrations - -There are currently 2 integrations for common ROS crates such as rosrust for ROS1 and R2R for ROS2. -- [rosrust_msg](https://github.com/adnanademovic/rosrust) - - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/rosrust_noetic.yml) -- [r2r_msg](https://github.com/sequenceplanner/r2r) - - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_galactic.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_galactic.yml) - - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_humble.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_humble.yml) - - [![Tests](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_iron.yml/badge.svg)](https://github.com/stelzo/ros_pointcloud2/actions/workflows/r2r_iron.yml) - -You can use them by enabling the corresponding feature. Example: -```toml -[dependencies] -ros_pointcloud2 = { version = "*", features = ["r2r_msg"]} -``` - Please open an issue or PR if you want to see support for other crates. ## Features From 6f55407a20932832e3a0dd4bbd4d66237e6c2067 Mon Sep 17 00:00:00 2001 From: stelzo Date: Fri, 26 Apr 2024 21:50:23 +0200 Subject: [PATCH 06/11] add iterator impl --- Cargo.toml | 2 - src/convert.rs | 349 +++++++++++++++++++ src/lib.rs | 761 ++---------------------------------------- src/pcl_utils.rs | 18 +- src/reader.rs | 316 ++++++++++++++++++ src/writer.rs | 171 ++++++++++ tests/e2e_test.rs | 706 +++++++++++++++++++++------------------ tests/r2r_msg_test.rs | 2 +- 8 files changed, 1247 insertions(+), 1078 deletions(-) create mode 100644 src/convert.rs create mode 100644 src/reader.rs create mode 100644 src/writer.rs diff --git a/Cargo.toml b/Cargo.toml index f5de2b8..8c2626e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,8 +14,6 @@ homepage = "https://github.com/stelzo/ros_pointcloud2" exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**/build/**", "**/dist/**", "**/docs/**", "**/doc/**"] [dependencies] -mem_macros = "1.0.1" -num-traits = "0.2.15" fallible-iterator = "0.3.0" rosrust_msg = { version = "0.1", optional = true } rosrust = { version = "0.9.11", optional = true } diff --git a/src/convert.rs b/src/convert.rs new file mode 100644 index 0000000..d57a4d6 --- /dev/null +++ b/src/convert.rs @@ -0,0 +1,349 @@ +use crate::*; + +/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html). +#[derive(Clone, Debug, PartialEq, Copy)] +pub enum FieldDatatype { + F32, + F64, + I32, + U8, + U16, + U32, + I8, + I16, +} + +impl Default for FieldDatatype { + fn default() -> Self { + FieldDatatype::F32 + } +} + +/// Getter trait for the datatype of a field value. +pub trait GetFieldDatatype { + fn field_datatype() -> FieldDatatype; +} + +impl GetFieldDatatype for f32 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::F32 + } +} + +impl GetFieldDatatype for f64 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::F64 + } +} + +impl GetFieldDatatype for i32 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::I32 + } +} + +impl GetFieldDatatype for u8 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::U8 + } +} + +impl GetFieldDatatype for u16 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::U16 + } +} + +impl GetFieldDatatype for u32 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::U32 + } +} + +impl GetFieldDatatype for i8 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::I8 + } +} + +impl GetFieldDatatype for i16 { + fn field_datatype() -> FieldDatatype { + FieldDatatype::I16 + } +} + +pub(crate) fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 { + match geo_type { + FieldDatatype::I8 => 1, + FieldDatatype::U8 => 2, + FieldDatatype::I16 => 3, + FieldDatatype::U16 => 4, + FieldDatatype::I32 => 5, + FieldDatatype::U32 => 6, + FieldDatatype::F32 => 7, + FieldDatatype::F64 => 8, + } +} + +pub(crate) fn convert_msg_code_to_type(code: u8) -> Result { + match code { + 7 => Ok(FieldDatatype::F32), + 8 => Ok(FieldDatatype::F64), + 5 => Ok(FieldDatatype::I32), + 2 => Ok(FieldDatatype::U8), + 4 => Ok(FieldDatatype::U16), + 6 => Ok(FieldDatatype::U32), + 1 => Ok(FieldDatatype::I8), + 3 => Ok(FieldDatatype::I16), + _ => Err(ConversionError::UnsupportedFieldType), + } +} + +pub(crate) fn check_coord( + coord: Option, + fields: &[PointFieldMsg], + xyz_field_type: &FieldDatatype, +) -> Result { + match coord { + Some(y_idx) => { + let field = &fields[y_idx]; + if field.datatype != convert_to_msg_code(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. +/// Always make sure to use the same order as in your Into<> implementation to have a correct mapping. +pub trait MetaNames { + fn meta_names() -> [String; METADIM]; +} + +pub(crate) fn datatype_size(datatype: &FieldDatatype) -> usize { + match datatype { + FieldDatatype::U8 => 1, + FieldDatatype::U16 => 2, + FieldDatatype::U32 => 4, + FieldDatatype::I8 => 1, + FieldDatatype::I16 => 2, + FieldDatatype::I32 => 4, + FieldDatatype::F32 => 4, + FieldDatatype::F64 => 8, + } +} + +#[inline(always)] +pub(crate) fn add_point_to_byte_buffer< + T: FromBytes, + const SIZE: usize, + const DIM: usize, + const METADIM: usize, + C: PointConvertible, +>( + coords: C, + cloud: &mut PointCloud2Msg, +) -> Result { + let (coords_data, coords_meta): ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() { + Ok(meta) => meta, + Err(_) => return Err(ConversionError::PointConversionError), + }; + + // (x, y, z...) + coords_data + .iter() + .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); + + // meta data description + coords_meta.iter().for_each(|meta| { + let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)]; + cloud.data.extend_from_slice(truncated_bytes); + }); + + cloud.width += 1; + + Ok(true) +} + +/// This trait is used to convert a byte slice to a primitive type. +/// All PointField types are supported. +pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype { + fn from_be_bytes(bytes: &[u8]) -> Self; + fn from_le_bytes(bytes: &[u8]) -> Self; + + fn bytes(_: &Self) -> Vec; +} + +impl FromBytes for i8 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0]]) + } + + fn bytes(x: &i8) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for i16 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1]]) + } + + fn bytes(x: &i16) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for u16 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1]]) + } + + fn bytes(x: &u16) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for u32 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + fn bytes(x: &u32) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for f32 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + fn bytes(x: &f32) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for i32 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) + } + + fn bytes(x: &i32) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for f64 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([ + bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], + ]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([ + bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], + ]) + } + + fn bytes(x: &f64) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +impl FromBytes for u8 { + fn from_be_bytes(bytes: &[u8]) -> Self { + Self::from_be_bytes([bytes[0]]) + } + fn from_le_bytes(bytes: &[u8]) -> Self { + Self::from_le_bytes([bytes[0]]) + } + + fn bytes(x: &u8) -> Vec { + Vec::from(x.to_le_bytes()) + } +} + +#[derive(Clone, Debug, PartialEq)] +pub(crate) enum Endianness { + Big, + Little, +} + +impl Default for Endianness { + fn default() -> Self { + Endianness::Little + } +} + +pub(crate) fn load_loadable( + start_idx: usize, + data: &[u8], + endian: &Endianness, +) -> Option { + match endian { + Endianness::Big => Some(T::from_be_bytes( + load_bytes::(start_idx, data)?.as_slice(), + )), + Endianness::Little => Some(T::from_le_bytes( + load_bytes::(start_idx, data)?.as_slice(), + )), + } +} + +fn load_bytes(start_idx: usize, data: &[u8]) -> Option<[u8; S]> { + if start_idx + S > data.len() { + return None; + } + let mut buff: [u8; S] = [0; S]; + for (byte, buff_val) in buff.iter_mut().enumerate().take(S) { + let raw_byte = data.get(start_idx + byte); + match raw_byte { + None => { + return None; + } + Some(some_byte) => { + *buff_val = *some_byte; + } + } + } + + Some(buff) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn from_bytes() { + i8::bytes(&1); + u8::bytes(&1); + i16::bytes(&1); + u16::bytes(&1); + i32::bytes(&1); + u32::bytes(&1); + f32::bytes(&1.0); + f64::bytes(&1.0); + } +} diff --git a/src/lib.rs b/src/lib.rs index eff935d..7375ddd 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,15 +1,30 @@ +// TODO test reading writing different types +// TODO read less info from the point cloud than is available +// TODO write less info to the point cloud than is available +// TODO explain templating arguments more for reader and writer + #![doc = include_str!("../README.md")] +pub mod convert; pub mod pcl_utils; pub mod ros_types; +#[macro_export] +macro_rules! size_of { + ($t:ty) => { + std::mem::size_of::<$t>() + }; +} + +pub mod reader; +pub mod writer; + +pub use ros_types::PointCloud2Msg; + +use crate::convert::*; use crate::pcl_utils::*; -use crate::ros_types::{PointCloud2Msg, PointFieldMsg}; -use num_traits::Zero; +use crate::ros_types::PointFieldMsg; -#[macro_use] -pub extern crate mem_macros; - -pub extern crate fallible_iterator; +pub use fallible_iterator::FallibleIterator; /// Errors that can occur when converting between PointCloud2 and a custom type. #[derive(Debug)] @@ -33,228 +48,12 @@ pub trait PointConvertible + MetaNames + Clone + + 'static where T: FromBytes, { } -/// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html). -#[derive(Clone, Debug, PartialEq, Copy)] -pub enum FieldDatatype { - F32, - F64, - I32, - U8, - U16, - U32, - I8, - I16, -} - -/// Getter trait for the datatype of a field value. -pub trait GetFieldDatatype { - fn field_datatype() -> FieldDatatype; -} - -impl GetFieldDatatype for f32 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::F32 - } -} - -impl GetFieldDatatype for f64 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::F64 - } -} - -impl GetFieldDatatype for i32 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::I32 - } -} - -impl GetFieldDatatype for u8 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::U8 - } -} - -impl GetFieldDatatype for u16 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::U16 - } -} - -impl GetFieldDatatype for u32 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::U32 - } -} - -impl GetFieldDatatype for i8 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::I8 - } -} - -impl GetFieldDatatype for i16 { - fn field_datatype() -> FieldDatatype { - FieldDatatype::I16 - } -} - -fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 { - match geo_type { - FieldDatatype::I8 => 1, - FieldDatatype::U8 => 2, - FieldDatatype::I16 => 3, - FieldDatatype::U16 => 4, - FieldDatatype::I32 => 5, - FieldDatatype::U32 => 6, - FieldDatatype::F32 => 7, - FieldDatatype::F64 => 8, - } -} - -fn convert_msg_code_to_type(code: u8) -> Result { - match code { - 7 => Ok(FieldDatatype::F32), - 8 => Ok(FieldDatatype::F64), - 5 => Ok(FieldDatatype::I32), - 2 => Ok(FieldDatatype::U8), - 4 => Ok(FieldDatatype::U16), - 6 => Ok(FieldDatatype::U32), - 1 => Ok(FieldDatatype::I8), - 3 => Ok(FieldDatatype::I16), - _ => Err(ConversionError::UnsupportedFieldType), - } -} - -fn check_coord( - coord: Option, - fields: &[PointFieldMsg], - xyz_field_type: &FieldDatatype, -) -> Result { - match coord { - Some(y_idx) => { - let field = &fields[y_idx]; - if field.datatype != convert_to_msg_code(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. -/// Always make sure to use the same order as in your Into<> implementation to have a correct mapping. -pub trait MetaNames { - fn meta_names() -> [String; METADIM]; -} - -/// The Convert struct is used to convert between a PointCloud2 message and a custom type. -/// A custom type must implement the FromBytes trait and the Into trait. -/// The Into trait is used to convert the custom type into a tuple of the coordinates and the meta data. -/// The FromBytes trait is used to convert the bytes from the PointCloud2 message into the custom type. -/// -/// # Example -/// ``` -/// use ros_pointcloud2::mem_macros::size_of; -/// use ros_pointcloud2::{Convert, PointMeta}; -/// use ros_pointcloud2::ros_types::PointCloud2Msg; -/// const DIM : usize = 3; // how many dimensions your point has (e.g. x, y, z) -/// const METADIM : usize = 4; // how many meta fields you have (e.g. r, g, b, a) -/// type MyConverter = Convert; -/// ``` -pub struct Convert -where - T: FromBytes, - C: PointConvertible, -{ - iteration: usize, - coordinates: Vec, - phantom_t: std::marker::PhantomData, - data: Vec, - point_step_size: usize, - cloud_length: usize, - offsets: Vec, - big_endian: Endianness, - xyz_field_type: FieldDatatype, - meta: Vec<(String, FieldDatatype)>, -} - -pub type ConvertXYZ = Convert; -pub type ConvertXYZI = Convert; -pub type ConvertXYZNormal = Convert; -pub type ConvertXYZRGB = Convert; -pub type ConvertXYZRGBL = Convert; -pub type ConvertXYZRGBA = Convert; -pub type ConvertXYZRGBNormal = Convert; -pub type ConvertXYZINormal = Convert; -pub type ConvertXYZL = Convert; - -impl TryFrom> - for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Error = ConversionError; - - /// Converts a vector of custom types into a Convert struct that implements the Iterator trait. - /// - /// # Example - /// ``` - /// use ros_pointcloud2::{ConvertXYZ, ConversionError}; - /// use ros_pointcloud2::pcl_utils::PointXYZ; - /// - /// let cloud_points: Vec = vec![ - /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, - /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, - /// ]; - /// let convert: Result = ConvertXYZ::try_from(cloud_points); - /// ``` - fn try_from(cloud: Vec) -> Result { - let length = cloud.len(); - - let mut meta: Vec<(String, FieldDatatype)> = vec![]; - let first_point = cloud.first().ok_or(ConversionError::NoPoints)?; - let point_meta: ([T; DIM], [PointMeta; METADIM]) = match first_point.clone().try_into() { - Ok(point_meta) => point_meta, - Err(_) => { - return Err(ConversionError::PointConversionError); - } - }; - let meta_names = C::meta_names(); - let mut point_step_size = DIM * SIZE; - for (idx, value) in point_meta.1.iter().enumerate() { - meta.push(( - meta_names - .get(idx) - .ok_or(ConversionError::MetaIndexLengthMismatch)? - .clone(), - value.datatype, - )); - point_step_size += datatype_size(&value.datatype); - } - - Ok(Self { - phantom_t: std::marker::PhantomData, - iteration: usize::zero(), - coordinates: cloud, - data: Vec::new(), - point_step_size, - cloud_length: length, - offsets: Vec::new(), - big_endian: Endianness::Little, - xyz_field_type: T::field_datatype(), - meta, - }) - } -} - /// Meta data representation for a point /// /// This struct is used to store meta data in a fixed size byte buffer along the with the @@ -270,14 +69,14 @@ where /// ``` #[derive(Debug, Clone, Copy)] pub struct PointMeta { - bytes: [u8; size_of!(f64)], + bytes: [u8; std::mem::size_of::()], datatype: FieldDatatype, } impl Default for PointMeta { fn default() -> Self { Self { - bytes: [0; size_of!(f64)], + bytes: [0; std::mem::size_of::()], datatype: FieldDatatype::F32, } } @@ -292,7 +91,7 @@ impl PointMeta { /// ``` pub fn new(value: T) -> Self { let raw_bytes = T::bytes(&value); - let mut bytes = [0; size_of!(f64)]; + let mut bytes = [0; std::mem::size_of::()]; for (idx, byte) in raw_bytes.iter().enumerate() { bytes[idx] = *byte; } @@ -311,7 +110,7 @@ impl PointMeta { let bytes = data .get(offset..offset + size) .ok_or(ConversionError::DataLengthMismatch)?; - let mut bytes_array = [0; size_of!(f64)]; // 8 bytes as f64 is the largest type + let mut bytes_array = [0; std::mem::size_of::()]; // 8 bytes as f64 is the largest type for (idx, byte) in bytes.iter().enumerate() { bytes_array[idx] = *byte; } @@ -341,511 +140,3 @@ impl PointMeta { } } } - -impl TryFrom - for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Error = ConversionError; - - /// Converts a ROS PointCloud2 message into a Convert struct that implements the Iterator trait. - /// Iterate over the struct to get or use the points. - /// - /// # Example - /// Since we do not have a ROS node here, we first create a PointCloud2 message and then convert back to a Convert struct. - /// ``` - /// use ros_pointcloud2::ros_types::PointCloud2Msg; - /// use ros_pointcloud2::ConvertXYZ; - /// use ros_pointcloud2::pcl_utils::PointXYZ; - /// - /// let cloud_points: Vec = vec![ - /// PointXYZ { x: 1.0, y: 2.0, z: 3.0 }, - /// PointXYZ { x: 4.0, y: 5.0, z: 6.0 }, - /// ]; - /// let msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap(); - /// - /// let convert: ConvertXYZ = ConvertXYZ::try_from(msg).unwrap(); // parse message - /// ``` - fn try_from(cloud: PointCloud2Msg) -> Result { - if cloud.fields.len() < DIM { - return Err(ConversionError::NotEnoughFields); - } - - let xyz_field_type = T::field_datatype(); - - let mut has_x: Option = None; - let mut has_y: Option = None; - let mut has_z: Option = None; - - let mut meta_with_offsets: Vec<(String, FieldDatatype, usize)> = - Vec::with_capacity(METADIM); - - let lower_meta_names = C::meta_names() - .iter() - .map(|x| x.to_lowercase()) - .collect::>(); - 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) { - meta_with_offsets.push(( - field.name.clone(), - convert_msg_code_to_type(field.datatype)?, - field.offset as usize, - )); - } - } - } - } - - meta_with_offsets.sort_by(|a, b| a.2.cmp(&b.2)); - let meta_offsets: Vec = meta_with_offsets.iter().map(|x| x.2).collect(); - let meta: Vec<(String, FieldDatatype)> = meta_with_offsets - .iter() - .map(|x| (x.0.clone(), x.1)) - .collect(); - - 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; - if point_step_size * cloud.width as usize * cloud.height as usize != cloud.data.len() { - return Err(ConversionError::DataLengthMismatch); - } - - if let Some(max_offset) = offsets.last() { - if let Some(last_meta) = meta.last() { - let size_with_last_meta = max_offset + datatype_size(&last_meta.1); - if size_with_last_meta > point_step_size { - return Err(ConversionError::DataLengthMismatch); - } - } - } - - Ok(Self { - phantom_t: std::marker::PhantomData, - iteration: usize::zero(), - coordinates: Vec::new(), - data: cloud.data, - point_step_size, - cloud_length: cloud.width as usize * cloud.height as usize, - offsets, - big_endian: endian, - xyz_field_type: T::field_datatype(), - meta, - }) - } -} - -fn datatype_size(datatype: &FieldDatatype) -> usize { - match datatype { - FieldDatatype::U8 => 1, - FieldDatatype::U16 => 2, - FieldDatatype::U32 => 4, - FieldDatatype::I8 => 1, - FieldDatatype::I16 => 2, - FieldDatatype::I32 => 4, - FieldDatatype::F32 => 4, - FieldDatatype::F64 => 8, - } -} - -impl TryInto - for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Error = ConversionError; - - /// Convert the point cloud to a ROS message. - /// First use the `try_from` method for initializing the conversion and parsing meta data. - /// Then use the `try_into` method to do the actual conversion. - /// - /// # Example - /// ``` - /// use ros_pointcloud2::ros_types::PointCloud2Msg; - /// use ros_pointcloud2::ConvertXYZ; - /// use ros_pointcloud2::pcl_utils::PointXYZ; - /// - /// let cloud_points: Vec = 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 = ConvertXYZ::try_from(cloud_points).unwrap().try_into().unwrap(); - /// ``` - fn try_into(self) -> Result { - let mut cloud = PointCloud2Msg::default(); - - // Define the message fields - let mut fields = Vec::new(); - if DIM > 3 { - return Err(ConversionError::TooManyDimensions); - } - - let datatype: u8 = convert_to_msg_code(&self.xyz_field_type); - - if DIM > 1 { - fields.push(PointFieldMsg { - name: "x".to_string(), - offset: 0, - datatype, - count: 1, - }); - fields.push(PointFieldMsg { - name: "y".to_string(), - offset: SIZE as u32, - datatype, - count: 1, - }); - } - - if DIM == 3 { - fields.push(PointFieldMsg { - name: "z".to_string(), - offset: 2 * SIZE as u32, - datatype, - count: 1, - }); - } - - // meta data fields - let first_point = self.coordinates.first().ok_or(ConversionError::NoPoints)?; - let meta: ([T; DIM], [PointMeta; METADIM]) = match first_point.clone().try_into() { - Ok(meta) => meta, - Err(_) => return Err(ConversionError::PointConversionError), - }; - - let meta_names = C::meta_names(); - - let sized_dim = DIM as u32 * SIZE as u32; - - for (idx, value) in meta.1.iter().enumerate() { - let datatype: u8 = convert_to_msg_code(&value.datatype); - let mut offset = sized_dim; - for i in 0..idx { - let datatype: u8 = convert_to_msg_code(&meta.1[i].datatype); - let field_type = convert_msg_code_to_type(datatype)?; - let field_size = datatype_size(&field_type); - offset += field_size as u32; - } - fields.push(PointFieldMsg { - name: meta_names[idx].to_string(), - offset, - datatype, - count: 1, - }); - } - - // calc all meta data points step size - let mut step_size = 0; - for field in fields.iter() { - let field_type = convert_msg_code_to_type(field.datatype)?; - let field_size = datatype_size(&field_type); - step_size += field.count * field_size as u32; - } - - cloud.fields = fields; - cloud.point_step = step_size; - cloud.row_step = self.coordinates.len() as u32 * cloud.point_step; - cloud.height = 1; - cloud.width = self.coordinates.len() as u32; - cloud.is_bigendian = false; - cloud.is_dense = true; - - for coords in self.coordinates { - let coords_data: ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() { - Ok(meta) => meta, - Err(_) => return Err(ConversionError::PointConversionError), - }; - coords_data - .0 - .iter() - .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); - coords_data.1.iter().for_each(|meta| { - let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)]; - cloud.data.extend_from_slice(truncated_bytes); - }); - } - - Ok(cloud) - } -} - -impl - Convert -where - T: FromBytes, - C: PointConvertible, -{ - /// Convenience getter for the number of points in the cloud. - pub fn len(&self) -> usize { - self.cloud_length - } - - pub fn is_empty(&self) -> bool { - self.cloud_length == 0 - } -} - -impl - fallible_iterator::FallibleIterator for Convert -where - T: FromBytes, - C: PointConvertible, -{ - type Item = C; - type Error = ConversionError; - - /// Iterate over the data buffer and interpret the data as a point. - fn next(&mut self) -> Result, Self::Error> { - if self.iteration >= self.cloud_length { - return Ok(None); // iteration finished - } - - let mut xyz: [T; DIM] = [T::zero(); DIM]; - let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM]; - for (idx, in_point_offset) in self.offsets.iter().enumerate() { - if idx < DIM { - match load_loadable::( - (self.iteration * self.point_step_size) + in_point_offset, - &self.data, - self.big_endian.clone(), - ) { - Some(c) => xyz[idx] = c, - None => return Err(ConversionError::EndOfBuffer), - } - } else { - let meta_idx = idx - DIM; - let meta_type = self.meta[meta_idx].1; - let start = (self.iteration * self.point_step_size) + in_point_offset; - if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) { - meta[meta_idx] = m; - } else { - return Err(ConversionError::MetaIndexLengthMismatch); // since we can not return an error in the iterator, we finish the iteration here early since the last point is not valid. This case is not expected since we catch it while parsing the file. - } - } - } - - self.iteration += 1; - let conv = C::try_from((xyz, meta)); // try convert the point - match conv { - Err(_) => Err(ConversionError::PointConversionError), - Ok(tuple) => Ok(Some(tuple)), - } - } -} - -/// This trait is used to convert a byte slice to a primitive type. -/// All PointField types are supported. -pub trait FromBytes: Zero + Sized + Copy + GetFieldDatatype { - fn from_be_bytes(bytes: &[u8]) -> Self; - fn from_le_bytes(bytes: &[u8]) -> Self; - - fn bytes(_: &Self) -> Vec; -} - -impl FromBytes for i8 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0]]) - } - - fn bytes(x: &i8) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for i16 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1]]) - } - - fn bytes(x: &i16) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for u16 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1]]) - } - - fn bytes(x: &u16) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for u32 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn bytes(x: &u32) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for f32 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn bytes(x: &f32) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for i32 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) - } - - fn bytes(x: &i32) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for f64 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([ - bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], - ]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([ - bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], - ]) - } - - fn bytes(x: &f64) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -impl FromBytes for u8 { - fn from_be_bytes(bytes: &[u8]) -> Self { - Self::from_be_bytes([bytes[0]]) - } - fn from_le_bytes(bytes: &[u8]) -> Self { - Self::from_le_bytes([bytes[0]]) - } - - fn bytes(x: &u8) -> Vec { - Vec::from(x.to_le_bytes()) - } -} - -#[derive(Clone, Debug, PartialEq)] -enum Endianness { - Big, - Little, -} - -fn load_loadable( - start_idx: usize, - data: &[u8], - endian: Endianness, -) -> Option { - match endian { - Endianness::Big => Some(T::from_be_bytes( - load_bytes::(start_idx, data)?.as_slice(), - )), - Endianness::Little => Some(T::from_le_bytes( - load_bytes::(start_idx, data)?.as_slice(), - )), - } -} - -fn load_bytes(start_idx: usize, data: &[u8]) -> Option<[u8; S]> { - if start_idx + S > data.len() { - return None; - } - let mut buff: [u8; S] = [u8::zero(); S]; - for (byte, buff_val) in buff.iter_mut().enumerate().take(S) { - let raw_byte = data.get(start_idx + byte); - match raw_byte { - None => { - return None; - } - Some(some_byte) => { - *buff_val = *some_byte; - } - } - } - - Some(buff) -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn from_bytes() { - i8::bytes(&1); - u8::bytes(&1); - i16::bytes(&1); - u16::bytes(&1); - i32::bytes(&1); - u32::bytes(&1); - f32::bytes(&1.0); - f64::bytes(&1.0); - } -} diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs index 359651b..1a4a333 100644 --- a/src/pcl_utils.rs +++ b/src/pcl_utils.rs @@ -50,7 +50,7 @@ impl MetaNames<0> for PointXYZ { } } -impl PointConvertible for PointXYZ {} +impl PointConvertible() }, 3, 0> for PointXYZ {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an intensity value. @@ -89,7 +89,7 @@ impl MetaNames<1> for PointXYZI { } } -impl PointConvertible for PointXYZI {} +impl PointConvertible() }, 3, 1> for PointXYZI {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an RGB color value. @@ -135,7 +135,7 @@ impl MetaNames<1> for PointXYZRGB { } } -impl PointConvertible for PointXYZRGB {} +impl PointConvertible() }, 3, 1> for PointXYZRGB {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and an RGBA color value. @@ -187,7 +187,7 @@ impl MetaNames<2> for PointXYZRGBA { } } -impl PointConvertible for PointXYZRGBA {} +impl PointConvertible() }, 3, 2> for PointXYZRGBA {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates, an RGB color value and a normal vector. @@ -247,7 +247,7 @@ impl MetaNames<4> for PointXYZRGBNormal { } } -impl PointConvertible for PointXYZRGBNormal {} +impl PointConvertible() }, 3, 4> for PointXYZRGBNormal {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates, an intensity value and a normal vector. @@ -300,7 +300,7 @@ impl MetaNames<4> for PointXYZINormal { } } -impl PointConvertible for PointXYZINormal {} +impl PointConvertible() }, 3, 4> for PointXYZINormal {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a label. @@ -339,7 +339,7 @@ impl MetaNames<1> for PointXYZL { } } -impl PointConvertible for PointXYZL {} +impl PointConvertible() }, 3, 1> for PointXYZL {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a label. @@ -390,7 +390,7 @@ impl MetaNames<2> for PointXYZRGBL { } } -impl PointConvertible for PointXYZRGBL {} +impl PointConvertible() }, 3, 2> for PointXYZRGBL {} /// Predefined point type commonly used in ROS with PCL. /// This is a 3D point with x, y, z coordinates and a normal vector. @@ -440,4 +440,4 @@ impl MetaNames<3> for PointXYZNormal { } } -impl PointConvertible for PointXYZNormal {} +impl PointConvertible() }, 3, 3> for PointXYZNormal {} diff --git a/src/reader.rs b/src/reader.rs new file mode 100644 index 0000000..f57aa3a --- /dev/null +++ b/src/reader.rs @@ -0,0 +1,316 @@ +use crate::*; + +/// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`). +/// Every additional meta data is ignored. +pub type ReaderXYZ = Reader; + +/// Provides the message as an Iterator over xyz coordinates and intensity (see `PointXYZI`). +/// Every additional meta data is ignored. +pub type ReaderXYZI = Reader; + +/// Provides the message as an Iterator over xyz coordinates and normal (see `PointXYZNormal`). +/// Every additional meta data is ignored. +pub type ReaderXYZNormal = Reader; + +/// 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 = Reader; + +/// 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 = Reader; + +/// 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 = Reader; + +/// 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 = Reader; + +/// Provides the message as an Iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`). +/// Every additional meta data is ignored. +pub type ReaderXYZINormal = Reader; + +/// 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 = Reader; + +/// The Reader provides an abstraction around PointCloud2Msg by implementing a fallible iterator around the data buffer. +/// +/// The iterator is defined at compile time, so the Point has to be described via template arguments. +/// At runtime, the iterator catches read errors from the Msg type which typically only occur with corrupted data. +/// +/// 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::{ +/// FallibleIterator, // needed for for_each and the other iterator methods +/// reader::ReaderXYZ, +/// writer::WriterXYZ, +/// PointCloud2Msg, +/// pcl_utils::PointXYZ, +/// }; +/// +/// let cloud_points: Vec = 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(); // parse message +/// +/// convert.for_each(|point: PointXYZ| { +/// // do something with the point +/// Ok(()) // return Ok to continue iteration +/// }) +/// .unwrap(); // handle point conversion errors +/// ``` +pub struct Reader +where + T: FromBytes, + C: MetaNames, +{ + iteration: usize, + data: Vec, + point_step_size: usize, + cloud_length: usize, + offsets: Vec, + meta: Vec<(String, FieldDatatype)>, + endianness: Endianness, + phantom_t: std::marker::PhantomData, // internally used for byte and datatype conversions + phantom_c: std::marker::PhantomData, // 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 FallibleIterator + for Reader +where + T: FromBytes, + C: PointConvertible, +{ + type Item = C; + type Error = ConversionError; + + /// 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>`, for example. + fn size_hint(&self) -> (usize, Option) { + (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) -> Result, Self::Error> { + if self.iteration >= self.cloud_length { + return Ok(None); // iteration finished + } + + let mut xyz: [T; DIM] = [T::default(); DIM]; + let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM]; + for (idx, in_point_offset) in self.offsets.iter().enumerate() { + if idx < DIM { + match load_loadable::( + (self.iteration * self.point_step_size) + in_point_offset, + &self.data, + &self.endianness, + ) { + Some(c) => xyz[idx] = c, + None => return Err(ConversionError::EndOfBuffer), + } + } else { + let meta_idx = idx - DIM; + let meta_type = self.meta[meta_idx].1; + let start = (self.iteration * self.point_step_size) + in_point_offset; + if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) { + meta[meta_idx] = m; + } else { + return Err(ConversionError::MetaIndexLengthMismatch); + } + } + } + + self.iteration += 1; + + match C::try_from((xyz, meta)) { + Err(_) => Err(ConversionError::PointConversionError), + Ok(tuple) => Ok(Some(tuple)), + } + } +} + +impl TryFrom + for Reader +where + T: FromBytes, + C: MetaNames, +{ + 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::{ + /// FallibleIterator, // needed for for_each and the other iterator methods + /// reader::ReaderXYZ, + /// writer::WriterXYZ, + /// PointCloud2Msg, + /// pcl_utils::PointXYZ, + /// }; + /// + /// let cloud_points: Vec = 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 { + if cloud.fields.len() < DIM { + return Err(ConversionError::NotEnoughFields); + } + + let xyz_field_type = T::field_datatype(); + + let mut has_x: Option = None; + let mut has_y: Option = None; + let mut has_z: Option = 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::>(); + + 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) { + println!("{} {} {}", idx, field.name, meta_with_offsets.len()); + match meta_with_offsets.get_mut(idx - DIM) { + None => return Err(ConversionError::MetaIndexLengthMismatch), + Some(data) => { + data.0 = field.name.clone(); + data.1 = convert_msg_code_to_type(field.datatype)?; + data.2 = field.offset as usize; + } + } + } + } + } + } + + meta_with_offsets.sort_unstable_by(|a, b| a.2.cmp(&b.2)); + let mut meta_offsets = [usize::default(); METADIM]; + let mut meta: Vec<(String, FieldDatatype)> = + vec![(String::default(), FieldDatatype::U8); METADIM]; + meta_with_offsets + .into_iter() + .enumerate() + .for_each(|(idx, (name, datatype, offset))| { + unsafe { + // all arrays have size METADIM + *meta.get_unchecked_mut(idx) = (name, datatype); + *meta_offsets.get_unchecked_mut(idx) = 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; + if point_step_size * cloud.width as usize * cloud.height as usize != cloud.data.len() { + return Err(ConversionError::DataLengthMismatch); + } + + if let Some(max_offset) = offsets.last() { + if let Some(last_meta) = meta.last() { + let size_with_last_meta = max_offset + datatype_size(&last_meta.1); + if size_with_last_meta > point_step_size { + return Err(ConversionError::DataLengthMismatch); + } + } + } + + Ok(Self { + iteration: 0, + data: cloud.data, + point_step_size: point_step_size, + cloud_length: cloud.width as usize * cloud.height as usize, + offsets: offsets, + meta: meta, + endianness: endian, + phantom_t: std::marker::PhantomData, + phantom_c: std::marker::PhantomData, + }) + } +} diff --git a/src/writer.rs b/src/writer.rs new file mode 100644 index 0000000..3586c53 --- /dev/null +++ b/src/writer.rs @@ -0,0 +1,171 @@ +use crate::*; + +/// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`). +pub type WriterXYZ = Writer; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity (see `PointXYZI`). +pub type WriterXYZI = Writer; + +/// Writes a point cloud message from an iterator over xyz coordinates and normal (see `PointXYZNormal`). +pub type WriterXYZNormal = Writer; + +/// Writes a point cloud message from an iterator over xyz coordinates and packs the rgb channels (see `PointXYZRGB`). +pub type WriterXYZRGB = Writer; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity and packs the rgb channels (see `PointXYZRGBL`). +pub type WriterXYZRGBL = Writer; + +/// 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 = Writer; + +/// Writes a point cloud message from an iterator over xyz coordinates and normal and packs the rgb channels (see `PointXYZRGBNormal`). +pub type WriterXYZRGBNormal = Writer; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity and normal (see `PointXYZINormal`). +pub type WriterXYZINormal = Writer; + +/// Writes a point cloud message from an iterator over xyz coordinates and intensity and label (see `PointXYZL`). +pub type WriterXYZL = Writer; + +// eats an iterator, can only write from iterators to messages, so must not implement iterator pattern! +pub struct Writer +where + T: FromBytes, + C: PointConvertible, +{ + coordinates: Box>, // internal iterator + xyz_field_type: FieldDatatype, + phantom_t: std::marker::PhantomData, +} + +impl TryInto + for Writer +where + T: FromBytes, + C: PointConvertible, +{ + type Error = ConversionError; + + /// Convert the point cloud to a ROS message. + /// First use the `try_from` method for initializing the conversion and parsing meta data. + /// Then use the `try_into` method to do the actual conversion. + /// + /// # Example + /// ``` + /// use ros_pointcloud2::ros_types::PointCloud2Msg; + /// use ros_pointcloud2::ConvertXYZ; + /// use ros_pointcloud2::pcl_utils::PointXYZ; + /// + /// let cloud_points: Vec = 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::try_from(cloud_points).unwrap().try_into().unwrap(); + /// ``` + fn try_into(mut self) -> Result { + if DIM > 3 { + return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time? + } + + let datatype: u8 = convert_to_msg_code(&self.xyz_field_type); + 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".to_string(), + offset: 0, + datatype, + count: 1, + }); + fields.push(PointFieldMsg { + name: "y".to_string(), + offset: SIZE as u32, + datatype, + count: 1, + }); + } + + if DIM == 3 { + fields.push(PointFieldMsg { + name: "z".to_string(), + offset: 2 * SIZE as u32, + datatype, + count: 1, + }); + } + + let mut cloud = PointCloud2Msg::default(); + + // self was created using a point iterator + let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?; + let meta: [PointMeta; METADIM] = match first_point.clone().try_into() { + Ok(meta) => meta.1, + Err(_) => return Err(ConversionError::PointConversionError), + }; + + let meta_names = C::meta_names(); + + let sized_dim = DIM as u32 * SIZE as u32; + + for (idx, value) in meta.iter().enumerate() { + let datatype: u8 = convert_to_msg_code(&value.datatype); + let mut offset = sized_dim; + for i in 0..idx { + let datatype: u8 = convert_to_msg_code(&meta[i].datatype); + let field_type = convert_msg_code_to_type(datatype)?; + let field_size = datatype_size(&field_type); + offset += field_size as u32; + } + fields.push(PointFieldMsg { + name: meta_names + .get(idx) + .ok_or(ConversionError::MetaIndexLengthMismatch)? + .clone(), + offset, + datatype, + count: 1, + }); + } + + // calculate point size inside byte buffer + cloud.point_step = fields.iter().fold(0, |acc, field| { + let field_type = convert_msg_code_to_type(field.datatype).unwrap(); + let field_size = datatype_size(&field_type); + acc + field.count * field_size as u32 + }); + + // 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 + Writer +where + T: FromBytes, + C: PointConvertible, +{ + /// Create a new Writer struct from an iterator. + /// + /// # Example + /// ``` + /// ``` + pub fn from(coordinates: impl IntoIterator + 'static) -> Self { + Self { + coordinates: Box::new(coordinates.into_iter()), + xyz_field_type: FieldDatatype::F32, + phantom_t: std::marker::PhantomData, + } + } +} diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index c8c2a90..dfd9293 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -1,31 +1,22 @@ use fallible_iterator::FallibleIterator; -use ros_pointcloud2::mem_macros::size_of; use ros_pointcloud2::pcl_utils::*; +use ros_pointcloud2::reader::*; use ros_pointcloud2::ros_types::PointCloud2Msg; +use ros_pointcloud2::writer::*; use ros_pointcloud2::*; use std::fmt::Debug; -use std::{cmp, fmt}; -fn convert_from_into(cloud: Vec

) -where - C: FallibleIterator - + TryFrom - + TryFrom> - + TryInto, - ::Error: Debug, - >::Error: Debug, - >::Error: Debug, - >>::Error: Debug, - P: Clone + fmt::Debug + cmp::PartialEq, -{ - let copy = cloud.clone(); - let msg: Result = C::try_from(cloud).unwrap().try_into(); - assert!(msg.is_ok()); - let to_p_type = C::try_from(msg.unwrap()); - assert!(to_p_type.is_ok()); - let to_p_type = to_p_type.unwrap(); - let back_to_type = to_p_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); +macro_rules! convert_from_into { + ($reader:ty, $writer:ty, $point:ty, $cloud:expr) => { + let copy = $cloud.clone(); + let msg: Result = <$writer>::from($cloud).try_into(); + assert!(msg.is_ok()); + let to_p_type = <$reader>::try_from(msg.unwrap()); + assert!(to_p_type.is_ok()); + let to_p_type = to_p_type.unwrap(); + let back_to_type = to_p_type.map(|point| Ok(point)).collect::>(); + assert_eq!(copy, back_to_type.unwrap()); + }; } #[test] @@ -39,7 +30,8 @@ fn custom_xyz_f32() { y: f32, z: f32, } - type MyConverter = Convert; + type MyReader = Reader; + type MyWriter = Writer; impl From for ([f32; DIM], [PointMeta; METADIM]) { fn from(point: CustomPoint) -> Self { ([point.x, point.y, point.z], []) @@ -55,30 +47,35 @@ fn custom_xyz_f32() { }) } } - impl MetaNames for CustomPoint { + impl convert::MetaNames for CustomPoint { fn meta_names() -> [String; METADIM] { [] } } impl PointConvertible for CustomPoint {} - convert_from_into::(vec![ - CustomPoint { - x: 1.0, - y: 2.0, - z: 3.0, - }, - CustomPoint { - x: 4.0, - y: 5.0, - z: 6.0, - }, - CustomPoint { - x: 7.0, - y: 8.0, - z: 9.0, - }, - ]); + convert_from_into!( + MyReader, + MyWriter, + CustomPoint, + vec![ + CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + }, + CustomPoint { + x: 4.0, + y: 5.0, + z: 6.0, + }, + CustomPoint { + x: 7.0, + y: 8.0, + z: 9.0, + }, + ] + ); } #[test] @@ -92,12 +89,15 @@ fn custom_xyzi_f32() { z: f32, i: u8, } - type MyConverter = Convert; + type MyReader = reader::Reader; impl From for ([f32; DIM], [PointMeta; METADIM]) { fn from(point: CustomPoint) -> Self { ([point.x, point.y, point.z], [PointMeta::new(point.i)]) } } + + type MyWriter = writer::Writer; + impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { type Error = ConversionError; fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { @@ -109,13 +109,14 @@ fn custom_xyzi_f32() { }) } } - impl MetaNames for CustomPoint { + impl convert::MetaNames for CustomPoint { fn meta_names() -> [String; METADIM] { - ["intensity"].map(|s| s.to_string()) + [format!("intensity")] } } impl PointConvertible for CustomPoint {} - convert_from_into::(vec![ + + let cloud = vec![ CustomPoint { x: 0.0, y: 1.0, @@ -140,7 +141,8 @@ fn custom_xyzi_f32() { z: f32::MAX, i: u8::MAX, }, - ]); + ]; + convert_from_into!(MyReader, MyWriter, CustomPoint, cloud); } #[test] @@ -157,7 +159,8 @@ fn custom_rgba_f32() { b: u8, a: u8, } - type MyConverter = Convert; + type MyReader = reader::Reader; + type MyWriter = writer::Writer; impl From for ([f32; DIM], [PointMeta; METADIM]) { fn from(point: CustomPoint) -> Self { ( @@ -185,13 +188,13 @@ fn custom_rgba_f32() { }) } } - impl MetaNames for CustomPoint { + impl convert::MetaNames for CustomPoint { fn meta_names() -> [String; METADIM] { ["r", "g", "b", "a"].map(|s| s.to_string()) } } impl PointConvertible for CustomPoint {} - convert_from_into::(vec![ + let cloud = vec![ CustomPoint { x: 0.0, y: 1.0, @@ -228,7 +231,8 @@ fn custom_rgba_f32() { b: u8::MAX, a: u8::MAX, }, - ]); + ]; + convert_from_into!(MyReader, MyWriter, CustomPoint, cloud); } #[test] @@ -256,317 +260,357 @@ fn converterxyz() { }, ]; - convert_from_into::(cloud); + convert_from_into!(ReaderXYZ, WriterXYZ, PointXYZ, cloud); } #[test] fn converterxyzrgba() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } #[test] fn converterxyzinormal() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } #[test] fn converterxyzrgbnormal() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } #[test] fn converterxyznormal() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } #[test] fn converterxyzrgbl() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } #[test] fn converterxyzrgb() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } #[test] fn converterxyzl() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } #[test] fn converterxyzi() { - convert_from_into::(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, - }, - ]); + 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, + }, + ] + ); } diff --git a/tests/r2r_msg_test.rs b/tests/r2r_msg_test.rs index fb430ed..a020f7d 100644 --- a/tests/r2r_msg_test.rs +++ b/tests/r2r_msg_test.rs @@ -1,11 +1,11 @@ #[cfg(feature = "r2r_msg")] #[test] fn convertxyz_r2r_msg() { + use r2r::sensor_msgs::msg::PointCloud2; use ros_pointcloud2::fallible_iterator::FallibleIterator; use ros_pointcloud2::pcl_utils::PointXYZ; use ros_pointcloud2::ros_types::PointCloud2Msg; use ros_pointcloud2::ConvertXYZ; - use r2r::sensor_msgs::msg::PointCloud2; let cloud = vec![ PointXYZ { From 77c222414fa1eb77c98a3173b3a5ce2ef5e9cbe1 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 29 Apr 2024 17:27:32 +0200 Subject: [PATCH 07/11] redo api --- Cargo.toml | 1 - README.md | 78 ++++++--- src/convert.rs | 131 +++++++------- src/lib.rs | 147 +++++++++++----- src/pcl_utils.rs | 358 ++++++++++++++++++-------------------- src/reader.rs | 256 ++++++++++++++++++--------- src/writer.rs | 231 ++++++++++++++++++------ tests/e2e_test.rs | 302 +++++++++++++++++++++++++------- tests/r2r_msg_test.rs | 12 +- tests/rosrust_msg_test.rs | 11 +- 10 files changed, 989 insertions(+), 538 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 8c2626e..a70ded8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,7 +14,6 @@ homepage = "https://github.com/stelzo/ros_pointcloud2" exclude = ["**/tests/**", "**/examples/**", "**/benches/**", "**/target/**", "**/build/**", "**/dist/**", "**/docs/**", "**/doc/**"] [dependencies] -fallible-iterator = "0.3.0" rosrust_msg = { version = "0.1", optional = true } rosrust = { version = "0.9.11", optional = true } r2r = { version = "0.8.4", optional = true } diff --git a/README.md b/README.md index 01d5766..c6c78de 100644 --- a/README.md +++ b/README.md @@ -5,16 +5,14 @@

-Providing a memory efficient way for message conversion while allowing user defined types without the cost of iterations. -Instead of converting the entire cloud into a `Vec`, you get an iterable type that converts each point from the message on the fly. +Providing a easy to use, generics defined, point-wise iterator abstraction of the byte buffer of `PointCloud2` to minimize iterations in your processing pipeline. -To keep the crate a general purpose library for the problem and support ROS1 and ROS2, it uses its own type for the message `ros_types::PointCloud2Msg`. +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`. + +## Example ```rust use ros_pointcloud2::{ - fallible_iterator::FallibleIterator, - pcl_utils::PointXYZ, - ros_types::PointCloud2Msg, - ConvertXYZ, + pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, }; // Your points (here using the predefined type PointXYZ). @@ -31,32 +29,41 @@ let cloud_points = vec![ }, ]; -let cloud_copy = cloud_points.clone(); // For checking equality later. +// For equality test later +let cloud_copy = cloud_points.clone(); -// Vector -> Converter -> Message -let internal_msg: PointCloud2Msg = ConvertXYZ::try_from(cloud_points) - .unwrap() - .try_into() +// Some changes to demonstrate lazy iterator usage +let changed_it = cloud_points.iter().map(|p| { + p.x = 0.5; +}); + +// Vector -> Writer -> Message +let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points) + .try_into() // iterating points here O(n) .unwrap(); -// Convert to your favorite ROS crate message type, we will use rosrust here. -// let msg: rosrust_msg::sensor_msgs::PointCloud2 = internal_msg.into(); +// Anything that implements `IntoIterator` also works - like another iterator. +let internal_msg_changed: PointCloud2Msg = WriterXYZ::from(changed_it) + .try_into() // iterating points here O(n) + .unwrap(); -// Back to this crate's message type. +// 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 -> Converter -> Vector -let convert: ConvertXYZ = ConvertXYZ::try_from(internal_msg).unwrap(); -let new_cloud_points = convert +// Message -> Reader. The Reader implements the Iterator trait. +let reader = ReaderXYZ::try_from(internal_msg).unwrap(); +let new_cloud_points = reader .map(|point: PointXYZ| { - // Insert your point business logic here - // or use other methods like .for_each(). - - // We are using a fallible iterator so we need to return a Result. - Ok(point) + // Some logic here + + point }) - .collect::>() - .unwrap(); // Handle point conversion or byte errors here. + .collect::>(); assert_eq!(new_cloud_points, cloud_copy); ``` @@ -226,10 +233,25 @@ let to_custom_type = MyConverter::try_from(custom_msg).unwrap(); ## Future Work - Benchmark vs PCL -- Proper error passing to the iterator result (currently merged into `PointConversionError`) -- remove allocations -- introduce no-panic for maximum stability - Add more predefined types +- Optional derive macros for custom point implementations + +## Changelog 1.0.0 +- `Convert` is now `Reader` and `Writer`. +`Convert` had to translate both ways and keep a different state depending on the way it was created. This led to an edge case where the user could create a point cloud that always returns uninitialized points. While the reproduction was unlikely to occur in normal use cases, the possibility of it alone is unnecessary ambiguity. +The new `Reader` and `Writer` not only eliminates ambiguity but also makes the code much more readable and concise. +- Zero-cost iterator abstraction. `Reader` and `Writer` now either take or implement iterable traits. This means, you can read a message, implement a `filter` function and pass the resulting iterator directly to the `Writer`. The entire pipeline is then compiled to a typesafe message-to-message filter with a single iteration. +- Divide error types to message and human errors. +Corrupted messages with valid descriptions and correct byte buffer length can not be classified as corrupted at runtime, thus point conversions can be assumed to always work when passing these checks. There now are less error types and the point conversions within the iteration can omit the `try_*` prefix. Discrepancies within the point cloud message and the described point are checked at `Reader` and `Writer` creation instead. +Human errors are possible with wrong point types or internal crate bugs that could lead to out-of-bound panics. These cases are checked in debug mode via debug_assert now. In release builds, the crate fully leans into performance optimizations and assumes a correct type description via generics (mainly the first 2 parameters coord_type and sizeof(coord_type)). Previously, these errors resulted in an error at runtime but the only possible source for them is the crate user, so the decision was made to promote them to a panic like a compile error. TODO check coord_type and sizeof at compile time statically? +- More Documentation and more relevant examples for every function and type. The example for custom points is moved from the Readme into the documentation as well. +- Performance and efficiency. By leaning into iterators whenever possible, the amount of bound checks have been reduced. There are less iterations in every function and dynamic memory usage is reduced. +- Removed dependencies. By switching to normal iterators and intializing different start values, the crate only depends on the std now. + +API changes +- replace every use of `([T; DIM], [ros_pointcloud2::PointMeta; METADIM])` with `ros_pointcloud2::Point` +- + ## License [MIT](https://choosealicense.com/licenses/mit/) diff --git a/src/convert.rs b/src/convert.rs index d57a4d6..7a59c7f 100644 --- a/src/convert.rs +++ b/src/convert.rs @@ -19,6 +19,21 @@ impl Default for FieldDatatype { } } +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, + } + } +} + /// Getter trait for the datatype of a field value. pub trait GetFieldDatatype { fn field_datatype() -> FieldDatatype; @@ -72,30 +87,36 @@ impl GetFieldDatatype for i16 { } } -pub(crate) fn convert_to_msg_code(geo_type: &FieldDatatype) -> u8 { - match geo_type { - FieldDatatype::I8 => 1, - FieldDatatype::U8 => 2, - FieldDatatype::I16 => 3, - FieldDatatype::U16 => 4, - FieldDatatype::I32 => 5, - FieldDatatype::U32 => 6, - FieldDatatype::F32 => 7, - FieldDatatype::F64 => 8, +impl TryFrom for FieldDatatype { + type Error = ConversionError; + + fn try_from(value: u8) -> Result { + match value { + 1 => Ok(FieldDatatype::I8), + 2 => Ok(FieldDatatype::U8), + 3 => Ok(FieldDatatype::I16), + 4 => Ok(FieldDatatype::U16), + 5 => Ok(FieldDatatype::I32), + 6 => Ok(FieldDatatype::U32), + 7 => Ok(FieldDatatype::F32), + 8 => Ok(FieldDatatype::F64), + _ => Err(ConversionError::UnsupportedFieldType), + } } } -pub(crate) fn convert_msg_code_to_type(code: u8) -> Result { - match code { - 7 => Ok(FieldDatatype::F32), - 8 => Ok(FieldDatatype::F64), - 5 => Ok(FieldDatatype::I32), - 2 => Ok(FieldDatatype::U8), - 4 => Ok(FieldDatatype::U16), - 6 => Ok(FieldDatatype::U32), - 1 => Ok(FieldDatatype::I8), - 3 => Ok(FieldDatatype::I16), - _ => Err(ConversionError::UnsupportedFieldType), +impl Into for FieldDatatype { + fn into(self) -> u8 { + match self { + FieldDatatype::I8 => 1, + FieldDatatype::U8 => 2, + FieldDatatype::I16 => 3, + FieldDatatype::U16 => 4, + FieldDatatype::I32 => 5, + FieldDatatype::U32 => 6, + FieldDatatype::F32 => 7, + FieldDatatype::F64 => 8, + } } } @@ -107,7 +128,7 @@ pub(crate) fn check_coord( match coord { Some(y_idx) => { let field = &fields[y_idx]; - if field.datatype != convert_to_msg_code(xyz_field_type) { + if field.datatype != (*xyz_field_type).into() { return Err(ConversionError::InvalidFieldFormat); } Ok(field.clone()) @@ -122,19 +143,6 @@ pub trait MetaNames { fn meta_names() -> [String; METADIM]; } -pub(crate) fn datatype_size(datatype: &FieldDatatype) -> usize { - match datatype { - FieldDatatype::U8 => 1, - FieldDatatype::U16 => 2, - FieldDatatype::U32 => 4, - FieldDatatype::I8 => 1, - FieldDatatype::I16 => 2, - FieldDatatype::I32 => 4, - FieldDatatype::F32 => 4, - FieldDatatype::F64 => 8, - } -} - #[inline(always)] pub(crate) fn add_point_to_byte_buffer< T: FromBytes, @@ -146,19 +154,17 @@ pub(crate) fn add_point_to_byte_buffer< coords: C, cloud: &mut PointCloud2Msg, ) -> Result { - let (coords_data, coords_meta): ([T; DIM], [PointMeta; METADIM]) = match coords.try_into() { - Ok(meta) => meta, - Err(_) => return Err(ConversionError::PointConversionError), - }; + let point: Point = coords.into(); // (x, y, z...) - coords_data + point + .coords .iter() .for_each(|x| cloud.data.extend_from_slice(T::bytes(x).as_slice())); // meta data description - coords_meta.iter().for_each(|meta| { - let truncated_bytes = &meta.bytes[0..datatype_size(&meta.datatype)]; + point.meta.iter().for_each(|meta| { + let truncated_bytes = &meta.bytes[0..meta.datatype.size()]; cloud.data.extend_from_slice(truncated_bytes); }); @@ -300,35 +306,28 @@ pub(crate) fn load_loadable( start_idx: usize, data: &[u8], endian: &Endianness, -) -> Option { +) -> T { match endian { - Endianness::Big => Some(T::from_be_bytes( - load_bytes::(start_idx, data)?.as_slice(), - )), - Endianness::Little => Some(T::from_le_bytes( - load_bytes::(start_idx, data)?.as_slice(), - )), + Endianness::Big => T::from_be_bytes(load_bytes::(start_idx, data).as_slice()), + Endianness::Little => T::from_le_bytes(load_bytes::(start_idx, data).as_slice()), } } -fn load_bytes(start_idx: usize, data: &[u8]) -> Option<[u8; S]> { - if start_idx + S > data.len() { - return None; - } - let mut buff: [u8; S] = [0; S]; - for (byte, buff_val) in buff.iter_mut().enumerate().take(S) { - let raw_byte = data.get(start_idx + byte); - match raw_byte { - None => { - return None; - } - Some(some_byte) => { - *buff_val = *some_byte; - } - } - } +/// Note: check if the data slice is long enough to load the bytes beforehand! +fn load_bytes(start_idx: usize, data: &[u8]) -> [u8; S] { + let mut target = [u8::default(); S]; - Some(buff) + 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)] diff --git a/src/lib.rs b/src/lib.rs index 7375ddd..19e1a10 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,13 +1,18 @@ -// TODO test reading writing different types -// TODO read less info from the point cloud than is available -// TODO write less info to the point cloud than is available -// TODO explain templating arguments more for reader and writer - -#![doc = include_str!("../README.md")] +//#![doc = include_str!("../README.md")] pub mod convert; pub mod pcl_utils; pub mod ros_types; +/// Macro to get the size of a type at compile time. This is a convenience macro to avoid writing out the full std::mem::size_of::(). +/// Use it for your custom Reader and Writer implementations. +/// # Example +/// ``` +/// use ros_pointcloud2::{ +/// writer::Writer, size_of, pcl_utils::PointXYZ, +/// }; +/// +/// type MyWriterXYZ = Writer; +/// ``` #[macro_export] macro_rules! size_of { ($t:ty) => { @@ -24,37 +29,43 @@ use crate::convert::*; use crate::pcl_utils::*; use crate::ros_types::PointFieldMsg; -pub use fallible_iterator::FallibleIterator; - -/// Errors that can occur when converting between PointCloud2 and a custom type. +/// 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, + + /// Not enough meta or dimensional fields in the PointCloud2 message. NotEnoughFields, + + /// The dimensionality of the point inside the message is too high. TooManyDimensions, + + /// The field type is not supported by the ROS message description. UnsupportedFieldType, + + /// There are no points in the point cloud. NoPoints, + + /// The length of the byte buffer in the message does not match the expected length computed from the fields. DataLengthMismatch, - MetaIndexLengthMismatch, - EndOfBuffer, - PointConversionError, - MetaDatatypeMismatch, +} + +pub struct Point { + pub coords: [T; DIM], + pub meta: [PointMeta; METADIM], } /// Trait to convert a point to a tuple of coordinates and meta data. /// Implement this trait for your point type to use the conversion. pub trait PointConvertible: - TryInto<([T; DIM], [PointMeta; METADIM])> - + TryFrom<([T; DIM], [PointMeta; METADIM])> - + MetaNames - + Clone - + 'static + From> + Into> + MetaNames + Clone + 'static where T: FromBytes, { } -/// Meta data representation for a point +/// Metadata representation for a point. /// /// This struct is used to store meta data in a fixed size byte buffer along the with the /// datatype that is encoded so that it can be decoded later. @@ -65,7 +76,7 @@ where /// /// let original_data: f64 = 1.0; /// let meta = PointMeta::new(original_data); -/// let my_data: f64 = meta.get().unwrap(); +/// let my_data: f64 = meta.get(); /// ``` #[derive(Debug, Clone, Copy)] pub struct PointMeta { @@ -92,51 +103,93 @@ impl PointMeta { pub fn new(value: T) -> Self { let raw_bytes = T::bytes(&value); let mut bytes = [0; std::mem::size_of::()]; - for (idx, byte) in raw_bytes.iter().enumerate() { - bytes[idx] = *byte; + for (byte, save_byte) in raw_bytes.into_iter().zip(bytes.iter_mut()) { + *save_byte = byte; } + Self { bytes, datatype: T::field_datatype(), } } - fn new_from_buffer( - data: &[u8], - offset: usize, - datatype: &FieldDatatype, - ) -> Result { - let size = datatype_size(datatype); - let bytes = data - .get(offset..offset + size) - .ok_or(ConversionError::DataLengthMismatch)?; + fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self { + debug_assert!(data.len() >= offset + datatype.size()); + + let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) }; let mut bytes_array = [0; std::mem::size_of::()]; // 8 bytes as f64 is the largest type - for (idx, byte) in bytes.iter().enumerate() { - bytes_array[idx] = *byte; + for (byte, save_byte) in bytes.into_iter().zip(bytes_array.iter_mut()) { + *save_byte = *byte; } - Ok(Self { + + Self { bytes: bytes_array, datatype: *datatype, - }) + } } - /// Get the value from the PointMeta. It will return None if the datatype does not match. + /// Get the numeric value from the PointMeta description. /// /// # Example /// ``` /// let original_data: f64 = 1.0; /// let meta = ros_pointcloud2::PointMeta::new(original_data); - /// let my_data: f64 = meta.get().unwrap(); + /// let my_data: f64 = meta.get(); /// ``` - pub fn get(&self) -> Result { - if self.datatype != T::field_datatype() { - return Err(ConversionError::MetaDatatypeMismatch); - } - let size = datatype_size(&T::field_datatype()); - if let Some(bytes) = self.bytes.get(0..size) { - Ok(T::from_le_bytes(bytes)) - } else { - Err(ConversionError::DataLengthMismatch) // self.bytes are not long enough, already handled while parsing - } + pub fn get(&self) -> T { + let size = T::field_datatype().size(); + let bytes = self + .bytes + .get(0..size) + .expect("Exceeds bounds of f64, which is the largest type"); + T::from_le_bytes(bytes) + } +} + +impl From for PointMeta { + fn from(value: f32) -> Self { + Self::new(value) + } +} + +impl From for PointMeta { + fn from(value: f64) -> Self { + Self::new(value) + } +} + +impl From for PointMeta { + fn from(value: i32) -> Self { + Self::new(value) + } +} + +impl From for PointMeta { + fn from(value: u8) -> Self { + Self::new(value) + } +} + +impl From for PointMeta { + fn from(value: u16) -> Self { + Self::new(value) + } +} + +impl From for PointMeta { + fn from(value: u32) -> Self { + Self::new(value) + } +} + +impl From for PointMeta { + fn from(value: i8) -> Self { + Self::new(value) + } +} + +impl From for PointMeta { + fn from(value: i16) -> Self { + Self::new(value) } } diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs index 1a4a333..a8fa2a0 100644 --- a/src/pcl_utils.rs +++ b/src/pcl_utils.rs @@ -1,11 +1,13 @@ -use crate::{ConversionError, MetaNames, PointConvertible, PointMeta}; +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(); @@ -24,23 +26,22 @@ pub struct PointXYZ { pub z: f32, } -impl TryInto<([f32; 3], [PointMeta; 0])> for PointXYZ { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 0]), Self::Error> { - Ok(([self.x, self.y, self.z], [])) +impl From> for PointXYZ { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + } } } -impl TryFrom<([f32; 3], [PointMeta; 0])> for PointXYZ { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 0])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - }) +impl Into> for PointXYZ { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: Default::default(), + } } } @@ -62,24 +63,23 @@ pub struct PointXYZI { pub intensity: f32, } -impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZI { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { - Ok(([self.x, self.y, self.z], [PointMeta::new(self.intensity)])) +impl From> for PointXYZI { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + intensity: point.meta[0].get(), + } } } -impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZI { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - intensity: data.1[0].get()?, - }) +impl Into> for PointXYZI { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [self.intensity.into()], + } } } @@ -103,29 +103,28 @@ pub struct PointXYZRGB { pub b: u8, } -impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZRGB { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(([self.x, self.y, self.z], [PointMeta::new(rgb)])) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZRGB { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGB { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + 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 Into> for PointXYZRGB { + fn into(self) -> Point { + let rgb = pack_rgb(self.r, self.g, self.b); + Point { + coords: [self.x, self.y, self.z], + meta: [rgb.into()], + } } } @@ -151,33 +150,29 @@ pub struct PointXYZRGBA { pub a: u8, } -impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBA { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(( - [self.x, self.y, self.z], - [PointMeta::new(rgb), PointMeta::new(self.a)], - )) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBA { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGBA { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + 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: data.1[1].get()?, - }) + a: point.meta[1].get(), + } + } +} + +impl Into> for PointXYZRGBA { + fn into(self) -> Point { + let rgb = pack_rgb(self.r, self.g, self.b); + Point { + coords: [self.x, self.y, self.z], + meta: [rgb.into(), self.a.into()], + } } } @@ -204,40 +199,36 @@ pub struct PointXYZRGBNormal { pub normal_z: f32, } -impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(( - [self.x, self.y, self.z], - [ - PointMeta::new(rgb), - PointMeta::new(self.normal_x), - PointMeta::new(self.normal_y), - PointMeta::new(self.normal_z), - ], - )) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZRGBNormal { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGBNormal { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + 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: data.1[1].get()?, - normal_y: data.1[2].get()?, - normal_z: data.1[3].get()?, - }) + normal_x: point.meta[1].get(), + normal_y: point.meta[2].get(), + normal_z: point.meta[3].get(), + } + } +} + +impl Into> for PointXYZRGBNormal { + fn into(self) -> Point { + let rgb = pack_rgb(self.r, self.g, self.b); + Point { + coords: [self.x, self.y, self.z], + meta: [ + rgb.into(), + self.normal_x.into(), + self.normal_y.into(), + self.normal_z.into(), + ], + } } } @@ -262,35 +253,31 @@ pub struct PointXYZINormal { pub normal_z: f32, } -impl TryInto<([f32; 3], [PointMeta; 4])> for PointXYZINormal { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 4]), Self::Error> { - Ok(( - [self.x, self.y, self.z], - [ - PointMeta::new(self.intensity), - PointMeta::new(self.normal_x), - PointMeta::new(self.normal_y), - PointMeta::new(self.normal_z), - ], - )) +impl From> for PointXYZINormal { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + intensity: point.meta[0].get(), + normal_x: point.meta[1].get(), + normal_y: point.meta[2].get(), + normal_z: point.meta[3].get(), + } } } -impl TryFrom<([f32; 3], [PointMeta; 4])> for PointXYZINormal { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 4])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - intensity: data.1[0].get()?, - normal_x: data.1[1].get()?, - normal_y: data.1[2].get()?, - normal_z: data.1[3].get()?, - }) +impl Into> for PointXYZINormal { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [ + self.intensity.into(), + self.normal_x.into(), + self.normal_y.into(), + self.normal_z.into(), + ], + } } } @@ -312,24 +299,23 @@ pub struct PointXYZL { pub label: u32, } -impl TryInto<([f32; 3], [PointMeta; 1])> for PointXYZL { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 1]), Self::Error> { - Ok(([self.x, self.y, self.z], [PointMeta::new(self.label)])) +impl From> for PointXYZL { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + label: point.meta[0].get(), + } } } -impl TryFrom<([f32; 3], [PointMeta; 1])> for PointXYZL { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 1])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - label: data.1[0].get()?, - }) +impl Into> for PointXYZL { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [self.label.into()], + } } } @@ -354,33 +340,29 @@ pub struct PointXYZRGBL { pub label: u32, } -impl TryInto<([f32; 3], [PointMeta; 2])> for PointXYZRGBL { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 2]), Self::Error> { - let rgb = pack_rgb(self.r, self.g, self.b); - Ok(( - [self.x, self.y, self.z], - [PointMeta::new(rgb), PointMeta::new(self.label)], - )) - } -} - -impl TryFrom<([f32; 3], [PointMeta; 2])> for PointXYZRGBL { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 2])) -> Result { - let rgb = data.1[0].get::()?; +impl From> for PointXYZRGBL { + fn from(point: Point) -> Self { + let rgb = point.meta[0].get::(); let rgb_unpacked = unpack_rgb(rgb); - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], + 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: data.1[1].get()?, - }) + label: point.meta[1].get(), + } + } +} + +impl Into> for PointXYZRGBL { + fn into(self) -> Point { + let rgb = pack_rgb(self.r, self.g, self.b); + Point { + coords: [self.x, self.y, self.z], + meta: [rgb.into(), self.label.into()], + } } } @@ -404,33 +386,29 @@ pub struct PointXYZNormal { pub normal_z: f32, } -impl TryInto<([f32; 3], [PointMeta; 3])> for PointXYZNormal { - type Error = ConversionError; - - fn try_into(self) -> Result<([f32; 3], [PointMeta; 3]), Self::Error> { - Ok(( - [self.x, self.y, self.z], - [ - PointMeta::new(self.normal_x), - PointMeta::new(self.normal_y), - PointMeta::new(self.normal_z), - ], - )) +impl From> for PointXYZNormal { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + normal_x: point.meta[0].get(), + normal_y: point.meta[1].get(), + normal_z: point.meta[2].get(), + } } } -impl TryFrom<([f32; 3], [PointMeta; 3])> for PointXYZNormal { - type Error = ConversionError; - - fn try_from(data: ([f32; 3], [PointMeta; 3])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - normal_x: data.1[0].get()?, - normal_y: data.1[1].get()?, - normal_z: data.1[2].get()?, - }) +impl Into> for PointXYZNormal { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [ + self.normal_x.into(), + self.normal_y.into(), + self.normal_z.into(), + ], + } } } diff --git a/src/reader.rs b/src/reader.rs index f57aa3a..c81860c 100644 --- a/src/reader.rs +++ b/src/reader.rs @@ -1,50 +1,85 @@ use crate::*; +use convert::*; + +/// 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 = + Reader() }, 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 = + Reader() }, 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 = + Reader() }, 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 = + Reader() }, 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 = + Reader() }, 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 = + Reader() }, 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 = + Reader() }, 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 = + Reader() }, DIM, METADIM, C>; /// Provides the message as an Iterator over xyz coordinates (see `PointXYZ`). /// Every additional meta data is ignored. -pub type ReaderXYZ = Reader; +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 = Reader; +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 = Reader; +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 = Reader; +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 = Reader; +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 = Reader; +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 = Reader; +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 = Reader; +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 = Reader; +pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>; -/// The Reader provides an abstraction around PointCloud2Msg by implementing a fallible iterator around the data buffer. +/// 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. -/// At runtime, the iterator catches read errors from the Msg type which typically only occur with corrupted data. +/// 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. @@ -59,29 +94,84 @@ pub type ReaderXYZL = Reader; /// /// ros_pointcloud2 supports r2r, ros2_rust and rosrust as conversion targets out of the box via feature flags. /// -/// # Example +/// ## Example /// ``` /// use ros_pointcloud2::{ -/// FallibleIterator, // needed for for_each and the other iterator methods -/// reader::ReaderXYZ, -/// writer::WriterXYZ, -/// PointCloud2Msg, -/// pcl_utils::PointXYZ, +/// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, /// }; /// +/// /// let cloud_points: Vec = 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(); // parse message +/// 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 -/// Ok(()) // return Ok to continue iteration -/// }) -/// .unwrap(); // handle point conversion errors +/// }); +/// ``` +/// # 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, convert::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> for CustomPoint { +/// fn from(point: Point) -> Self { +/// Self { +/// x: point.coords[0], +/// y: point.coords[1], +/// z: point.coords[2], +/// i: point.meta[0].get(), +/// } +/// } +/// } +/// +///impl Into> for CustomPoint { +/// fn into(self) -> Point { +/// Point { +/// coords: [self.x, self.y, self.z], +/// meta: [self.i.into()], +/// } +/// } +///} +/// +/// impl MetaNames for CustomPoint { +/// fn meta_names() -> [String; METADIM] { +/// [format!("intensity")] +/// } +/// } +/// +/// impl PointConvertible for CustomPoint {} +/// +/// type MyReader = Reader; +/// // ^^^^^^^^ your custom Reader +/// type MyWriter = Writer; /// ``` pub struct Reader where @@ -103,14 +193,13 @@ where /// 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 FallibleIterator +impl Iterator for Reader where T: FromBytes, C: PointConvertible, { type Item = C; - type Error = ConversionError; /// The size_hint is the length of the remaining elements and the maximum length of the iterator. /// @@ -124,41 +213,40 @@ where /// 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) -> Result, Self::Error> { + fn next(&mut self) -> Option { if self.iteration >= self.cloud_length { - return Ok(None); // iteration finished + return None; // iteration finished } let mut xyz: [T; DIM] = [T::default(); DIM]; - let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM]; - for (idx, in_point_offset) in self.offsets.iter().enumerate() { - if idx < DIM { - match load_loadable::( + xyz.iter_mut() + .zip(self.offsets.iter()) + .for_each(|(p_xyz, in_point_offset)| { + *p_xyz = load_loadable::( (self.iteration * self.point_step_size) + in_point_offset, &self.data, &self.endianness, - ) { - Some(c) => xyz[idx] = c, - None => return Err(ConversionError::EndOfBuffer), - } - } else { - let meta_idx = idx - DIM; - let meta_type = self.meta[meta_idx].1; + ); + }); + + debug_assert!(self.meta.len() == METADIM, "Meta data length mismatch"); + debug_assert!( + self.offsets.len() == DIM + METADIM, + "Offset length mismatch" + ); + + let mut meta: [PointMeta; METADIM] = [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; - if let Ok(m) = PointMeta::new_from_buffer(&self.data, start, &meta_type) { - meta[meta_idx] = m; - } else { - return Err(ConversionError::MetaIndexLengthMismatch); - } - } - } + *p_meta = PointMeta::from_buffer(&self.data, start, meta_type); + }); self.iteration += 1; - match C::try_from((xyz, meta)) { - Err(_) => Err(ConversionError::PointConversionError), - Ok(tuple) => Ok(Some(tuple)), - } + Some(C::from(Point { coords: xyz, meta })) } } @@ -179,11 +267,7 @@ where /// # Example /// ``` /// use ros_pointcloud2::{ - /// FallibleIterator, // needed for for_each and the other iterator methods - /// reader::ReaderXYZ, - /// writer::WriterXYZ, - /// PointCloud2Msg, - /// pcl_utils::PointXYZ, + /// pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, /// }; /// /// let cloud_points: Vec = vec![ @@ -222,33 +306,36 @@ where "z" => has_z = Some(idx), _ => { if lower_meta_names.contains(&lower_field_name) { - println!("{} {} {}", idx, field.name, meta_with_offsets.len()); - match meta_with_offsets.get_mut(idx - DIM) { - None => return Err(ConversionError::MetaIndexLengthMismatch), - Some(data) => { - data.0 = field.name.clone(); - data.1 = convert_msg_code_to_type(field.datatype)?; - data.2 = field.offset as usize; - } - } + 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(|a, b| a.2.cmp(&b.2)); + 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, FieldDatatype)> = - vec![(String::default(), FieldDatatype::U8); METADIM]; + let mut meta = vec![(String::default(), FieldDatatype::default()); METADIM]; + meta_with_offsets .into_iter() - .enumerate() - .for_each(|(idx, (name, datatype, offset))| { - unsafe { - // all arrays have size METADIM - *meta.get_unchecked_mut(idx) = (name, datatype); - *meta_offsets.get_unchecked_mut(idx) = offset; - }; + .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)?; @@ -288,26 +375,31 @@ where } let point_step_size = cloud.point_step as usize; - if point_step_size * cloud.width as usize * cloud.height as usize != cloud.data.len() { + let cloud_length = cloud.width as usize * cloud.height as usize; + if point_step_size * cloud_length != cloud.data.len() { return Err(ConversionError::DataLengthMismatch); } - if let Some(max_offset) = offsets.last() { - if let Some(last_meta) = meta.last() { - let size_with_last_meta = max_offset + datatype_size(&last_meta.1); - if size_with_last_meta > point_step_size { - 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: point_step_size, + point_step_size, cloud_length: cloud.width as usize * cloud.height as usize, - offsets: offsets, - meta: meta, + offsets, + meta, endianness: endian, phantom_t: std::marker::PhantomData, phantom_c: std::marker::PhantomData, diff --git a/src/writer.rs b/src/writer.rs index 3586c53..c614d5f 100644 --- a/src/writer.rs +++ b/src/writer.rs @@ -1,40 +1,163 @@ use crate::*; +/// 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 = + Writer() }, 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 = + Writer() }, 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 = + Writer() }, 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 = + Writer() }, 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 = + Writer() }, 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 = + Writer() }, 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 = + Writer() }, 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 = + Writer() }, DIM, METADIM, C>; + /// Writes a point cloud message from an iterator over xyz coordinates (see `PointXYZ`). -pub type WriterXYZ = Writer; +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 = Writer; +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 = Writer; +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 = Writer; +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 = Writer; +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 = Writer; +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 = Writer; +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 = Writer; +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 = Writer; +pub type WriterXYZL = WriterF32<3, 1, PointXYZL>; -// eats an iterator, can only write from iterators to messages, so must not implement iterator pattern! +/// 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 = 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, convert::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> for CustomPoint { +/// fn from(point: Point) -> Self { +/// Self { +/// x: point.coords[0], +/// y: point.coords[1], +/// z: point.coords[2], +/// i: point.meta[0].get(), +/// } +/// } +/// } +/// +///impl Into> for CustomPoint { +/// fn into(self) -> Point { +/// Point { +/// coords: [self.x, self.y, self.z], +/// meta: [self.i.into()], +/// } +/// } +///} +/// +/// impl MetaNames for CustomPoint { +/// fn meta_names() -> [String; METADIM] { +/// [format!("intensity")] +/// } +/// } +/// impl PointConvertible for CustomPoint {} +/// +/// type MyReader = Reader; +/// type MyWriter = Writer; +/// // ^^^^^^^^ your custom Writer +/// ``` pub struct Writer where T: FromBytes, C: PointConvertible, { - coordinates: Box>, // internal iterator - xyz_field_type: FieldDatatype, + coordinates: Box>, phantom_t: std::marker::PhantomData, } @@ -46,41 +169,45 @@ where { type Error = ConversionError; - /// Convert the point cloud to a ROS message. - /// First use the `try_from` method for initializing the conversion and parsing meta data. + /// 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::ros_types::PointCloud2Msg; - /// use ros_pointcloud2::ConvertXYZ; - /// use ros_pointcloud2::pcl_utils::PointXYZ; + /// use ros_pointcloud2::{ + /// pcl_utils::PointXYZ, writer::WriterXYZ, PointCloud2Msg, + /// }; /// /// let cloud_points: Vec = 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::try_from(cloud_points).unwrap().try_into().unwrap(); + /// let msg_out: PointCloud2Msg = WriterXYZ::from(cloud_points).try_into().unwrap(); + /// // ^^^^^^^^ ROS message conversion /// ``` fn try_into(mut self) -> Result { if DIM > 3 { return Err(ConversionError::TooManyDimensions); // maybe can be checked at compile time? } - let datatype: u8 = convert_to_msg_code(&self.xyz_field_type); 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".to_string(), offset: 0, - datatype, + datatype: T::field_datatype().into(), count: 1, }); + fields.push(PointFieldMsg { name: "y".to_string(), offset: SIZE as u32, - datatype, + datatype: T::field_datatype().into(), count: 1, }); } @@ -89,48 +216,37 @@ where fields.push(PointFieldMsg { name: "z".to_string(), offset: 2 * SIZE as u32, - datatype, + datatype: T::field_datatype().into(), count: 1, }); } - let mut cloud = PointCloud2Msg::default(); - // self was created using a point iterator let first_point = self.coordinates.next().ok_or(ConversionError::NoPoints)?; - let meta: [PointMeta; METADIM] = match first_point.clone().try_into() { - Ok(meta) => meta.1, - Err(_) => return Err(ConversionError::PointConversionError), - }; - + let point: Point = first_point.clone().into(); let meta_names = C::meta_names(); - let sized_dim = DIM as u32 * SIZE as u32; - - for (idx, value) in meta.iter().enumerate() { - let datatype: u8 = convert_to_msg_code(&value.datatype); - let mut offset = sized_dim; - for i in 0..idx { - let datatype: u8 = convert_to_msg_code(&meta[i].datatype); - let field_type = convert_msg_code_to_type(datatype)?; - let field_size = datatype_size(&field_type); - offset += field_size as u32; + 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_names - .get(idx) - .ok_or(ConversionError::MetaIndexLengthMismatch)? - .clone(), - offset, - datatype, + name: meta_name, + offset: meta_offsets_acc, + datatype: datatype_code, count: 1, }); - } + meta_offsets_acc += meta_value.datatype.size() as u32 + }; - // calculate point size inside byte buffer + let mut cloud = PointCloud2Msg::default(); + cloud.point_step = fields.iter().fold(0, |acc, field| { - let field_type = convert_msg_code_to_type(field.datatype).unwrap(); - let field_size = datatype_size(&field_type); + 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 }); @@ -156,15 +272,28 @@ where T: FromBytes, C: PointConvertible, { - /// Create a new Writer struct from an iterator. + /// 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 = 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 + 'static) -> Self { Self { coordinates: Box::new(coordinates.into_iter()), - xyz_field_type: FieldDatatype::F32, phantom_t: std::marker::PhantomData, } } diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index dfd9293..dcd7239 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -1,4 +1,3 @@ -use fallible_iterator::FallibleIterator; use ros_pointcloud2::pcl_utils::*; use ros_pointcloud2::reader::*; use ros_pointcloud2::ros_types::PointCloud2Msg; @@ -8,14 +7,27 @@ use std::fmt::Debug; macro_rules! convert_from_into { ($reader:ty, $writer:ty, $point:ty, $cloud:expr) => { - let copy = $cloud.clone(); - let msg: Result = <$writer>::from($cloud).try_into(); + convert_from_into_in_out_cloud!( + $reader, + $writer, + $point, + $cloud.clone(), + $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 = <$writer>::from($in_cloud).try_into(); assert!(msg.is_ok()); let to_p_type = <$reader>::try_from(msg.unwrap()); assert!(to_p_type.is_ok()); let to_p_type = to_p_type.unwrap(); - let back_to_type = to_p_type.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + let back_to_type = to_p_type.collect::>(); + assert_eq!($out_cloud, back_to_type); }; } @@ -30,29 +42,35 @@ fn custom_xyz_f32() { y: f32, z: f32, } - type MyReader = Reader; - type MyWriter = Writer; - impl From for ([f32; DIM], [PointMeta; METADIM]) { - fn from(point: CustomPoint) -> Self { - ([point.x, point.y, point.z], []) + + type MyReader = ReaderF32; + type MyWriter = WriterF32; + + impl From> for CustomPoint { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + } } } - impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - }) + + impl Into> for CustomPoint { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [], + } } } + impl convert::MetaNames for CustomPoint { fn meta_names() -> [String; METADIM] { [] } } - impl PointConvertible for CustomPoint {} + impl PointConvertible() }, 3, 0> for CustomPoint {} convert_from_into!( MyReader, @@ -80,6 +98,8 @@ fn custom_xyz_f32() { #[test] fn custom_xyzi_f32() { + type Xyz = f32; + const XYZ_S: usize = std::mem::size_of::(); const DIM: usize = 3; const METADIM: usize = 1; #[derive(Debug, PartialEq, Clone)] @@ -89,32 +109,37 @@ fn custom_xyzi_f32() { z: f32, i: u8, } - type MyReader = reader::Reader; - impl From for ([f32; DIM], [PointMeta; METADIM]) { - fn from(point: CustomPoint) -> Self { - ([point.x, point.y, point.z], [PointMeta::new(point.i)]) + + impl From> for CustomPoint { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + i: point.meta[0].get(), + } } } - type MyWriter = writer::Writer; - - impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - i: data.1.first().unwrap().get().unwrap(), - }) + impl Into> for CustomPoint { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [self.i.into()], + } } } + impl convert::MetaNames for CustomPoint { fn meta_names() -> [String; METADIM] { [format!("intensity")] } } - impl PointConvertible for CustomPoint {} + + type MyReader = reader::Reader; + type MyWriter = writer::Writer; + + impl PointConvertible for CustomPoint {} let cloud = vec![ CustomPoint { @@ -159,33 +184,29 @@ fn custom_rgba_f32() { b: u8, a: u8, } - type MyReader = reader::Reader; - type MyWriter = writer::Writer; - impl From for ([f32; DIM], [PointMeta; METADIM]) { - fn from(point: CustomPoint) -> Self { - ( - [point.x, point.y, point.z], - [ - PointMeta::new(point.r), - PointMeta::new(point.g), - PointMeta::new(point.b), - PointMeta::new(point.a), - ], - ) + type MyReader = reader::Reader() }, DIM, METADIM, CustomPoint>; + type MyWriter = writer::Writer() }, DIM, METADIM, CustomPoint>; + + impl From> for CustomPoint { + fn from(point: Point) -> 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 TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - r: data.1[0].get()?, - g: data.1[1].get()?, - b: data.1[2].get()?, - a: data.1[3].get()?, - }) + + impl Into> for CustomPoint { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [self.r.into(), self.g.into(), self.b.into(), self.a.into()], + } } } impl convert::MetaNames for CustomPoint { @@ -193,7 +214,7 @@ fn custom_rgba_f32() { ["r", "g", "b", "a"].map(|s| s.to_string()) } } - impl PointConvertible for CustomPoint {} + impl PointConvertible() }, DIM, METADIM> for CustomPoint {} let cloud = vec![ CustomPoint { x: 0.0, @@ -614,3 +635,162 @@ 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, + }, + ]; + + 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, + }, + ]; + + convert_from_into_in_out_cloud!( + ReaderXYZ, + WriterXYZI, + PointXYZI, + write_cloud, + PointXYZI, + read_cloud, + PointXYZ + ); +} + +#[test] +fn write_less_than_available() { + const DIM: usize = 3; + const METADIM: usize = 0; + + #[derive(Debug, PartialEq, Clone)] + struct CustomPoint { + x: f32, + y: f32, + z: f32, + dummy: f32, + } + + type MyReader = ReaderF32; + type MyWriter = WriterF32; + + impl From> for CustomPoint { + fn from(point: Point) -> Self { + Self { + x: point.coords[0], + y: point.coords[1], + z: point.coords[2], + dummy: 0.0, + } + } + } + + impl Into> for CustomPoint { + fn into(self) -> Point { + Point { + coords: [self.x, self.y, self.z], + meta: [], + } + } + } + + impl convert::MetaNames for CustomPoint { + fn meta_names() -> [String; METADIM] { + [] + } + } + impl PointConvertible() }, 3, 0> for CustomPoint {} + + let write_cloud = vec![ + CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + dummy: -10.0, + }, + CustomPoint { + x: 4.0, + y: 5.0, + z: 6.0, + dummy: -10.0, + }, + CustomPoint { + x: 7.0, + y: 8.0, + z: 9.0, + dummy: -10.0, + }, + ]; + + let read_cloud = vec![ + CustomPoint { + x: 1.0, + y: 2.0, + z: 3.0, + dummy: 0.0, + }, + CustomPoint { + x: 4.0, + y: 5.0, + z: 6.0, + dummy: 0.0, + }, + CustomPoint { + x: 7.0, + y: 8.0, + z: 9.0, + dummy: 0.0, + }, + ]; + + convert_from_into_in_out_cloud!( + MyReader, + MyWriter, + CustomPoint, + write_cloud, + CustomPoint, + read_cloud, + CustomPoint + ); +} diff --git a/tests/r2r_msg_test.rs b/tests/r2r_msg_test.rs index a020f7d..43e1413 100644 --- a/tests/r2r_msg_test.rs +++ b/tests/r2r_msg_test.rs @@ -1,11 +1,11 @@ #[cfg(feature = "r2r_msg")] #[test] fn convertxyz_r2r_msg() { + use ros_pointcloud2::{ + pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, + }; + use r2r::sensor_msgs::msg::PointCloud2; - use ros_pointcloud2::fallible_iterator::FallibleIterator; - use ros_pointcloud2::pcl_utils::PointXYZ; - use ros_pointcloud2::ros_types::PointCloud2Msg; - use ros_pointcloud2::ConvertXYZ; let cloud = vec![ PointXYZ { @@ -25,10 +25,10 @@ fn convertxyz_r2r_msg() { }, ]; let copy = cloud.clone(); - let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); + let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap(); let r2r_msg_cloud: PointCloud2 = internal_cloud.into(); let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into(); - let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap(); + let to_convert: ConvertXYZ = ReaderXYZ::try_from(convert_back_internal).unwrap(); let back_to_type = to_convert.map(|point| Ok(point)).collect::>(); assert_eq!(copy, back_to_type.unwrap()); } diff --git a/tests/rosrust_msg_test.rs b/tests/rosrust_msg_test.rs index d519947..1530ad9 100644 --- a/tests/rosrust_msg_test.rs +++ b/tests/rosrust_msg_test.rs @@ -1,10 +1,9 @@ #[cfg(feature = "rosrust_msg")] #[test] fn convertxyz_rosrust_msg() { - use ros_pointcloud2::fallible_iterator::FallibleIterator; - use ros_pointcloud2::pcl_utils::PointXYZ; - use ros_pointcloud2::ros_types::PointCloud2Msg; - use ros_pointcloud2::ConvertXYZ; + use ros_pointcloud2::{ + pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, + }; let cloud = vec![ PointXYZ { @@ -24,10 +23,10 @@ fn convertxyz_rosrust_msg() { }, ]; let copy = cloud.clone(); - let internal_cloud: PointCloud2Msg = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); + let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().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: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap(); + let to_convert: ConvertXYZ = ReaderXYZ::try_from(convert_back_internal).unwrap(); let back_to_type = to_convert.map(|point| Ok(point)).collect::>(); assert_eq!(copy, back_to_type.unwrap()); } From 91b0ae7ecff5730340971fc308bddd1ca36c6ca8 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 29 Apr 2024 20:32:55 +0200 Subject: [PATCH 08/11] docs and clippy --- README.md | 185 +++++----------------------------------------- src/convert.rs | 62 +++++++++++----- src/lib.rs | 74 ++++++++++++++++++- src/pcl_utils.rs | 153 +++++++++++++++++++------------------- src/reader.rs | 25 +++---- src/ros_types.rs | 56 +++++++------- src/writer.rs | 45 ++++++----- tests/e2e_test.rs | 95 +++++++++++++++++++----- 8 files changed, 350 insertions(+), 345 deletions(-) diff --git a/README.md b/README.md index c6c78de..990c657 100644 --- a/README.md +++ b/README.md @@ -5,11 +5,13 @@

-Providing a easy to use, generics defined, point-wise iterator abstraction of the byte buffer of `PointCloud2` to minimize iterations in your processing pipeline. +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 and support ROS1 and ROS2, it uses its own type for the message `PointCloud2Msg`. -## Example +## Examples + +Using a `Vec<>`. ```rust use ros_pointcloud2::{ pcl_utils::PointXYZ, reader::ReaderXYZ, writer::WriterXYZ, PointCloud2Msg, @@ -18,10 +20,11 @@ use ros_pointcloud2::{ // Your points (here using the predefined type PointXYZ). let cloud_points = vec![ PointXYZ { - x: 1.3, - y: 1.6, - z: 5.7, + x: 91.486, + y: -4.1, + z: 42.0001, }, + PointXYZ { x: f32::MAX, y: f32::MIN, @@ -32,20 +35,11 @@ let cloud_points = vec![ // For equality test later let cloud_copy = cloud_points.clone(); -// Some changes to demonstrate lazy iterator usage -let changed_it = cloud_points.iter().map(|p| { - p.x = 0.5; -}); - -// Vector -> Writer -> Message -let internal_msg: PointCloud2Msg = WriterXYZ::from(cloud_points) - .try_into() // iterating points here O(n) - .unwrap(); - -// Anything that implements `IntoIterator` also works - like another iterator. -let internal_msg_changed: PointCloud2Msg = WriterXYZ::from(changed_it) - .try_into() // iterating points here O(n) - .unwrap(); +// Vector -> Writer -> Message. +// You can also just give the Vec or anything that implements `IntoIter`. +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(); @@ -58,12 +52,12 @@ let internal_msg_changed: PointCloud2Msg = WriterXYZ::from(changed_it) // 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 + .map(|point: PointXYZ| { + // Some logic here - point - }) - .collect::>(); + point + }) + .collect::>(); // iterating points here O(n) assert_eq!(new_cloud_points, cloud_copy); ``` @@ -102,156 +96,13 @@ Also, indicate the following dependencies to your linker inside the `package.xml builtin_interfaces ``` -### Others -To use `ros_pointcloud2` somewhere else, you can also implement the `Into` and `From` traits for `PointCloud2Msg`. - -Try to avoid cloning the `data: Vec` field. -```rust -use ros_pointcloud2::ros_types::PointCloud2Msg; - -struct YourROSPointCloud2 {} // Likely to be generated by your ROS crate. - -impl Into for PointCloud2Msg { - fn into(self) -> YourROSPointCloud2 { - todo!() - } -} - -impl From for PointCloud2Msg { - fn from(msg: YourROSPointCloud2) -> Self { - todo!() - } -} -``` - Please open an issue or PR if you want to see support for other crates. -## Features - -- Easy to integrate into your favorite ROS1 or ROS2 crate -- Custom point types -- Predefined types for common conversions (compared to PCL) - - PointXYZ - - PointXYZI - - PointXYZL - - PointXYZRGB - - PointXYZRGBA - - PointXYZRGBL - - PointXYZNormal - - PointXYZINormal - - PointXYZRGBNormal -- 2D and 3D - -## Custom Types - -You can freely convert to your own point types without reiterating the entire cloud. - -You just need to implement some traits. -```rust -use ros_pointcloud2::mem_macros::size_of; -use ros_pointcloud2::ros_types::PointCloud2Msg; -use ros_pointcloud2::{ConversionError, Convert, MetaNames, PointConvertible, PointMeta}; - -const DIM: usize = 3; -const METADIM: usize = 4; - -#[derive(Debug, PartialEq, Clone)] -struct CustomPoint { - x: f32, // DIM 1 - y: f32, // DIM 2 - z: f32, // DIM 3 - r: u8, // METADIM 1 - g: u8, // METADIM 2 - b: u8, // METADIM 3 - a: u8, // METADIM 4 -} - -// Converting your custom point to the crate's internal representation -impl From for ([f32; DIM], [PointMeta; METADIM]) { - fn from(point: CustomPoint) -> Self { - ( - [point.x, point.y, point.z], - [ - PointMeta::new(point.r), - PointMeta::new(point.g), - PointMeta::new(point.b), - PointMeta::new(point.a), - ], - ) - } -} - -// The mappings for index of meta idx to field names. Example: 0 -> "r", 1 -> "g", 2 -> "b", 3 -> "a" -impl MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { - ["r", "g", "b", "a"].map(|s| s.to_string()) - } -} - -// Converting crate's internal representation to your custom point -impl TryFrom<([f32; DIM], [PointMeta; METADIM])> for CustomPoint { - type Error = ConversionError; - fn try_from(data: ([f32; DIM], [PointMeta; METADIM])) -> Result { - Ok(Self { - x: data.0[0], - y: data.0[1], - z: data.0[2], - r: data.1[0].get()?, - g: data.1[1].get()?, - b: data.1[2].get()?, - a: data.1[3].get()?, - }) - } -} - -impl PointConvertible for CustomPoint {} - -type MyConverter = Convert; - -// Your custom cloud (Vector) -let custom_cloud = vec![ - CustomPoint { - x: f32::MAX, - y: f32::MIN, - z: f32::MAX, - r: u8::MAX, - g: u8::MAX, - b: u8::MAX, - a: u8::MAX, - }, -]; - -// Cloud -> ROS message -let custom_msg: PointCloud2Msg = MyConverter::try_from(custom_cloud) - .unwrap() - .try_into() - .unwrap(); - -// ROS message -> Cloud -let to_custom_type = MyConverter::try_from(custom_msg).unwrap(); -``` - ## Future Work - Benchmark vs PCL - Add more predefined types - Optional derive macros for custom point implementations -## Changelog 1.0.0 -- `Convert` is now `Reader` and `Writer`. -`Convert` had to translate both ways and keep a different state depending on the way it was created. This led to an edge case where the user could create a point cloud that always returns uninitialized points. While the reproduction was unlikely to occur in normal use cases, the possibility of it alone is unnecessary ambiguity. -The new `Reader` and `Writer` not only eliminates ambiguity but also makes the code much more readable and concise. -- Zero-cost iterator abstraction. `Reader` and `Writer` now either take or implement iterable traits. This means, you can read a message, implement a `filter` function and pass the resulting iterator directly to the `Writer`. The entire pipeline is then compiled to a typesafe message-to-message filter with a single iteration. -- Divide error types to message and human errors. -Corrupted messages with valid descriptions and correct byte buffer length can not be classified as corrupted at runtime, thus point conversions can be assumed to always work when passing these checks. There now are less error types and the point conversions within the iteration can omit the `try_*` prefix. Discrepancies within the point cloud message and the described point are checked at `Reader` and `Writer` creation instead. -Human errors are possible with wrong point types or internal crate bugs that could lead to out-of-bound panics. These cases are checked in debug mode via debug_assert now. In release builds, the crate fully leans into performance optimizations and assumes a correct type description via generics (mainly the first 2 parameters coord_type and sizeof(coord_type)). Previously, these errors resulted in an error at runtime but the only possible source for them is the crate user, so the decision was made to promote them to a panic like a compile error. TODO check coord_type and sizeof at compile time statically? -- More Documentation and more relevant examples for every function and type. The example for custom points is moved from the Readme into the documentation as well. -- Performance and efficiency. By leaning into iterators whenever possible, the amount of bound checks have been reduced. There are less iterations in every function and dynamic memory usage is reduced. -- Removed dependencies. By switching to normal iterators and intializing different start values, the crate only depends on the std now. - -API changes -- replace every use of `([T; DIM], [ros_pointcloud2::PointMeta; METADIM])` with `ros_pointcloud2::Point` -- - ## License [MIT](https://choosealicense.com/licenses/mit/) diff --git a/src/convert.rs b/src/convert.rs index 7a59c7f..8ada125 100644 --- a/src/convert.rs +++ b/src/convert.rs @@ -1,24 +1,21 @@ use crate::*; /// Datatypes from the [PointField message](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointField.html). -#[derive(Clone, Debug, PartialEq, Copy)] +#[derive(Default, Clone, Debug, PartialEq, Copy)] pub enum FieldDatatype { F32, F64, I32, U8, U16, + + #[default] U32, + I8, I16, } -impl Default for FieldDatatype { - fn default() -> Self { - FieldDatatype::F32 - } -} - impl FieldDatatype { pub fn size(&self) -> usize { match self { @@ -105,9 +102,9 @@ impl TryFrom for FieldDatatype { } } -impl Into for FieldDatatype { - fn into(self) -> u8 { - match self { +impl From for u8 { + fn from(val: FieldDatatype) -> Self { + match val { FieldDatatype::I8 => 1, FieldDatatype::U8 => 2, FieldDatatype::I16 => 3, @@ -140,7 +137,7 @@ pub(crate) fn check_coord( /// Matching field names from each meta data per point to the PointField name. /// Always make sure to use the same order as in your Into<> implementation to have a correct mapping. pub trait MetaNames { - fn meta_names() -> [String; METADIM]; + fn meta_names() -> [&'static str; METADIM]; } #[inline(always)] @@ -183,71 +180,92 @@ pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype { } impl FromBytes for i8 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0]]) } + + #[inline] fn from_le_bytes(bytes: &[u8]) -> Self { Self::from_le_bytes([bytes[0]]) } + #[inline] fn bytes(x: &i8) -> Vec { Vec::from(x.to_le_bytes()) } } impl FromBytes for i16 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0], bytes[1]]) } + + #[inline] fn from_le_bytes(bytes: &[u8]) -> Self { Self::from_le_bytes([bytes[0], bytes[1]]) } + #[inline] fn bytes(x: &i16) -> Vec { Vec::from(x.to_le_bytes()) } } impl FromBytes for u16 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0], bytes[1]]) } + + #[inline] fn from_le_bytes(bytes: &[u8]) -> Self { Self::from_le_bytes([bytes[0], bytes[1]]) } + #[inline] fn bytes(x: &u16) -> Vec { Vec::from(x.to_le_bytes()) } } impl FromBytes for u32 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) } + + #[inline] fn from_le_bytes(bytes: &[u8]) -> Self { Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) } + #[inline] fn bytes(x: &u32) -> Vec { Vec::from(x.to_le_bytes()) } } impl FromBytes for f32 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) } + + #[inline] fn from_le_bytes(bytes: &[u8]) -> Self { Self::from_le_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) } + #[inline] fn bytes(x: &f32) -> Vec { Vec::from(x.to_le_bytes()) } } impl FromBytes for i32 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0], bytes[1], bytes[2], bytes[3]]) } @@ -261,47 +279,52 @@ impl FromBytes for i32 { } impl FromBytes for f64 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([ bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], ]) } + + #[inline] fn from_le_bytes(bytes: &[u8]) -> Self { Self::from_le_bytes([ bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5], bytes[6], bytes[7], ]) } + #[inline] fn bytes(x: &f64) -> Vec { Vec::from(x.to_le_bytes()) } } impl FromBytes for u8 { + #[inline] fn from_be_bytes(bytes: &[u8]) -> Self { Self::from_be_bytes([bytes[0]]) } + + #[inline] fn from_le_bytes(bytes: &[u8]) -> Self { Self::from_le_bytes([bytes[0]]) } + #[inline] fn bytes(x: &u8) -> Vec { Vec::from(x.to_le_bytes()) } } -#[derive(Clone, Debug, PartialEq)] +#[derive(Default, Clone, Debug, PartialEq)] pub(crate) enum Endianness { Big, + + #[default] Little, } -impl Default for Endianness { - fn default() -> Self { - Endianness::Little - } -} - +#[inline(always)] pub(crate) fn load_loadable( start_idx: usize, data: &[u8], @@ -313,7 +336,8 @@ pub(crate) fn load_loadable( } } -/// Note: check if the data slice is long enough to load the bytes beforehand! +/// Note: check if the data slice is long enough to load the bytes beforehand! Uses unsafe indexing. +#[inline(always)] fn load_bytes(start_idx: usize, data: &[u8]) -> [u8; S] { let mut target = [u8::default(); S]; diff --git a/src/lib.rs b/src/lib.rs index 19e1a10..dd41293 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,72 @@ -//#![doc = include_str!("../README.md")] +//! A library to work with the PointCloud2 message type from ROS. +//! +//! ros_pointcloud2 mainly provides iterator-based `Reader` and `Writer` to interact +//! with the internal message type `PointCloud2Msg` without forcing an iteration. +//! +//! For ROS interoperability, the message type generated by the respective crate must be converted to +//! the internal message type `PointCloud2Msg` using the `From` trait, +//! which is mostly a ownership transfer 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_types` module for more information. +//! +//! Common point types like `PointXYZ` are provided in the `pcl_utils` module. You can implement any custom point type +//! that can be described by the specification of the `PointCloud2Msg` message type. +//! See the `Reader` or `Writer` struct documentation for an example. +//! +//! # Example +//! ``` +//! 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: 9.0006, +//! y: 42.0, +//! z: -6.2, +//! }, +//! +//! PointXYZ { +//! x: f32::MAX, +//! y: f32::MIN, +//! z: f32::MAX, +//! }, +//! ]; +//! +//! // For equality test later +//! let cloud_copy = cloud_points.clone(); +//! +//! // Vector -> Writer -> Message. +//! // Vector -> Writer -> Message. +//! // You can also just give the Vec or anything that implements `IntoIter`. +//! 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(); +//! +//! // 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::>(); // iterating points here O(n) +//! +//! assert_eq!(new_cloud_points, cloud_copy); +//! ``` + pub mod convert; pub mod pcl_utils; pub mod ros_types; @@ -100,6 +168,7 @@ impl PointMeta { /// ``` /// let meta = ros_pointcloud2::PointMeta::new(1.0); /// ``` + #[inline(always)] pub fn new(value: T) -> Self { let raw_bytes = T::bytes(&value); let mut bytes = [0; std::mem::size_of::()]; @@ -113,12 +182,13 @@ impl PointMeta { } } + #[inline(always)] fn from_buffer(data: &[u8], offset: usize, datatype: &FieldDatatype) -> Self { debug_assert!(data.len() >= offset + datatype.size()); let bytes = unsafe { data.get_unchecked(offset..offset + datatype.size()) }; let mut bytes_array = [0; std::mem::size_of::()]; // 8 bytes as f64 is the largest type - for (byte, save_byte) in bytes.into_iter().zip(bytes_array.iter_mut()) { + for (byte, save_byte) in bytes.iter().zip(bytes_array.iter_mut()) { *save_byte = *byte; } diff --git a/src/pcl_utils.rs b/src/pcl_utils.rs index a8fa2a0..2439e9d 100644 --- a/src/pcl_utils.rs +++ b/src/pcl_utils.rs @@ -36,17 +36,17 @@ impl From> for PointXYZ { } } -impl Into> for PointXYZ { - fn into(self) -> Point { +impl From for Point { + fn from(point: PointXYZ) -> Self { Point { - coords: [self.x, self.y, self.z], - meta: Default::default(), + coords: [point.x, point.y, point.z], + meta: [], } } } impl MetaNames<0> for PointXYZ { - fn meta_names() -> [String; 0] { + fn meta_names() -> [&'static str; 0] { [] } } @@ -63,6 +63,15 @@ pub struct PointXYZI { pub intensity: f32, } +impl From for Point { + fn from(point: PointXYZI) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [point.intensity.into()], + } + } +} + impl From> for PointXYZI { fn from(point: Point) -> Self { Self { @@ -74,18 +83,9 @@ impl From> for PointXYZI { } } -impl Into> for PointXYZI { - fn into(self) -> Point { - Point { - coords: [self.x, self.y, self.z], - meta: [self.intensity.into()], - } - } -} - impl MetaNames<1> for PointXYZI { - fn meta_names() -> [String; 1] { - ["intensity"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 1] { + ["intensity"] } } @@ -118,19 +118,18 @@ impl From> for PointXYZRGB { } } -impl Into> for PointXYZRGB { - fn into(self) -> Point { - let rgb = pack_rgb(self.r, self.g, self.b); +impl From for Point { + fn from(point: PointXYZRGB) -> Self { Point { - coords: [self.x, self.y, self.z], - meta: [rgb.into()], + 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() -> [String; 1] { - ["rgb"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 1] { + ["rgb"] } } @@ -166,19 +165,19 @@ impl From> for PointXYZRGBA { } } -impl Into> for PointXYZRGBA { - fn into(self) -> Point { - let rgb = pack_rgb(self.r, self.g, self.b); +impl From for Point { + fn from(point: PointXYZRGBA) -> Self { + let rgb = pack_rgb(point.r, point.g, point.b); Point { - coords: [self.x, self.y, self.z], - meta: [rgb.into(), self.a.into()], + coords: [point.x, point.y, point.z], + meta: [rgb.into(), point.a.into()], } } } impl MetaNames<2> for PointXYZRGBA { - fn meta_names() -> [String; 2] { - ["rgb", "a"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 2] { + ["rgb", "a"] } } @@ -217,24 +216,24 @@ impl From> for PointXYZRGBNormal { } } -impl Into> for PointXYZRGBNormal { - fn into(self) -> Point { - let rgb = pack_rgb(self.r, self.g, self.b); +impl From for Point { + fn from(point: PointXYZRGBNormal) -> Self { + let rgb = pack_rgb(point.r, point.g, point.b); Point { - coords: [self.x, self.y, self.z], + coords: [point.x, point.y, point.z], meta: [ rgb.into(), - self.normal_x.into(), - self.normal_y.into(), - self.normal_z.into(), + point.normal_x.into(), + point.normal_y.into(), + point.normal_z.into(), ], } } } impl MetaNames<4> for PointXYZRGBNormal { - fn meta_names() -> [String; 4] { - ["rgb", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 4] { + ["rgb", "normal_x", "normal_y", "normal_z"] } } @@ -253,6 +252,20 @@ pub struct PointXYZINormal { pub normal_z: f32, } +impl From for Point { + fn from(point: PointXYZINormal) -> Self { + Point { + coords: [point.x, point.y, point.z], + meta: [ + point.intensity.into(), + point.normal_x.into(), + point.normal_y.into(), + point.normal_z.into(), + ], + } + } +} + impl From> for PointXYZINormal { fn from(point: Point) -> Self { Self { @@ -267,23 +280,9 @@ impl From> for PointXYZINormal { } } -impl Into> for PointXYZINormal { - fn into(self) -> Point { - Point { - coords: [self.x, self.y, self.z], - meta: [ - self.intensity.into(), - self.normal_x.into(), - self.normal_y.into(), - self.normal_z.into(), - ], - } - } -} - impl MetaNames<4> for PointXYZINormal { - fn meta_names() -> [String; 4] { - ["intensity", "normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 4] { + ["intensity", "normal_x", "normal_y", "normal_z"] } } @@ -310,18 +309,18 @@ impl From> for PointXYZL { } } -impl Into> for PointXYZL { - fn into(self) -> Point { +impl From for Point { + fn from(point: PointXYZL) -> Self { Point { - coords: [self.x, self.y, self.z], - meta: [self.label.into()], + coords: [point.x, point.y, point.z], + meta: [point.label.into()], } } } impl MetaNames<1> for PointXYZL { - fn meta_names() -> [String; 1] { - ["label".to_string()] + fn meta_names() -> [&'static str; 1] { + ["label"] } } @@ -356,19 +355,21 @@ impl From> for PointXYZRGBL { } } -impl Into> for PointXYZRGBL { - fn into(self) -> Point { - let rgb = pack_rgb(self.r, self.g, self.b); +impl From for Point { + fn from(point: PointXYZRGBL) -> Self { Point { - coords: [self.x, self.y, self.z], - meta: [rgb.into(), self.label.into()], + 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() -> [String; 2] { - ["rgb", "label"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 2] { + ["rgb", "label"] } } @@ -399,22 +400,22 @@ impl From> for PointXYZNormal { } } -impl Into> for PointXYZNormal { - fn into(self) -> Point { +impl From for Point { + fn from(point: PointXYZNormal) -> Self { Point { - coords: [self.x, self.y, self.z], + coords: [point.x, point.y, point.z], meta: [ - self.normal_x.into(), - self.normal_y.into(), - self.normal_z.into(), + point.normal_x.into(), + point.normal_y.into(), + point.normal_z.into(), ], } } } impl MetaNames<3> for PointXYZNormal { - fn meta_names() -> [String; 3] { - ["normal_x", "normal_y", "normal_z"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; 3] { + ["normal_x", "normal_y", "normal_z"] } } diff --git a/src/reader.rs b/src/reader.rs index c81860c..7aa5c93 100644 --- a/src/reader.rs +++ b/src/reader.rs @@ -1,5 +1,4 @@ use crate::*; -use convert::*; /// 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 = @@ -152,18 +151,20 @@ pub type ReaderXYZL = ReaderF32<3, 1, PointXYZL>; /// } /// } /// -///impl Into> for CustomPoint { -/// fn into(self) -> Point { +///impl From for Point { +/// fn from(point: CustomPoint) -> Self { /// Point { -/// coords: [self.x, self.y, self.z], -/// meta: [self.i.into()], +/// coords: [point.x, point.y, point.z], +/// meta: [ +/// point.i.into(), +/// ], /// } /// } ///} /// /// impl MetaNames for CustomPoint { -/// fn meta_names() -> [String; METADIM] { -/// [format!("intensity")] +/// fn meta_names() -> [&'static str; METADIM] { +/// ["intensity"] /// } /// } /// @@ -218,7 +219,7 @@ where return None; // iteration finished } - let mut xyz: [T; DIM] = [T::default(); DIM]; + let mut xyz = [T::default(); DIM]; xyz.iter_mut() .zip(self.offsets.iter()) .for_each(|(p_xyz, in_point_offset)| { @@ -235,7 +236,7 @@ where "Offset length mismatch" ); - let mut meta: [PointMeta; METADIM] = [PointMeta::default(); METADIM]; + let mut meta = [PointMeta::default(); METADIM]; meta.iter_mut() .zip(self.offsets.iter().skip(DIM)) .zip(self.meta.iter()) @@ -387,10 +388,8 @@ where 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); - } + } else if last_offset + xyz_field_type.size() > point_step_size { + return Err(ConversionError::DataLengthMismatch); } Ok(Self { diff --git a/src/ros_types.rs b/src/ros_types.rs index 9d729b6..8d6d9eb 100644 --- a/src/ros_types.rs +++ b/src/ros_types.rs @@ -70,19 +70,19 @@ impl From for PointCloud2Msg { } #[cfg(feature = "r2r_msg")] -impl Into for PointCloud2Msg { - fn into(self) -> r2r::sensor_msgs::msg::PointCloud2 { +impl From for r2r::sensor_msgs::msg::PointCloud2 { + fn from(msg: PointCloud2Msg) -> Self { r2r::sensor_msgs::msg::PointCloud2 { header: r2r::std_msgs::msg::Header { stamp: r2r::builtin_interfaces::msg::Time { - sec: self.header.stamp.sec as i32, - nanosec: self.header.stamp.nsec, + sec: msg.header.stamp.sec as i32, + nanosec: msg.header.stamp.nsec, }, - frame_id: self.header.frame_id, + frame_id: msg.header.frame_id, }, - height: self.height, - width: self.width, - fields: self + height: msg.height, + width: msg.width, + fields: msg .fields .into_iter() .map(|field| r2r::sensor_msgs::msg::PointField { @@ -92,11 +92,11 @@ impl Into for PointCloud2Msg { count: field.count, }) .collect(), - is_bigendian: self.is_bigendian, - point_step: self.point_step, - row_step: self.row_step, - data: self.data, - is_dense: self.is_dense, + is_bigendian: msg.is_bigendian, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + is_dense: msg.is_dense, } } } @@ -135,20 +135,20 @@ impl From for PointCloud2Msg { } #[cfg(feature = "rosrust_msg")] -impl Into for PointCloud2Msg { - fn into(self) -> rosrust_msg::sensor_msgs::PointCloud2 { +impl From for rosrust_msg::sensor_msgs::PointCloud2 { + fn from(msg: PointCloud2Msg) -> Self { rosrust_msg::sensor_msgs::PointCloud2 { header: rosrust_msg::std_msgs::Header { - seq: self.header.seq, - stamp: rosrust::Time { - sec: self.header.stamp.sec, - nsec: self.header.stamp.nsec, + seq: msg.header.seq, + stamp: rosrust_msg::Time { + sec: msg.header.stamp.sec, + nsec: msg.header.stamp.nsec, }, - frame_id: self.header.frame_id, + frame_id: msg.header.frame_id, }, - height: self.height, - width: self.width, - fields: self + height: msg.height, + width: msg.width, + fields: msg .fields .into_iter() .map(|field| rosrust_msg::sensor_msgs::PointField { @@ -158,11 +158,11 @@ impl Into for PointCloud2Msg { count: field.count, }) .collect(), - is_bigendian: self.is_bigendian, - point_step: self.point_step, - row_step: self.row_step, - data: self.data, - is_dense: self.is_dense, + is_bigendian: msg.is_bigendian, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + is_dense: msg.is_dense, } } } diff --git a/src/writer.rs b/src/writer.rs index c614d5f..fdbec99 100644 --- a/src/writer.rs +++ b/src/writer.rs @@ -132,18 +132,20 @@ pub type WriterXYZL = WriterF32<3, 1, PointXYZL>; /// } /// } /// -///impl Into> for CustomPoint { -/// fn into(self) -> Point { +///impl From for Point { +/// fn from(point: CustomPoint) -> Self { /// Point { -/// coords: [self.x, self.y, self.z], -/// meta: [self.i.into()], +/// coords: [point.x, point.y, point.z], +/// meta: [ +/// point.i.into(), +/// ], /// } /// } ///} /// /// impl MetaNames for CustomPoint { -/// fn meta_names() -> [String; METADIM] { -/// [format!("intensity")] +/// fn meta_names() -> [&'static str; METADIM] { +/// ["intensity"] /// } /// } /// impl PointConvertible for CustomPoint {} @@ -198,14 +200,14 @@ where if DIM > 1 { fields.push(PointFieldMsg { - name: "x".to_string(), + name: "x".into(), offset: 0, datatype: T::field_datatype().into(), count: 1, }); fields.push(PointFieldMsg { - name: "y".to_string(), + name: "y".into(), offset: SIZE as u32, datatype: T::field_datatype().into(), count: 1, @@ -214,41 +216,44 @@ where if DIM == 3 { fields.push(PointFieldMsg { - name: "z".to_string(), + 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 = 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()) { + 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, + 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::default(); - - cloud.point_step = fields.iter().fold(0, |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 - }); + 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)?; diff --git a/tests/e2e_test.rs b/tests/e2e_test.rs index dcd7239..35d4eee 100644 --- a/tests/e2e_test.rs +++ b/tests/e2e_test.rs @@ -56,17 +56,17 @@ fn custom_xyz_f32() { } } - impl Into> for CustomPoint { - fn into(self) -> Point { + impl From for Point { + fn from(point: CustomPoint) -> Self { Point { - coords: [self.x, self.y, self.z], + coords: [point.x, point.y, point.z], meta: [], } } } impl convert::MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { + fn meta_names() -> [&'static str; METADIM] { [] } } @@ -121,18 +121,18 @@ fn custom_xyzi_f32() { } } - impl Into> for CustomPoint { - fn into(self) -> Point { + impl From for Point { + fn from(point: CustomPoint) -> Self { Point { - coords: [self.x, self.y, self.z], - meta: [self.i.into()], + coords: [point.x, point.y, point.z], + meta: [point.i.into()], } } } impl convert::MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { - [format!("intensity")] + fn meta_names() -> [&'static str; METADIM] { + ["intensity"] } } @@ -201,17 +201,23 @@ fn custom_rgba_f32() { } } - impl Into> for CustomPoint { - fn into(self) -> Point { + impl From for Point { + fn from(point: CustomPoint) -> Self { Point { - coords: [self.x, self.y, self.z], - meta: [self.r.into(), self.g.into(), self.b.into(), self.a.into()], + coords: [point.x, point.y, point.z], + meta: [ + point.r.into(), + point.g.into(), + point.b.into(), + point.a.into(), + ], } } } + impl convert::MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { - ["r", "g", "b", "a"].map(|s| s.to_string()) + fn meta_names() -> [&'static str; METADIM] { + ["r", "g", "b", "a"] } } impl PointConvertible() }, DIM, METADIM> for CustomPoint {} @@ -726,17 +732,17 @@ fn write_less_than_available() { } } - impl Into> for CustomPoint { - fn into(self) -> Point { + impl From for Point { + fn from(point: CustomPoint) -> Self { Point { - coords: [self.x, self.y, self.z], + coords: [point.x, point.y, point.z], meta: [], } } } impl convert::MetaNames for CustomPoint { - fn meta_names() -> [String; METADIM] { + fn meta_names() -> [&'static str; METADIM] { [] } } @@ -794,3 +800,52 @@ fn write_less_than_available() { 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::>(); + + assert_eq!(new_cloud_points, cloud_copy); +} From 212f7b05cc2ea6c4bcd99150d516e6902c27efbe Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 29 Apr 2024 20:45:01 +0200 Subject: [PATCH 09/11] fix rosrust time --- src/convert.rs | 2 +- src/lib.rs | 14 +++++++------- src/ros_types.rs | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/convert.rs b/src/convert.rs index 8ada125..bb27969 100644 --- a/src/convert.rs +++ b/src/convert.rs @@ -125,7 +125,7 @@ pub(crate) fn check_coord( match coord { Some(y_idx) => { let field = &fields[y_idx]; - if field.datatype != (*xyz_field_type).into() { + if field.datatype != u8::from(*xyz_field_type) { return Err(ConversionError::InvalidFieldFormat); } Ok(field.clone()) diff --git a/src/lib.rs b/src/lib.rs index dd41293..f3967e0 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,19 +1,19 @@ //! A library to work with the PointCloud2 message type from ROS. //! -//! ros_pointcloud2 mainly provides iterator-based `Reader` and `Writer` to interact -//! with the internal message type `PointCloud2Msg` without forcing an iteration. +//! 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. //! //! For ROS interoperability, the message type generated by the respective crate must be converted to -//! the internal message type `PointCloud2Msg` using the `From` trait, +//! the [`ros_pointcloud2::PointCloud2Msg`] using the `From` trait, //! which is mostly a ownership transfer 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_types` module for more information. +//! See the [`ros_pointcloud2::ros_types`] module for more information. //! -//! Common point types like `PointXYZ` are provided in the `pcl_utils` module. You can implement any custom point type -//! that can be described by the specification of the `PointCloud2Msg` message type. -//! See the `Reader` or `Writer` struct documentation for an example. +//! Common point types like `PointXYZ` 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 //! ``` diff --git a/src/ros_types.rs b/src/ros_types.rs index 8d6d9eb..0e2bbfc 100644 --- a/src/ros_types.rs +++ b/src/ros_types.rs @@ -140,7 +140,7 @@ impl From for rosrust_msg::sensor_msgs::PointCloud2 { rosrust_msg::sensor_msgs::PointCloud2 { header: rosrust_msg::std_msgs::Header { seq: msg.header.seq, - stamp: rosrust_msg::Time { + stamp: TimeMsg { sec: msg.header.stamp.sec, nsec: msg.header.stamp.nsec, }, From 01cf6f2dedc432ecb8015caa8669c8e9b819edb4 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 29 Apr 2024 20:58:20 +0200 Subject: [PATCH 10/11] no convert --- tests/r2r_msg_test.rs | 2 +- tests/rosrust_msg_test.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/r2r_msg_test.rs b/tests/r2r_msg_test.rs index 43e1413..1f447ba 100644 --- a/tests/r2r_msg_test.rs +++ b/tests/r2r_msg_test.rs @@ -28,7 +28,7 @@ fn convertxyz_r2r_msg() { let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().unwrap(); let r2r_msg_cloud: PointCloud2 = internal_cloud.into(); let convert_back_internal: PointCloud2Msg = r2r_msg_cloud.into(); - let to_convert: ConvertXYZ = ReaderXYZ::try_from(convert_back_internal).unwrap(); + let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap(); let back_to_type = to_convert.map(|point| Ok(point)).collect::>(); assert_eq!(copy, back_to_type.unwrap()); } diff --git a/tests/rosrust_msg_test.rs b/tests/rosrust_msg_test.rs index 1530ad9..c4d23a7 100644 --- a/tests/rosrust_msg_test.rs +++ b/tests/rosrust_msg_test.rs @@ -26,7 +26,7 @@ fn convertxyz_rosrust_msg() { let internal_cloud: PointCloud2Msg = WriterXYZ::from(cloud).try_into().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: ConvertXYZ = ReaderXYZ::try_from(convert_back_internal).unwrap(); + let to_convert = ReaderXYZ::try_from(convert_back_internal).unwrap(); let back_to_type = to_convert.map(|point| Ok(point)).collect::>(); assert_eq!(copy, back_to_type.unwrap()); } From d0745c4390f38026b84e2f1f1fd1f2610338e041 Mon Sep 17 00:00:00 2001 From: stelzo Date: Mon, 29 Apr 2024 21:16:01 +0200 Subject: [PATCH 11/11] no optional point collect --- tests/r2r_msg_test.rs | 4 ++-- tests/rosrust_msg_test.rs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/r2r_msg_test.rs b/tests/r2r_msg_test.rs index 1f447ba..1c4540b 100644 --- a/tests/r2r_msg_test.rs +++ b/tests/r2r_msg_test.rs @@ -29,6 +29,6 @@ fn convertxyz_r2r_msg() { 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 back_to_type = to_convert.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + let back_to_type = to_convert.collect::>(); + assert_eq!(copy, back_to_type); } diff --git a/tests/rosrust_msg_test.rs b/tests/rosrust_msg_test.rs index c4d23a7..03d7fef 100644 --- a/tests/rosrust_msg_test.rs +++ b/tests/rosrust_msg_test.rs @@ -27,6 +27,6 @@ fn convertxyz_rosrust_msg() { 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 back_to_type = to_convert.map(|point| Ok(point)).collect::>(); - assert_eq!(copy, back_to_type.unwrap()); + let back_to_type = to_convert.collect::>(); + assert_eq!(copy, back_to_type); }