mirror
/
ros_pointcloud2
mirror of https://github.com/stelzo/ros_pointcloud2.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

merge no_std

This commit is contained in:
stelzo 2024-05-15 18:42:45 +02:00
parent b2e9209333 529c7e1a77
commit 756647e44f
17 changed files with 395 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/workflows/crates.yml vendored
View File

@ -19,4 +19,4 @@ jobs:
uses: actions-rs/cargo@v1 uses: actions-rs/cargo@v1
with: with:
command: publish command: publish
args: --token ${{ secrets.CRATES_IO_TOKEN }} args: --token ${{ secrets.CRATES_IO_TOKEN }}

6
.github/workflows/r2r_galactic.yml vendored
View File

@ -16,6 +16,6 @@ jobs:
tests_galactic: tests_galactic:
runs-on: ubuntu-latest runs-on: ubuntu-latest
steps: steps:
- uses: actions/checkout@v2 - uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_r2r_galactic --tag r2r_galactic - run: docker build . --file ./tests/Dockerfile_r2r_galactic --tag r2r_galactic
- run: docker run r2r_galactic cargo test --features r2r_msg,derive,nalgebra,rayon - run: docker run r2r_galactic cargo test --features r2r_msg,derive,nalgebra,rayon

6
.github/workflows/r2r_humble.yml vendored
View File

@ -16,6 +16,6 @@ jobs:
tests_humble: tests_humble:
runs-on: ubuntu-latest runs-on: ubuntu-latest
steps: steps:
- uses: actions/checkout@v2 - uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_r2r_humble --tag r2r_humble - run: docker build . --file ./tests/Dockerfile_r2r_humble --tag r2r_humble
- run: docker run r2r_humble cargo test --features r2r_msg,derive,nalgebra,rayon - run: docker run r2r_humble cargo test --features r2r_msg,derive,nalgebra,rayon

6
.github/workflows/r2r_iron.yml vendored
View File

@ -16,6 +16,6 @@ jobs:
tests_humble: tests_humble:
runs-on: ubuntu-latest runs-on: ubuntu-latest
steps: steps:
- uses: actions/checkout@v2 - uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_r2r_iron --tag r2r_iron - run: docker build . --file ./tests/Dockerfile_r2r_iron --tag r2r_iron
- run: docker run r2r_iron cargo test --features r2r_msg,derive,nalgebra,rayon - run: docker run r2r_iron cargo test --features r2r_msg,derive,nalgebra,rayon

6
.github/workflows/rclrs_humble.yml vendored
View File

@ -16,6 +16,6 @@ jobs:
tests_humble: tests_humble:
runs-on: ubuntu-latest runs-on: ubuntu-latest
steps: steps:
- uses: actions/checkout@v2 - uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble - run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble
- run: docker run rclrs_humble cargo test --features derive,nalgebra,rayon - run: docker run rclrs_humble cargo test --features derive,nalgebra,rayon

6
.github/workflows/rclrs_iron.yml vendored
View File

@ -16,6 +16,6 @@ jobs:
tests_iron: tests_iron:
runs-on: ubuntu-latest runs-on: ubuntu-latest
steps: steps:
- uses: actions/checkout@v2 - uses: actions/checkout@v2
- run: docker build . --file ./tests/Dockerfile_rclrs_iron --tag rclrs_iron - run: docker build . --file ./tests/Dockerfile_rclrs_iron --tag rclrs_iron
- run: docker run rclrs_iron cargo test --features derive,nalgebra,rayon - run: docker run rclrs_iron cargo test --features derive,nalgebra,rayon

128
.github/workflows/rosrust_noetic.yml vendored
View File

@ -13,11 +13,11 @@ jobs:
build: build:
runs-on: ubuntu-20.04 runs-on: ubuntu-20.04
env: env:
ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...]
# CI_SOURCE_PATH: $(pwd) # CI_SOURCE_PATH: $(pwd)
ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall
CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options
ROS_PARALLEL_JOBS: '-j8 -l6' ROS_PARALLEL_JOBS: "-j8 -l6"
# Set the python path manually to include /usr/-/python2.7/dist-packages # Set the python path manually to include /usr/-/python2.7/dist-packages
# as this is where apt-get installs python packages. # as this is where apt-get installs python packages.
PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages
@ -27,81 +27,81 @@ jobs:
uses: actions/checkout@v1 uses: actions/checkout@v1
- name: Install latest rust - name: Install latest rust
run: | run: |
curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
rustc --version rustc --version
cargo --version cargo --version
- name: Configure ROS for install - name: Configure ROS for install
run: | run: |
sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt-get update -qq sudo apt-get update -qq
sudo apt-get install dpkg sudo apt-get install dpkg
sudo apt-get install -y libyaml-cpp-dev sudo apt-get install -y libyaml-cpp-dev
- name: Install ROS basic packages - name: Install ROS basic packages
run: | run: |
sudo apt-get install -y python3-catkin-pkg sudo apt-get install -y python3-catkin-pkg
sudo apt-get install -y python3-catkin-tools sudo apt-get install -y python3-catkin-tools
sudo apt-get install -y python3-rosdep sudo apt-get install -y python3-rosdep
sudo apt-get install -y python3-wstool sudo apt-get install -y python3-wstool
sudo apt-get install -y python3-osrf-pycommon sudo apt-get install -y python3-osrf-pycommon
sudo apt-get install -y ros-cmake-modules sudo apt-get install -y ros-cmake-modules
sudo apt-get install -y ros-$ROS_DISTRO-ros-base sudo apt-get install -y ros-$ROS_DISTRO-ros-base
source /opt/ros/$ROS_DISTRO/setup.bash source /opt/ros/$ROS_DISTRO/setup.bash
sudo rosdep init sudo rosdep init
rosdep update # --include-eol-distros # Support EOL distros. rosdep update # --include-eol-distros # Support EOL distros.
- name: Install ROS additional packages (TODO maybe don't need these) - name: Install ROS additional packages
# Does installing these mean rosrust_msg builds more messages, which is a better # Does installing these mean rosrust_msg builds more messages, which is a better
# check? Or does it just slow down the action? # check? Or does it just slow down the action?
run: | run: |
sudo apt-get install -y ros-$ROS_DISTRO-actionlib sudo apt-get install -y ros-$ROS_DISTRO-actionlib
sudo apt-get install -y ros-$ROS_DISTRO-actionlib-msgs sudo apt-get install -y ros-$ROS_DISTRO-actionlib-msgs
sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager
sudo apt-get install -y ros-$ROS_DISTRO-compressed-image-transport sudo apt-get install -y ros-$ROS_DISTRO-compressed-image-transport
sudo apt-get install -y ros-$ROS_DISTRO-catkin sudo apt-get install -y ros-$ROS_DISTRO-catkin
sudo apt-get install -y ros-$ROS_DISTRO-class-loader sudo apt-get install -y ros-$ROS_DISTRO-class-loader
sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules
sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge
sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure
sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure
# Not in noetic yet # Not in noetic yet
# sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure-python # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure-python
sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions
sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs
sudo apt-get install -y ros-$ROS_DISTRO-genmsg sudo apt-get install -y ros-$ROS_DISTRO-genmsg
sudo apt-get install -y ros-$ROS_DISTRO-image-geometry sudo apt-get install -y ros-$ROS_DISTRO-image-geometry
sudo apt-get install -y ros-$ROS_DISTRO-image-proc sudo apt-get install -y ros-$ROS_DISTRO-image-proc
sudo apt-get install -y ros-$ROS_DISTRO-image-transport sudo apt-get install -y ros-$ROS_DISTRO-image-transport
sudo apt-get install -y ros-$ROS_DISTRO-message-generation sudo apt-get install -y ros-$ROS_DISTRO-message-generation
sudo apt-get install -y ros-$ROS_DISTRO-message-runtime sudo apt-get install -y ros-$ROS_DISTRO-message-runtime
# sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core
# sudo apt-get install -y ros-$ROS_DISTRO-nodelet-topic-tools # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-topic-tools
# sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions
# sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros
sudo apt-get install -y ros-$ROS_DISTRO-pluginlib sudo apt-get install -y ros-$ROS_DISTRO-pluginlib
sudo apt-get install -y ros-$ROS_DISTRO-roscpp sudo apt-get install -y ros-$ROS_DISTRO-roscpp
sudo apt-get install -y ros-$ROS_DISTRO-roslib sudo apt-get install -y ros-$ROS_DISTRO-roslib
sudo apt-get install -y ros-$ROS_DISTRO-roslint sudo apt-get install -y ros-$ROS_DISTRO-roslint
sudo apt-get install -y ros-$ROS_DISTRO-rospy sudo apt-get install -y ros-$ROS_DISTRO-rospy
sudo apt-get install -y ros-$ROS_DISTRO-rospy-message-converter sudo apt-get install -y ros-$ROS_DISTRO-rospy-message-converter
sudo apt-get install -y ros-$ROS_DISTRO-rostest sudo apt-get install -y ros-$ROS_DISTRO-rostest
sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs
sudo apt-get install -y ros-$ROS_DISTRO-std-msgs sudo apt-get install -y ros-$ROS_DISTRO-std-msgs
sudo apt-get install -y ros-$ROS_DISTRO-tf sudo apt-get install -y ros-$ROS_DISTRO-tf
sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions
sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs
sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs
sudo apt-get install -y ros-$ROS_DISTRO-tf2-py sudo apt-get install -y ros-$ROS_DISTRO-tf2-py
sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros
sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs
- name: build - name: build
run: | run: |
source /opt/ros/$ROS_DISTRO/setup.bash source /opt/ros/$ROS_DISTRO/setup.bash
cargo build # --verbose cargo build # --verbose
- name: Install ROS packages for tests - name: Install ROS packages for tests
run: | run: |
sudo apt-get install -y ros-$ROS_DISTRO-actionlib-tutorials sudo apt-get install -y ros-$ROS_DISTRO-actionlib-tutorials
sudo apt-get install -y ros-$ROS_DISTRO-roscpp-tutorials sudo apt-get install -y ros-$ROS_DISTRO-roscpp-tutorials
sudo apt-get install -y ros-$ROS_DISTRO-rospy-tutorials sudo apt-get install -y ros-$ROS_DISTRO-rospy-tutorials
- name: test - name: test
run: | run: |
source /opt/ros/$ROS_DISTRO/setup.bash source /opt/ros/$ROS_DISTRO/setup.bash

6
.github/workflows/tests.yml vendored
View File

@ -19,9 +19,9 @@ jobs:
- uses: actions/checkout@v2 - uses: actions/checkout@v2
- name: Install latest Rust - name: Install latest Rust
run: | run: |
curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
rustc --version rustc --version
cargo --version cargo --version
- name: Linting - name: Linting
run: cargo clippy --all-targets -- -D warnings run: cargo clippy --all-targets -- -D warnings
- name: Tests - name: Tests

8
Cargo.toml
View File

@ -26,6 +26,7 @@ exclude = [
"**/docs/**", "**/docs/**",
"**/doc/**", "**/doc/**",
] ]
rust-version = "1.63"
[dependencies] [dependencies]
rosrust_msg = { version = "0.1", optional = true } rosrust_msg = { version = "0.1", optional = true }
@ -34,7 +35,7 @@ r2r = { version = "0.8.4", optional = true }
rayon = { version = "1", optional = true } rayon = { version = "1", optional = true }
nalgebra = { version = "0", optional = true } nalgebra = { version = "0", optional = true }
rpcl2_derive = { path = "./rpcl2_derive", optional = true } rpcl2_derive = { path = "./rpcl2_derive", optional = true }
type-layout = { version = "0.2", optional = true } type-layout = { git = "https://github.com/stelzo/type-layout", branch = "syn2", optional = true }
sensor_msgs = { version = "*", optional = true } sensor_msgs = { version = "*", optional = true }
std_msgs = { version = "*", optional = true } std_msgs = { version = "*", optional = true }
@ -55,11 +56,10 @@ r2r_msg = ["dep:r2r"]
rayon = ["dep:rayon"] rayon = ["dep:rayon"]
derive = ["dep:rpcl2_derive", "dep:type-layout"] derive = ["dep:rpcl2_derive", "dep:type-layout"]
nalgebra = ["dep:nalgebra"] nalgebra = ["dep:nalgebra"]
std = []
default = ["rclrs_msg"] default = ["derive", "std", "rclrs_msg"]
[package.metadata.docs.rs] [package.metadata.docs.rs]
features = ["derive", "nalgebra", "rayon"] features = ["derive", "nalgebra", "rayon"]
default-target = "x86_64-unknown-linux-gnu" default-target = "x86_64-unknown-linux-gnu"
rclrs_msg = ["dep:sensor_msgs", "dep:std_msgs", "dep:builtin_interfaces"]

4
README.md
View File

@ -95,6 +95,10 @@ The results are measured on an Intel i7-14700 with benchmarks from [this reposit
For minimizing the conversion overhead in general, always use the functions that best fit your use case. For minimizing the conversion overhead in general, always use the functions that best fit your use case.
## `#[no_std]`
The `_iter` conversions are compatible with `#[no_std]` environments when an allocator is provided. This is due to the fact that names for point fields do not have a maximum length, and PointCloud2 data vectors can have arbitrary sizes. Use `default-features = false` to disable std for this crate.
## License ## License
[MIT](https://choosealicense.com/licenses/mit/) [MIT](https://choosealicense.com/licenses/mit/)

6
rpcl2_derive/Cargo.toml
View File

@ -7,6 +7,6 @@ edition = "2021"
proc-macro = true proc-macro = true
[dependencies] [dependencies]
syn = "1" syn = "2.0"
quote = "1" quote = "1.0"
proc-macro2 = "1" proc-macro2 = "1.0"

79
rpcl2_derive/src/lib.rs
View File

@ -4,7 +4,7 @@ use std::collections::HashMap;
use proc_macro::TokenStream; use proc_macro::TokenStream;
use quote::{quote, ToTokens}; use quote::{quote, ToTokens};
use syn::{parse_macro_input, DeriveInput}; use syn::{parenthesized, parse_macro_input, DeriveInput, LitStr};
fn get_allowed_types() -> HashMap<&'static str, usize> { fn get_allowed_types() -> HashMap<&'static str, usize> {
let mut allowed_datatypes = HashMap::<&'static str, usize>::new(); let mut allowed_datatypes = HashMap::<&'static str, usize>::new();
@ -19,28 +19,6 @@ fn get_allowed_types() -> HashMap<&'static str, usize> {
allowed_datatypes allowed_datatypes
} }
// Given a field, get the value of the `rpcl2` renaming attribute like
// #[rpcl2(name = "new_name")]
fn get_ros_fields_attribute(attrs: &[syn::Attribute]) -> Option<syn::Lit> {
for attr in attrs {
if attr.path.is_ident("rpcl2") {
let meta = attr.parse_meta().unwrap();
if let syn::Meta::List(meta_list) = meta {
for nested_meta in meta_list.nested {
if let syn::NestedMeta::Meta(meta) = nested_meta {
if let syn::Meta::NameValue(meta_name_value) = meta {
if meta_name_value.path.is_ident("name") {
return Some(meta_name_value.lit);
}
}
}
}
}
}
}
None
}
fn struct_field_rename_array(input: &DeriveInput) -> Vec<String> { fn struct_field_rename_array(input: &DeriveInput) -> Vec<String> {
let fields = match input.data { let fields = match input.data {
syn::Data::Struct(ref data) => match data.fields { syn::Data::Struct(ref data) => match data.fields {
@ -50,31 +28,39 @@ fn struct_field_rename_array(input: &DeriveInput) -> Vec<String> {
_ => panic!("StructNames can only be derived for structs"), _ => panic!("StructNames can only be derived for structs"),
}; };
fields let mut field_names = Vec::with_capacity(fields.len());
.iter() for f in fields.iter() {
.map(|field| { if f.attrs.len() == 0 {
let field_name = field.ident.as_ref().unwrap(); field_names.push(f.ident.as_ref().unwrap().to_token_stream().to_string());
let ros_fields_attr = get_ros_fields_attribute(&field.attrs); } else {
match ros_fields_attr { f.attrs.iter().for_each(|attr| {
Some(ros_fields) => match ros_fields { if attr.path().is_ident("rpcl2") {
syn::Lit::Str(lit_str) => { let res = attr.parse_nested_meta(|meta| {
let val = lit_str.value(); if meta.path.is_ident("rename") {
if val.is_empty() { let new_name;
panic!("Empty string literals are not allowed for the rpcl2 attribute"); parenthesized!(new_name in meta.input);
let lit: LitStr = new_name.parse()?;
field_names.push(lit.value());
Ok(())
} else {
panic!("expected `name` attribute");
} }
val });
if let Err(err) = res {
panic!("Error parsing attribute: {}", err);
} }
_ => { }
panic!("Only string literals are allowed for the rpcl2 attribute") });
} }
}, }
None => String::from(field_name.to_token_stream().to_string()),
} field_names
})
.collect()
} }
/// This macro will implement the `Fields` trait for your struct so you can use your point for the PointCloud2 conversion. /// This macro implements the `Fields` trait which is a subset of the `PointConvertible` trait.
/// It is useful for points that convert the `From` trait themselves but want to use this macro for not repeating the field names.
///
/// You can rename the fields with the `rename` attribute.
/// ///
/// Use the rename attribute if your struct field name should be different to the ROS field name. /// Use the rename attribute if your struct field name should be different to the ROS field name.
#[proc_macro_derive(Fields, attributes(rpcl2))] #[proc_macro_derive(Fields, attributes(rpcl2))]
@ -104,11 +90,14 @@ pub fn ros_point_fields_derive(input: TokenStream) -> TokenStream {
expanded.into() expanded.into()
} }
/// This macro will fully implement the `PointConvertible` trait for your struct so you can use your point for the PointCloud2 conversion. /// This macro implements the `PointConvertible` trait for your struct so you can use your point for the PointCloud2 conversion.
///
/// The struct field names are used in the message if you do not use the `rename` attribute for a custom name.
/// ///
/// Note that the repr(C) attribute is required for the struct to work efficiently with C++ PCL. /// Note that the repr(C) attribute is required for the struct to work efficiently with C++ PCL.
/// With Rust layout optimizations, the struct might not work with the PCL library but the message still conforms to the description of PointCloud2. /// With Rust layout optimizations, the struct might not work with the PCL library but the message still conforms to the description of PointCloud2.
/// Furthermore, Rust layout can lead to smaller messages to be send over the network. /// Furthermore, Rust layout can lead to smaller messages to be send over the network.
///
#[proc_macro_derive(PointConvertible, attributes(rpcl2))] #[proc_macro_derive(PointConvertible, attributes(rpcl2))]
pub fn ros_point_derive(input: TokenStream) -> TokenStream { pub fn ros_point_derive(input: TokenStream) -> TokenStream {
let input = parse_macro_input!(input as DeriveInput); let input = parse_macro_input!(input as DeriveInput);

95
src/iterator.rs
View File

@ -1,10 +1,19 @@
//! Iterator implementations for PointCloud2Msg including a parallel iterator for rayon. //! Iterator implementations for [`PointCloud2Msg`] including a parallel iterator for rayon.
use crate::{ use crate::{
Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData, Endian, FieldDatatype, Fields, MsgConversionError, PointCloud2Msg, PointConvertible, PointData,
RPCL2Point, RPCL2Point,
}; };
/// The PointCloudIterator provides a an iterator abstraction of the PointCloud2Msg. #[cfg(not(feature = "std"))]
use alloc::string::String;
#[cfg(not(feature = "std"))]
use alloc::vec::Vec;
#[cfg(not(feature = "std"))]
use alloc::borrow::ToOwned;
/// The PointCloudIterator provides a an iterator abstraction of the [`PointCloud2Msg`].
/// ///
/// The iterator is defined at compile time, so the point has to be described via template arguments. /// The iterator is defined at compile time, so the point has to be described via template arguments.
/// ///
@ -19,7 +28,7 @@ use crate::{
/// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into(); /// let msg: r2r::sensor_msgs::msg::PointCloud2 = internal_msg.into();
/// let converted: ros_pointcloud2::PointCloud2Msg = msg.into(); /// let converted: ros_pointcloud2::PointCloud2Msg = msg.into();
/// ///
/// ros_pointcloud2 supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags. /// `ros_pointcloud2` supports r2r, rclrs and rosrust as conversion targets out of the box via feature flags.
/// ///
pub struct PointCloudIterator<const N: usize, C> pub struct PointCloudIterator<const N: usize, C>
where where
@ -28,7 +37,7 @@ where
iteration: usize, iteration: usize,
iteration_back: usize, iteration_back: usize,
data: ByteBufferView<N>, data: ByteBufferView<N>,
phantom_c: std::marker::PhantomData<C>, // internally used for meta names array phantom_c: core::marker::PhantomData<C>, // internally used for pdata names array
} }
#[cfg(feature = "rayon")] #[cfg(feature = "rayon")]
@ -133,7 +142,6 @@ where
} }
} }
/// Implementation of the iterator trait.
impl<const N: usize, C> Iterator for PointCloudIterator<N, C> impl<const N: usize, C> Iterator for PointCloudIterator<N, C>
where where
C: PointConvertible<N>, C: PointConvertible<N>,
@ -157,12 +165,15 @@ where
} }
struct ByteBufferView<const N: usize> { struct ByteBufferView<const N: usize> {
#[cfg(feature = "rayon")]
data: std::sync::Arc<[u8]>, data: std::sync::Arc<[u8]>,
#[cfg(not(feature = "rayon"))]
data: Vec<u8>,
start_point_idx: usize, start_point_idx: usize,
end_point_idx: usize, end_point_idx: usize,
point_step_size: usize, point_step_size: usize,
offsets: [usize; N], offsets: [usize; N],
meta: Vec<(String, FieldDatatype)>, pdata: Vec<(String, FieldDatatype)>,
endian: Endian, endian: Endian,
} }
@ -173,16 +184,19 @@ impl<const N: usize> ByteBufferView<N> {
start_point_idx: usize, start_point_idx: usize,
end_point_idx: usize, end_point_idx: usize,
offsets: [usize; N], offsets: [usize; N],
meta: Vec<(String, FieldDatatype)>, pdata: Vec<(String, FieldDatatype)>,
endian: Endian, endian: Endian,
) -> Self { ) -> Self {
Self { Self {
#[cfg(feature = "rayon")]
data: std::sync::Arc::<[u8]>::from(data), data: std::sync::Arc::<[u8]>::from(data),
#[cfg(not(feature = "rayon"))]
data,
start_point_idx, start_point_idx,
end_point_idx, end_point_idx,
point_step_size, point_step_size,
offsets, offsets,
meta, pdata,
endian, endian,
} }
} }
@ -192,25 +206,24 @@ impl<const N: usize> ByteBufferView<N> {
self.end_point_idx - self.start_point_idx + 1 self.end_point_idx - self.start_point_idx + 1
} }
#[inline(always)] #[inline]
fn point_at(&self, idx: usize) -> RPCL2Point<N> { fn point_at(&self, idx: usize) -> RPCL2Point<N> {
let offset = (self.start_point_idx + idx) * self.point_step_size; let offset = (self.start_point_idx + idx) * self.point_step_size;
let mut pdata = [PointData::default(); N];
// TODO memcpy entire point at once, then extract fields? pdata
let mut meta = [PointData::default(); N]; .iter_mut()
meta.iter_mut()
.zip(self.offsets.iter()) .zip(self.offsets.iter())
.zip(self.meta.iter()) .zip(self.pdata.iter())
.for_each(|((p_meta, in_point_offset), (_, meta_type))| { .for_each(|((pdata_entry, in_point_offset), (_, pdata_type))| {
*p_meta = PointData::from_buffer( *pdata_entry = PointData::from_buffer(
&self.data, &self.data,
offset + in_point_offset, offset + in_point_offset,
*meta_type, *pdata_type,
self.endian, self.endian,
); );
}); });
RPCL2Point { fields: meta } pdata.into()
} }
#[inline] #[inline]
@ -221,7 +234,7 @@ impl<const N: usize> ByteBufferView<N> {
end_point_idx: start + size - 1, end_point_idx: start + size - 1,
point_step_size: self.point_step_size, point_step_size: self.point_step_size,
offsets: self.offsets, offsets: self.offsets,
meta: self.meta.clone(), pdata: self.pdata.clone(),
endian: self.endian, endian: self.endian,
} }
} }
@ -231,9 +244,8 @@ impl<const N: usize> ByteBufferView<N> {
let left_start = self.start_point_idx; let left_start = self.start_point_idx;
let left_size = point_index; let left_size = point_index;
let right_start = point_index; let right_start = self.start_point_idx + point_index;
let right_size = self.len() - point_index; let right_size = self.len() - point_index;
( (
self.clone_with_bounds(left_start, left_size), self.clone_with_bounds(left_start, left_size),
self.clone_with_bounds(right_start, right_size), self.clone_with_bounds(right_start, right_size),
@ -247,13 +259,13 @@ where
{ {
type Error = MsgConversionError; type Error = MsgConversionError;
/// Convert a PointCloud2Msg into an iterator. /// Convert a [`PointCloud2Msg`] into an iterator.
/// Converting a PointCloud2Msg into an iterator is a fallible operation since the message can contain only a subset of the required fields. /// The conversion to an iterator is a fallible operation since the message could contain 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. /// The theoretical time complexity is O(n) where n is the number of fields defined in the message for a single point which is typically small.
/// It therefore has a constant time complexity O(1) for practical purposes. /// It therefore has a constant time complexity O(1) for practical purposes.
fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> { fn try_from(cloud: PointCloud2Msg) -> Result<Self, Self::Error> {
let mut meta_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N]; let mut pdata_with_offsets = vec![(String::default(), FieldDatatype::default(), 0); N];
let not_found_fieldnames = C::field_names_ordered() let not_found_fieldnames = C::field_names_ordered()
.into_iter() .into_iter()
@ -273,7 +285,7 @@ where
} }
let ordered_fieldnames = C::field_names_ordered(); let ordered_fieldnames = C::field_names_ordered();
for (field, with_offset) in cloud.fields.iter().zip(meta_with_offsets.iter_mut()) { for (field, with_offset) in cloud.fields.iter().zip(pdata_with_offsets.iter_mut()) {
if ordered_fieldnames.contains(&field.name.as_str()) { if ordered_fieldnames.contains(&field.name.as_str()) {
*with_offset = ( *with_offset = (
field.name.clone(), field.name.clone(),
@ -283,25 +295,26 @@ where
} }
} }
meta_with_offsets.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2)); pdata_with_offsets
.sort_unstable_by(|(_, _, offset1), (_, _, offset2)| offset1.cmp(offset2));
debug_assert!( debug_assert!(
meta_with_offsets.len() == N, pdata_with_offsets.len() == N,
"Not all fields were found in the message. Expected {} but found {}.", "Not all fields were found in the message. Expected {} but found {}.",
N, N,
meta_with_offsets.len() pdata_with_offsets.len()
); );
let mut offsets = [usize::default(); N]; let mut offsets = [usize::default(); N];
let mut meta = vec![(String::default(), FieldDatatype::default()); N]; let mut pdata = vec![(String::default(), FieldDatatype::default()); N];
meta_with_offsets pdata_with_offsets
.into_iter() .into_iter()
.zip(meta.iter_mut()) .zip(pdata.iter_mut())
.zip(offsets.iter_mut()) .zip(offsets.iter_mut())
.for_each(|(((name, datatype, offset), meta), meta_offset)| { .for_each(|(((name, datatype, offset), pdata), pdata_offset)| {
*meta = (name, datatype); *pdata = (name, datatype);
*meta_offset = offset; *pdata_offset = offset;
}); });
let point_step_size = cloud.point_step as usize; let point_step_size = cloud.point_step as usize;
@ -312,9 +325,9 @@ where
let last_offset = offsets.last().expect("Dimensionality is 0."); let last_offset = offsets.last().expect("Dimensionality is 0.");
let last_meta = meta.last().expect("Dimensionality is 0."); let last_pdata = pdata.last().expect("Dimensionality is 0.");
let size_with_last_meta = last_offset + last_meta.1.size(); let size_with_last_pdata = last_offset + last_pdata.1.size();
if size_with_last_meta > point_step_size { if size_with_last_pdata > point_step_size {
return Err(MsgConversionError::DataLengthMismatch); return Err(MsgConversionError::DataLengthMismatch);
} }
@ -326,7 +339,7 @@ where
0, 0,
cloud_length - 1, cloud_length - 1,
offsets, offsets,
meta, pdata,
cloud.endian, cloud.endian,
); );
@ -334,7 +347,7 @@ where
iteration: 0, iteration: 0,
iteration_back: cloud_length - 1, iteration_back: cloud_length - 1,
data, data,
phantom_c: std::marker::PhantomData, phantom_c: core::marker::PhantomData,
}) })
} }
} }
@ -344,16 +357,18 @@ where
C: Fields<N>, C: Fields<N>,
{ {
#[inline] #[inline]
#[must_use]
fn from_byte_buffer_view(data: ByteBufferView<N>) -> Self { fn from_byte_buffer_view(data: ByteBufferView<N>) -> Self {
Self { Self {
iteration: 0, iteration: 0,
iteration_back: data.len() - 1, iteration_back: data.len() - 1,
data, data,
phantom_c: std::marker::PhantomData, phantom_c: core::marker::PhantomData,
} }
} }
#[inline] #[inline]
#[must_use]
pub fn split_at(self, point_index: usize) -> (Self, Self) { pub fn split_at(self, point_index: usize) -> (Self, Self) {
let (left_data, right_data) = self.data.split_at(point_index); let (left_data, right_data) = self.data.split_at(point_index);
( (

210
src/lib.rs
View File

@ -72,7 +72,7 @@
//! pub x: f32, //! pub x: f32,
//! pub y: f32, //! pub y: f32,
//! pub z: f32, //! pub z: f32,
//! #[cfg_attr(feature = "derive", rpcl2(name = "i"))] //! #[cfg_attr(feature = "derive", rpcl2(rename("i")))]
//! pub intensity: f32, //! pub intensity: f32,
//! } //! }
//! //!
@ -121,14 +121,21 @@
//! pub x: f32, //! pub x: f32,
//! pub y: f32, //! pub y: f32,
//! pub z: f32, //! pub z: f32,
//! #[rpcl2(name = "i")] //! #[rpcl2(rename("i"))]
//! pub intensity: f32, //! pub intensity: f32,
//! } //! }
//! ``` //! ```
#![crate_type = "lib"] #![crate_type = "lib"]
#![warn(clippy::print_stderr)] #![warn(clippy::print_stderr)]
#![warn(clippy::print_stdout)] #![warn(clippy::print_stdout)]
#![warn(clippy::unwrap_used)]
#![warn(clippy::cargo)]
#![warn(clippy::std_instead_of_core)]
#![warn(clippy::alloc_instead_of_core)]
#![warn(clippy::std_instead_of_alloc)]
#![cfg_attr(not(feature = "std"), no_std)]
// Setup an allocator with #[global_allocator]
// see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html
pub mod points; pub mod points;
pub mod prelude; pub mod prelude;
@ -136,46 +143,65 @@ pub mod ros;
pub mod iterator; pub mod iterator;
use std::num::TryFromIntError;
use std::str::FromStr;
use crate::ros::{HeaderMsg, PointFieldMsg}; use crate::ros::{HeaderMsg, PointFieldMsg};
use core::str::FromStr;
#[cfg(not(feature = "std"))]
#[macro_use]
extern crate alloc;
#[cfg(not(feature = "std"))]
use alloc::string::String;
#[cfg(not(feature = "std"))]
use alloc::vec::Vec;
/// All errors that can occur while converting to or from the message type. /// All errors that can occur while converting to or from the message type.
#[derive(Debug)] #[derive(Debug)]
pub enum MsgConversionError { pub enum MsgConversionError {
InvalidFieldFormat, InvalidFieldFormat,
#[cfg(feature = "std")]
UnsupportedFieldType(String), UnsupportedFieldType(String),
#[cfg(not(feature = "std"))]
UnsupportedFieldType,
DataLengthMismatch, DataLengthMismatch,
FieldsNotFound(Vec<String>), FieldsNotFound(Vec<String>),
UnsupportedFieldCount, UnsupportedFieldCount,
NumberConversion, NumberConversion,
} }
impl From<TryFromIntError> for MsgConversionError { impl From<core::num::TryFromIntError> for MsgConversionError {
fn from(_: TryFromIntError) -> Self { fn from(_: core::num::TryFromIntError) -> Self {
MsgConversionError::NumberConversion MsgConversionError::NumberConversion
} }
} }
impl std::fmt::Display for MsgConversionError { impl core::fmt::Display for MsgConversionError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self { match self {
MsgConversionError::InvalidFieldFormat => { MsgConversionError::InvalidFieldFormat => {
write!(f, "The field does not match the expected datatype.") write!(f, "The field does not match the expected datatype.")
} }
#[cfg(feature = "std")]
MsgConversionError::UnsupportedFieldType(datatype) => { MsgConversionError::UnsupportedFieldType(datatype) => {
write!( write!(
f, f,
"The field datatype is not supported by the ROS message description: {}", "The field datatype is not supported by the ROS message description: {datatype}"
datatype )
}
#[cfg(not(feature = "std"))]
MsgConversionError::UnsupportedFieldType => {
write!(
f,
"There is an unsupported field type in the ROS message description."
) )
} }
MsgConversionError::DataLengthMismatch => { MsgConversionError::DataLengthMismatch => {
write!(f, "The length of the byte buffer in the message does not match the expected length computed from the fields, indicating a corrupted or malformed message.") write!(f, "The length of the byte buffer in the message does not match the expected length computed from the fields, indicating a corrupted or malformed message.")
} }
MsgConversionError::FieldsNotFound(fields) => { MsgConversionError::FieldsNotFound(fields) => {
write!(f, "Some fields are not found in the message: {:?}", fields) write!(f, "Some fields are not found in the message: {fields:?}")
} }
MsgConversionError::UnsupportedFieldCount => { MsgConversionError::UnsupportedFieldCount => {
write!( write!(
@ -190,6 +216,7 @@ impl std::fmt::Display for MsgConversionError {
} }
} }
#[cfg(feature = "std")]
impl std::error::Error for MsgConversionError { impl std::error::Error for MsgConversionError {
fn source(&self) -> Option<&(dyn std::error::Error + 'static)> { fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
None None
@ -246,11 +273,12 @@ enum ByteSimilarity {
Different, Different,
} }
/// Creating a CloudDimensions type with the builder pattern to avoid invalid states when using 1-row point clouds. /// Creating a [`CloudDimensions`] type with the builder pattern to avoid invalid states when using 1-row point clouds.
#[derive(Clone, Debug)] #[derive(Clone, Debug)]
pub struct CloudDimensionsBuilder(usize); pub struct CloudDimensionsBuilder(usize);
impl CloudDimensionsBuilder { impl CloudDimensionsBuilder {
#[must_use]
pub fn new_with_width(width: usize) -> Self { pub fn new_with_width(width: usize) -> Self {
Self(width) Self(width)
} }
@ -263,12 +291,12 @@ impl CloudDimensionsBuilder {
Ok(CloudDimensions { Ok(CloudDimensions {
width, width,
height: if self.0 > 0 { 1 } else { 0 }, height: u32::from(self.0 > 0),
}) })
} }
} }
/// Creating a PointCloud2Msg with the builder pattern to avoid invalid states. /// Creating a [`PointCloud2Msg`] with the builder pattern to avoid invalid states.
#[derive(Clone, Debug, Default)] #[derive(Clone, Debug, Default)]
pub struct PointCloud2MsgBuilder { pub struct PointCloud2MsgBuilder {
header: HeaderMsg, header: HeaderMsg,
@ -282,50 +310,63 @@ pub struct PointCloud2MsgBuilder {
} }
impl PointCloud2MsgBuilder { impl PointCloud2MsgBuilder {
#[must_use]
pub fn new() -> Self { pub fn new() -> Self {
Self::default() Self::default()
} }
#[must_use]
pub fn header(mut self, header: HeaderMsg) -> Self { pub fn header(mut self, header: HeaderMsg) -> Self {
self.header = header; self.header = header;
self self
} }
#[must_use]
pub fn width(mut self, width: u32) -> Self { pub fn width(mut self, width: u32) -> Self {
self.width = width; self.width = width;
self self
} }
#[must_use]
pub fn fields(mut self, fields: Vec<PointFieldMsg>) -> Self { pub fn fields(mut self, fields: Vec<PointFieldMsg>) -> Self {
self.fields = fields; self.fields = fields;
self self
} }
#[must_use]
pub fn endian(mut self, is_big_endian: bool) -> Self { pub fn endian(mut self, is_big_endian: bool) -> Self {
self.is_big_endian = is_big_endian; self.is_big_endian = is_big_endian;
self self
} }
#[must_use]
pub fn point_step(mut self, point_step: u32) -> Self { pub fn point_step(mut self, point_step: u32) -> Self {
self.point_step = point_step; self.point_step = point_step;
self self
} }
#[must_use]
pub fn row_step(mut self, row_step: u32) -> Self { pub fn row_step(mut self, row_step: u32) -> Self {
self.row_step = row_step; self.row_step = row_step;
self self
} }
#[must_use]
pub fn data(mut self, data: Vec<u8>) -> Self { pub fn data(mut self, data: Vec<u8>) -> Self {
self.data = data; self.data = data;
self self
} }
#[must_use]
pub fn dense(mut self, is_dense: bool) -> Self { pub fn dense(mut self, is_dense: bool) -> Self {
self.is_dense = is_dense; self.is_dense = is_dense;
self self
} }
/// Build the [`PointCloud2Msg`] from the builder.
///
/// # Errors
/// Returns an error if the fields are empty, the field count is not 1, the field format is invalid, the data length does not match the point step, or the field size is too large.
pub fn build(self) -> Result<PointCloud2Msg, MsgConversionError> { pub fn build(self) -> Result<PointCloud2Msg, MsgConversionError> {
if self.fields.is_empty() { if self.fields.is_empty() {
return Err(MsgConversionError::FieldsNotFound(vec![])); return Err(MsgConversionError::FieldsNotFound(vec![]));
@ -382,7 +423,7 @@ pub struct CloudDimensions {
impl PointCloud2Msg { impl PointCloud2Msg {
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
#[inline(always)] #[inline]
fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError> fn byte_similarity<const N: usize, C>(&self) -> Result<ByteSimilarity, MsgConversionError>
where where
C: PointConvertible<N>, C: PointConvertible<N>,
@ -405,7 +446,8 @@ impl PointCloud2Msg {
size, size,
count, count,
} => { } => {
let f_translated = field_names[field_counter].to_string(); let f_translated = String::from_str(field_names[field_counter])
.expect("Field name is not a valid string.");
field_counter += 1; field_counter += 1;
if msg_f.name != f_translated if msg_f.name != f_translated
@ -431,7 +473,7 @@ impl PointCloud2Msg {
}) })
} }
/// Create a PointCloud2Msg from any iterable type. /// Create a [`PointCloud2Msg`] from any iterable type.
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -457,33 +499,33 @@ impl PointCloud2Msg {
let field_names = C::field_names_ordered(); let field_names = C::field_names_ordered();
debug_assert!(field_names.len() == N); debug_assert!(field_names.len() == N);
let mut meta_offsets_acc: u32 = 0; let mut pdata_offsets_acc: u32 = 0;
let mut fields = vec![PointFieldMsg::default(); N]; let mut fields = vec![PointFieldMsg::default(); N];
let field_count: u32 = 1; let field_count: u32 = 1;
for ((meta_value, field_name), field_val) in point for ((pdata_entry, field_name), field_val) in point
.fields .fields
.into_iter() .into_iter()
.zip(field_names.into_iter()) .zip(field_names.into_iter())
.zip(fields.iter_mut()) .zip(fields.iter_mut())
{ {
let datatype_code = meta_value.datatype.into(); let datatype_code = pdata_entry.datatype.into();
let _ = FieldDatatype::try_from(datatype_code)?; let _ = FieldDatatype::try_from(datatype_code)?;
*field_val = PointFieldMsg { *field_val = PointFieldMsg {
name: field_name.into(), name: field_name.into(),
offset: meta_offsets_acc, offset: pdata_offsets_acc,
datatype: datatype_code, datatype: datatype_code,
count: 1, count: 1,
}; };
meta_offsets_acc += field_count * meta_value.datatype.size() as u32; pdata_offsets_acc += field_count * pdata_entry.datatype.size() as u32;
} }
( (
PointCloud2MsgBuilder::new() PointCloud2MsgBuilder::new()
.fields(fields) .fields(fields)
.point_step(meta_offsets_acc), .point_step(pdata_offsets_acc),
meta_offsets_acc, pdata_offsets_acc,
) )
}; };
let mut cloud_width = 0; let mut cloud_width = 0;
@ -491,9 +533,9 @@ impl PointCloud2Msg {
iterable.into_iter().for_each(|pointdata| { iterable.into_iter().for_each(|pointdata| {
let point: RPCL2Point<N> = pointdata.into(); let point: RPCL2Point<N> = pointdata.into();
point.fields.iter().for_each(|meta| { point.fields.iter().for_each(|pdata| {
let truncated_bytes = unsafe { let truncated_bytes = unsafe {
std::slice::from_raw_parts(meta.bytes.as_ptr(), meta.datatype.size()) core::slice::from_raw_parts(pdata.bytes.as_ptr(), pdata.datatype.size())
}; };
cloud.data.extend_from_slice(truncated_bytes); cloud.data.extend_from_slice(truncated_bytes);
}); });
@ -518,7 +560,7 @@ impl PointCloud2Msg {
Self::try_from_vec(iterable.collect::<Vec<_>>()) Self::try_from_vec(iterable.collect::<Vec<_>>())
} }
/// Create a PointCloud2Msg from a Vec of points. /// Create a [`PointCloud2Msg`] from a Vec of points.
/// Since the point type is known at compile time, the conversion is done by direct copy. /// Since the point type is known at compile time, the conversion is done by direct copy.
/// ///
/// # Example /// # Example
@ -532,6 +574,9 @@ impl PointCloud2Msg {
/// ///
/// let msg_out = PointCloud2Msg::try_from_vec(cloud_points).unwrap(); /// let msg_out = PointCloud2Msg::try_from_vec(cloud_points).unwrap();
/// ``` /// ```
///
/// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError> pub fn try_from_vec<const N: usize, C>(vec: Vec<C>) -> Result<Self, MsgConversionError>
where where
@ -552,7 +597,8 @@ impl PointCloud2Msg {
let mut offset = 0; let mut offset = 0;
let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(layout.fields.len()); let mut fields: Vec<PointFieldMsg> = Vec::with_capacity(layout.fields.len());
for f in layout.fields.into_iter() { for f in layout.fields.into_iter() {
let f_translated = field_names[fields.len()].to_string(); let f_translated = String::from_str(field_names[fields.len()])
.expect("Field name is not a valid string.");
match f { match f {
PointField::Field { PointField::Field {
datatype, datatype,
@ -585,9 +631,9 @@ impl PointCloud2Msg {
cloud.data.resize(bytes_total, u8::default()); cloud.data.resize(bytes_total, u8::default());
let raw_data: *mut C = cloud.data.as_ptr() as *mut C; let raw_data: *mut C = cloud.data.as_ptr() as *mut C;
unsafe { unsafe {
std::ptr::copy_nonoverlapping( core::ptr::copy_nonoverlapping(
vec.as_ptr() as *const u8, vec.as_ptr().cast::<u8>(),
raw_data as *mut u8, raw_data.cast::<u8>(),
bytes_total, bytes_total,
); );
} }
@ -601,7 +647,7 @@ impl PointCloud2Msg {
} }
} }
/// Convert the PointCloud2Msg to a Vec of points. /// Convert the [`PointCloud2Msg`] to a Vec of points.
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -616,6 +662,9 @@ impl PointCloud2Msg {
/// let cloud_points_out: Vec<PointXYZ> = msg_out.try_into_vec().unwrap(); /// let cloud_points_out: Vec<PointXYZ> = msg_out.try_into_vec().unwrap();
/// assert_eq!(1.0, cloud_points_out.get(0).unwrap().x); /// assert_eq!(1.0, cloud_points_out.get(0).unwrap().x);
/// ``` /// ```
///
/// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError> pub fn try_into_vec<const N: usize, C>(self) -> Result<Vec<C>, MsgConversionError>
where where
@ -631,12 +680,12 @@ impl PointCloud2Msg {
let cloud_width = self.dimensions.width as usize; let cloud_width = self.dimensions.width as usize;
let point_step = self.point_step as usize; let point_step = self.point_step as usize;
let mut vec = Vec::with_capacity(cloud_width); let mut vec: Vec<C> = Vec::with_capacity(cloud_width);
if bytematch { if bytematch {
unsafe { unsafe {
std::ptr::copy_nonoverlapping( core::ptr::copy_nonoverlapping(
self.data.as_ptr(), self.data.as_ptr(),
vec.as_mut_ptr() as *mut u8, vec.as_mut_ptr().cast::<u8>(),
self.data.len(), self.data.len(),
); );
vec.set_len(cloud_width); vec.set_len(cloud_width);
@ -644,7 +693,7 @@ impl PointCloud2Msg {
} else { } else {
unsafe { unsafe {
for i in 0..cloud_width { for i in 0..cloud_width {
let point_ptr = self.data.as_ptr().add(i * point_step) as *const C; let point_ptr = self.data.as_ptr().add(i * point_step).cast::<C>();
let point = point_ptr.read(); let point = point_ptr.read();
vec.push(point); vec.push(point);
} }
@ -657,7 +706,7 @@ impl PointCloud2Msg {
} }
} }
/// Convert the PointCloud2Msg to an iterator. /// Convert the [`PointCloud2Msg`] to an iterator.
/// ///
/// # Example /// # Example
/// ``` /// ```
@ -671,6 +720,8 @@ impl PointCloud2Msg {
/// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap(); /// let msg_out = PointCloud2Msg::try_from_iter(cloud_points).unwrap();
/// let cloud_points_out = msg_out.try_into_iter().unwrap().collect::<Vec<PointXYZ>>(); /// let cloud_points_out = msg_out.try_into_iter().unwrap().collect::<Vec<PointXYZ>>();
/// ``` /// ```
/// # Errors
/// Returns an error if the byte buffer does not match the expected layout or the message contains other discrepancies.
pub fn try_into_iter<const N: usize, C>( pub fn try_into_iter<const N: usize, C>(
self, self,
) -> Result<impl Iterator<Item = C>, MsgConversionError> ) -> Result<impl Iterator<Item = C>, MsgConversionError>
@ -706,7 +757,7 @@ impl PointCloud2Msg {
} }
} }
/// Internal point representation. It is used to store the coordinates and meta data of a point. /// Internal point representation. It is used to store the point data entries.
/// ///
/// In each iteration, an internal point representation is converted to the desired point type. /// In each iteration, an internal point representation is converted to the desired point type.
/// Implement the `From` traits for your point type to use the conversion. /// Implement the `From` traits for your point type to use the conversion.
@ -724,7 +775,7 @@ impl<const N: usize> Default for RPCL2Point<N> {
} }
} }
impl<const N: usize> std::ops::Index<usize> for RPCL2Point<N> { impl<const N: usize> core::ops::Index<usize> for RPCL2Point<N> {
type Output = PointData; type Output = PointData;
fn index(&self, index: usize) -> &Self::Output { fn index(&self, index: usize) -> &Self::Output {
@ -779,7 +830,7 @@ impl<const N: usize> From<[PointData; N]> for RPCL2Point<N> {
/// ``` /// ```
#[cfg(not(feature = "derive"))] #[cfg(not(feature = "derive"))]
pub trait PointConvertible<const N: usize>: pub trait PointConvertible<const N: usize>:
From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Clone + 'static + Default From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Clone + Default
{ {
} }
@ -840,7 +891,7 @@ pub trait PointConvertible<const N: usize>:
/// ``` /// ```
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
pub trait PointConvertible<const N: usize>: pub trait PointConvertible<const N: usize>:
type_layout::TypeLayout + From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + 'static + Default type_layout::TypeLayout + From<RPCL2Point<N>> + Into<RPCL2Point<N>> + Fields<N> + Default
{ {
} }
@ -926,12 +977,12 @@ impl TryFrom<type_layout::TypeLayoutInfo> for TypeLayoutInfo {
/// use ros_pointcloud2::PointData; /// use ros_pointcloud2::PointData;
/// ///
/// let original_data: f64 = 1.0; /// let original_data: f64 = 1.0;
/// let meta = PointData::new(original_data); /// let pdata = PointData::new(original_data);
/// let my_data: f64 = meta.get(); /// let my_data: f64 = pdata.get();
/// ``` /// ```
#[derive(Debug, Clone, Copy)] #[derive(Debug, Clone, Copy)]
pub struct PointData { pub struct PointData {
bytes: [u8; std::mem::size_of::<f64>()], bytes: [u8; core::mem::size_of::<f64>()],
endian: Endian, endian: Endian,
datatype: FieldDatatype, datatype: FieldDatatype,
} }
@ -939,7 +990,7 @@ pub struct PointData {
impl Default for PointData { impl Default for PointData {
fn default() -> Self { fn default() -> Self {
Self { Self {
bytes: [u8::default(); std::mem::size_of::<f64>()], bytes: [u8::default(); core::mem::size_of::<f64>()],
datatype: FieldDatatype::F32, datatype: FieldDatatype::F32,
endian: Endian::default(), endian: Endian::default(),
} }
@ -947,13 +998,13 @@ impl Default for PointData {
} }
impl PointData { impl PointData {
/// Create a new PointData from a value. /// Create a new [`PointData`] from a value.
/// ///
/// # Example /// # Example
/// ``` /// ```
/// let meta = ros_pointcloud2::PointData::new(1.0); /// let pdata = ros_pointcloud2::PointData::new(1.0);
/// ``` /// ```
#[inline(always)] #[inline]
pub fn new<T: FromBytes>(value: T) -> Self { pub fn new<T: FromBytes>(value: T) -> Self {
Self { Self {
bytes: value.into().raw(), bytes: value.into().raw(),
@ -962,31 +1013,32 @@ impl PointData {
} }
} }
#[inline(always)] #[inline]
fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self { fn from_buffer(data: &[u8], offset: usize, datatype: FieldDatatype, endian: Endian) -> Self {
debug_assert!(data.len() >= offset + datatype.size()); debug_assert!(data.len() >= offset + datatype.size());
let bytes = [u8::default(); std::mem::size_of::<f64>()]; let bytes = [u8::default(); core::mem::size_of::<f64>()];
unsafe { unsafe {
let data_ptr = data.as_ptr().add(offset); let data_ptr = data.as_ptr().add(offset);
let bytes_ptr = bytes.as_ptr() as *mut u8; let bytes_ptr = bytes.as_ptr() as *mut u8;
std::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size()); core::ptr::copy_nonoverlapping(data_ptr, bytes_ptr, datatype.size());
} }
Self { Self {
bytes, bytes,
datatype,
endian, endian,
datatype,
} }
} }
/// Get the numeric value from the PointData description. /// Get the numeric value from the [`PointData`] description.
/// ///
/// # Example /// # Example
/// ``` /// ```
/// let original_data: f64 = 1.0; /// let original_data: f64 = 1.0;
/// let meta = ros_pointcloud2::PointData::new(original_data); /// let pdata = ros_pointcloud2::PointData::new(original_data);
/// let my_data: f64 = meta.get(); /// let my_data: f64 = pdata.get();
/// ``` /// ```
#[must_use]
pub fn get<T: FromBytes>(&self) -> T { pub fn get<T: FromBytes>(&self) -> T {
match self.endian { match self.endian {
Endian::Big => T::from_be_bytes(PointDataBuffer::new(self.bytes)), Endian::Big => T::from_be_bytes(PointDataBuffer::new(self.bytes)),
@ -1062,22 +1114,22 @@ pub enum FieldDatatype {
} }
impl FieldDatatype { impl FieldDatatype {
#[must_use]
pub fn size(&self) -> usize { pub fn size(&self) -> usize {
match self { match self {
FieldDatatype::U8 => std::mem::size_of::<u8>(), FieldDatatype::U8 => core::mem::size_of::<u8>(),
FieldDatatype::U16 => std::mem::size_of::<u16>(), FieldDatatype::U16 => core::mem::size_of::<u16>(),
FieldDatatype::U32 => std::mem::size_of::<u32>(), FieldDatatype::U32 => core::mem::size_of::<u32>(),
FieldDatatype::I8 => std::mem::size_of::<i8>(), FieldDatatype::I8 => core::mem::size_of::<i8>(),
FieldDatatype::I16 => std::mem::size_of::<i16>(), FieldDatatype::I16 => core::mem::size_of::<i16>(),
FieldDatatype::I32 => std::mem::size_of::<i32>(), FieldDatatype::I32 => core::mem::size_of::<i32>(),
FieldDatatype::F32 => std::mem::size_of::<f32>(), FieldDatatype::F32 | FieldDatatype::RGB => core::mem::size_of::<f32>(), // packed in f32
FieldDatatype::F64 => std::mem::size_of::<f64>(), FieldDatatype::F64 => core::mem::size_of::<f64>(),
FieldDatatype::RGB => std::mem::size_of::<f32>(), // packed in f32
} }
} }
} }
impl FromStr for FieldDatatype { impl core::str::FromStr for FieldDatatype {
type Err = MsgConversionError; type Err = MsgConversionError;
fn from_str(s: &str) -> Result<Self, Self::Err> { fn from_str(s: &str) -> Result<Self, Self::Err> {
@ -1091,7 +1143,10 @@ impl FromStr for FieldDatatype {
"i8" => Ok(FieldDatatype::I8), "i8" => Ok(FieldDatatype::I8),
"i16" => Ok(FieldDatatype::I16), "i16" => Ok(FieldDatatype::I16),
"rgb" => Ok(FieldDatatype::RGB), "rgb" => Ok(FieldDatatype::RGB),
#[cfg(feature = "std")]
_ => Err(MsgConversionError::UnsupportedFieldType(s.into())), _ => Err(MsgConversionError::UnsupportedFieldType(s.into())),
#[cfg(not(feature = "std"))]
_ => Err(MsgConversionError::UnsupportedFieldType),
} }
} }
} }
@ -1169,7 +1224,10 @@ impl TryFrom<u8> for FieldDatatype {
6 => Ok(FieldDatatype::U32), 6 => Ok(FieldDatatype::U32),
7 => Ok(FieldDatatype::F32), 7 => Ok(FieldDatatype::F32),
8 => Ok(FieldDatatype::F64), 8 => Ok(FieldDatatype::F64),
#[cfg(feature = "std")]
_ => Err(MsgConversionError::UnsupportedFieldType(value.to_string())), _ => Err(MsgConversionError::UnsupportedFieldType(value.to_string())),
#[cfg(not(feature = "std"))]
_ => Err(MsgConversionError::UnsupportedFieldType),
} }
} }
} }
@ -1183,9 +1241,8 @@ impl From<FieldDatatype> for u8 {
FieldDatatype::U16 => 4, FieldDatatype::U16 => 4,
FieldDatatype::I32 => 5, FieldDatatype::I32 => 5,
FieldDatatype::U32 => 6, FieldDatatype::U32 => 6,
FieldDatatype::F32 => 7, FieldDatatype::F32 | FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer
FieldDatatype::F64 => 8, FieldDatatype::F64 => 8,
FieldDatatype::RGB => 7, // RGB is marked as f32 in the buffer
} }
} }
} }
@ -1200,10 +1257,10 @@ impl TryFrom<&ros::PointFieldMsg> for FieldDatatype {
/// Byte buffer alias for endian-aware point data reading and writing. /// Byte buffer alias for endian-aware point data reading and writing.
/// ///
/// It uses a fixed size buffer of 8 bytes since the largest supported datatype for PointFieldMsg is f64. /// It uses a fixed size buffer of 8 bytes since the largest supported datatype for [`ros::PointFieldMsg`] is f64.
pub struct PointDataBuffer([u8; 8]); pub struct PointDataBuffer([u8; 8]);
impl std::ops::Index<usize> for PointDataBuffer { impl core::ops::Index<usize> for PointDataBuffer {
type Output = u8; type Output = u8;
fn index(&self, index: usize) -> &Self::Output { fn index(&self, index: usize) -> &Self::Output {
@ -1212,18 +1269,22 @@ impl std::ops::Index<usize> for PointDataBuffer {
} }
impl PointDataBuffer { impl PointDataBuffer {
#[must_use]
pub fn new(data: [u8; 8]) -> Self { pub fn new(data: [u8; 8]) -> Self {
Self(data) Self(data)
} }
#[must_use]
pub fn as_slice(&self) -> &[u8] { pub fn as_slice(&self) -> &[u8] {
&self.0 &self.0
} }
#[must_use]
pub fn raw(self) -> [u8; 8] { pub fn raw(self) -> [u8; 8] {
self.0 self.0
} }
#[must_use]
pub fn from_slice(data: &[u8]) -> Self { pub fn from_slice(data: &[u8]) -> Self {
let mut buffer = [0; 8]; let mut buffer = [0; 8];
data.iter().enumerate().for_each(|(i, &v)| buffer[i] = v); data.iter().enumerate().for_each(|(i, &v)| buffer[i] = v);
@ -1298,7 +1359,7 @@ impl From<points::RGB> for PointDataBuffer {
} }
/// This trait is used to convert a byte slice to a primitive type. /// This trait is used to convert a byte slice to a primitive type.
/// All PointField types are supported. /// All [`PointField`] types are supported.
pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype + Into<PointDataBuffer> { pub trait FromBytes: Default + Sized + Copy + GetFieldDatatype + Into<PointDataBuffer> {
fn from_be_bytes(bytes: PointDataBuffer) -> Self; fn from_be_bytes(bytes: PointDataBuffer) -> Self;
fn from_le_bytes(bytes: PointDataBuffer) -> Self; fn from_le_bytes(bytes: PointDataBuffer) -> Self;
@ -1404,11 +1465,14 @@ mod tests {
use super::Fields; use super::Fields;
use rpcl2_derive::Fields; use rpcl2_derive::Fields;
#[cfg(not(feature = "std"))]
use alloc::string::String;
#[allow(dead_code)] #[allow(dead_code)]
#[derive(Fields)] #[derive(Fields)]
struct TestStruct { struct TestStruct {
field1: String, field1: String,
#[rpcl2(name = "renamed_field")] #[rpcl2(rename("renamed_field"))]
field2: i32, field2: i32,
field3: f64, field3: f64,
field4: bool, field4: bool,

33
src/points.rs
View File

@ -4,6 +4,9 @@ use crate::{Fields, PointConvertible, RPCL2Point};
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
use type_layout::TypeLayout; use type_layout::TypeLayout;
#[cfg(all(not(feature = "std"), feature = "derive"))]
use alloc::vec::Vec;
/// A packed RGB color encoding as used in ROS tools. /// A packed RGB color encoding as used in ROS tools.
#[derive(Clone, Copy)] #[derive(Clone, Copy)]
#[repr(C)] #[repr(C)]
@ -28,13 +31,13 @@ impl PartialEq for RGB {
} }
impl core::fmt::Display for RGB { impl core::fmt::Display for RGB {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
write!(f, "#{:02X}{:02X}{:02X}", self.r(), self.g(), self.b()) write!(f, "#{:02X}{:02X}{:02X}", self.r(), self.g(), self.b())
} }
} }
impl core::fmt::Debug for RGB { impl core::fmt::Debug for RGB {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.debug_struct("RGB") f.debug_struct("RGB")
.field("r", &self.r()) .field("r", &self.r())
.field("g", &self.g()) .field("g", &self.g())
@ -44,32 +47,39 @@ impl core::fmt::Debug for RGB {
} }
impl RGB { impl RGB {
#[must_use]
pub fn new(r: u8, g: u8, b: u8) -> Self { pub fn new(r: u8, g: u8, b: u8) -> Self {
Self { Self {
unpacked: [b, g, r, 0], unpacked: [b, g, r, 0],
} }
} }
#[must_use]
pub fn new_from_packed_f32(packed: f32) -> Self { pub fn new_from_packed_f32(packed: f32) -> Self {
Self { packed } Self { packed }
} }
#[must_use]
pub fn new_from_packed(packed: u32) -> Self { pub fn new_from_packed(packed: u32) -> Self {
Self::new_from_packed_f32(f32::from_bits(packed)) Self::new_from_packed_f32(f32::from_bits(packed))
} }
#[must_use]
pub fn raw(&self) -> f32 { pub fn raw(&self) -> f32 {
unsafe { self.packed } unsafe { self.packed }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
unsafe { self.unpacked[2] } unsafe { self.unpacked[2] }
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
unsafe { self.unpacked[1] } unsafe { self.unpacked[1] }
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
unsafe { self.unpacked[0] } unsafe { self.unpacked[0] }
} }
@ -129,6 +139,7 @@ impl From<PointXYZ> for nalgebra::Point3<f32> {
} }
impl PointXYZ { impl PointXYZ {
#[must_use]
pub fn new(x: f32, y: f32, z: f32) -> Self { pub fn new(x: f32, y: f32, z: f32) -> Self {
Self { x, y, z } Self { x, y, z }
} }
@ -289,19 +300,23 @@ pub struct PointXYZRGB {
} }
impl PointXYZRGB { impl PointXYZRGB {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8) -> Self { pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8) -> Self {
let rgb = RGB::new(r, g, b); let rgb = RGB::new(r, g, b);
Self { x, y, z, rgb } Self { x, y, z, rgb }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -361,19 +376,23 @@ pub struct PointXYZRGBA {
} }
impl PointXYZRGBA { impl PointXYZRGBA {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, a: u8) -> Self { pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, a: u8) -> Self {
let rgb = RGB::new(r, g, b); let rgb = RGB::new(r, g, b);
Self { x, y, z, rgb, a } Self { x, y, z, rgb, a }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -436,6 +455,7 @@ pub struct PointXYZRGBNormal {
} }
impl PointXYZRGBNormal { impl PointXYZRGBNormal {
#[must_use]
pub fn new( pub fn new(
x: f32, x: f32,
y: f32, y: f32,
@ -456,14 +476,17 @@ impl PointXYZRGBNormal {
} }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -530,6 +553,7 @@ pub struct PointXYZINormal {
} }
impl PointXYZINormal { impl PointXYZINormal {
#[must_use]
pub fn new( pub fn new(
x: f32, x: f32,
y: f32, y: f32,
@ -613,6 +637,7 @@ unsafe impl Send for PointXYZRGBL {}
unsafe impl Sync for PointXYZRGBL {} unsafe impl Sync for PointXYZRGBL {}
impl PointXYZRGBL { impl PointXYZRGBL {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, label: u32) -> Self { pub fn new(x: f32, y: f32, z: f32, r: u8, g: u8, b: u8, label: u32) -> Self {
let rgb = RGB::new(r, g, b); let rgb = RGB::new(r, g, b);
Self { Self {
@ -624,14 +649,17 @@ impl PointXYZRGBL {
} }
} }
#[must_use]
pub fn r(&self) -> u8 { pub fn r(&self) -> u8 {
self.rgb.r() self.rgb.r()
} }
#[must_use]
pub fn g(&self) -> u8 { pub fn g(&self) -> u8 {
self.rgb.g() self.rgb.g()
} }
#[must_use]
pub fn b(&self) -> u8 { pub fn b(&self) -> u8 {
self.rgb.b() self.rgb.b()
} }
@ -690,6 +718,7 @@ pub struct PointXYZNormal {
} }
impl PointXYZNormal { impl PointXYZNormal {
#[must_use]
pub fn new(x: f32, y: f32, z: f32, normal_x: f32, normal_y: f32, normal_z: f32) -> Self { pub fn new(x: f32, y: f32, z: f32, normal_x: f32, normal_y: f32, normal_z: f32) -> Self {
Self { Self {
x, x,

3
src/ros.rs
View File

@ -23,6 +23,9 @@
//! } //! }
//! ``` //! ```
#[cfg(not(feature = "std"))]
use alloc::string::String;
/// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages. /// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages.
#[cfg(not(any(feature = "rclrs_msg")))] #[cfg(not(any(feature = "rclrs_msg")))]
#[derive(Clone, Debug, Default)] #[derive(Clone, Debug, Default)]

44
tests/e2e_test.rs
View File

@ -70,9 +70,6 @@ fn write_cloud_from_vec() {
let msg = PointCloud2Msg::try_from_vec(cloud); let msg = PointCloud2Msg::try_from_vec(cloud);
assert!(msg.is_ok()); assert!(msg.is_ok());
let msg = msg.unwrap();
println!("{:?}", msg);
} }
#[test] #[test]
@ -92,6 +89,47 @@ fn write_empty_cloud_iter() {
assert!(msg.unwrap().data.is_empty()); assert!(msg.unwrap().data.is_empty());
} }
#[test]
#[cfg(all(feature = "derive", feature = "rayon"))]
fn conv_cloud_par_iter() {
let cloud = vec![
PointXYZ::new(0.0, 1.0, 5.0),
PointXYZ::new(1.0, 1.5, 5.0),
PointXYZ::new(1.3, 1.6, 5.7),
];
let copy = cloud.clone();
let msg: Result<PointCloud2Msg, MsgConversionError> = PointCloud2Msg::try_from_vec(cloud);
assert!(msg.is_ok());
let msg = msg.unwrap();
let to_p_type = msg.try_into_par_iter();
assert!(to_p_type.is_ok());
let to_p_type = to_p_type.unwrap();
let back_to_type = to_p_type.collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type);
}
#[test]
#[cfg(all(feature = "derive", feature = "rayon"))]
fn conv_cloud_par_par_iter() {
let cloud = vec![
PointXYZ::new(0.0, 1.0, 5.0),
PointXYZ::new(1.0, 1.5, 5.0),
PointXYZ::new(1.3, 1.6, 5.7),
PointXYZ::new(f32::MAX, f32::MIN, f32::MAX),
];
let copy = cloud.clone();
let msg = PointCloud2Msg::try_from_par_iter(cloud.into_par_iter());
assert!(msg.is_ok());
let msg = msg.unwrap();
let to_p_type = msg.try_into_par_iter();
assert!(to_p_type.is_ok());
let to_p_type = to_p_type.unwrap();
let back_to_type = to_p_type.collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type);
}
#[test] #[test]
#[cfg(feature = "derive")] #[cfg(feature = "derive")]
fn custom_xyz_f32() { fn custom_xyz_f32() {