Compare commits

...

15 Commits
v0.5.1 ... main

Author SHA1 Message Date
stelzo 9d46ff4373
fix lint 2025-04-26 12:24:54 +02:00
stelzo 88ed94b282
fix lint 2025-04-26 12:19:18 +02:00
stelzo 0e3b34b668 outdated 2025-02-07 10:24:26 +01:00
stelzo 922d3e226d ros2 client jazzy 2025-02-07 10:14:54 +01:00
stelzo c16334736e
bump version 2025-02-04 11:50:17 +01:00
stelzo 17e5bec524 fix time 2025-02-04 11:37:40 +01:00
stelzo c7134732d2 remove hint 2025-02-04 10:53:23 +01:00
stelzo 42dad82157 rosrust fix 2025-02-04 10:44:24 +01:00
stelzo 506ff826a3 fix build and tests 2025-02-01 21:38:16 +01:00
stelzo 54c5e8f6bb add serde 2025-02-01 20:15:19 +01:00
stelzo 83e9d1019e
note for deprecation 2024-11-01 17:42:44 +01:00
stelzo 46771bfdd0
use core offset, xyz casting 2024-11-01 17:36:33 +01:00
Christopher Sieh 15a1db79d6
source jazzy 2024-10-18 10:31:51 +02:00
Christopher Sieh 4c1e60210e
source jazzy 2024-10-18 10:30:56 +02:00
stelzo dbe88e97d0 ignore test scripts 2024-10-04 14:57:48 +02:00
27 changed files with 456 additions and 83 deletions

1
.gitattributes vendored Normal file
View File

@ -0,0 +1 @@
rpcl2/tests/*.bash linguist-vendored

View File

@ -18,17 +18,17 @@ jobs:
with:
components: clippy
- name: Linting
run: cargo clippy --all-targets --features derive,nalgebra,rayon -- -D warnings
run: cargo clippy --all-targets --features derive,nalgebra,rayon,serde,ros2-interfaces-jazzy -- -D warnings
- name: Tests
run: cargo test --features derive,nalgebra,rayon
run: cargo test --features derive,nalgebra,rayon,serde,ros2-interfaces-jazzy
outdated:
name: Outdated
runs-on: ubuntu-latest
if: github.event_name != 'pull_request'
timeout-minutes: 45
steps:
- uses: actions/checkout@v4
- uses: dtolnay/rust-toolchain@stable
- uses: dtolnay/install@cargo-outdated
- run: cargo outdated --exit-code 1
# outdated:
# name: Outdated
# runs-on: ubuntu-latest
# if: github.event_name != 'pull_request'
# timeout-minutes: 45
# steps:
# - uses: actions/checkout@v4
# - uses: dtolnay/rust-toolchain@stable
# - uses: dtolnay/install@cargo-outdated
# - run: cargo outdated --exit-code 1

View File

@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./rpcl2/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,serde

View File

@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./rpcl2/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,serde

View File

@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./rpcl2/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,serde

View File

@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./rpcl2/tests/Dockerfile_r2r_jazzy --tag r2r_jazzy
- run: docker run r2r_jazzy cargo test --features r2r_msg,derive,nalgebra,rayon
- run: docker run r2r_jazzy cargo test --features r2r_msg,derive,nalgebra,rayon,serde

View File

@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./rpcl2/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,serde

View File

@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./rpcl2/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,serde

View File

@ -18,4 +18,4 @@ jobs:
steps:
- uses: actions/checkout@v2
- run: docker build . --file ./rpcl2/tests/Dockerfile_rclrs_jazzy --tag rclrs_jazzy
- run: docker run rclrs_jazzy cargo test --features derive,nalgebra,rayon
- run: docker run rclrs_jazzy cargo test --features derive,nalgebra,rayon,serde

View File

@ -105,4 +105,4 @@ jobs:
- name: test
run: |
source /opt/ros/$ROS_DISTRO/setup.bash
cargo test --features rosrust_msg,derive,nalgebra,rayon
cargo test --features rosrust_msg,derive,nalgebra,rayon,serde

View File

@ -21,8 +21,8 @@ jobs:
with:
components: clippy
- name: Linting
run: cargo clippy --all-targets --features derive,nalgebra,rayon -- -D warnings
run: cargo clippy --all-targets --features derive,nalgebra,rayon,serde -- -D warnings
- name: Build examples with features
run: cargo build --examples --features derive,nalgebra,rayon
run: cargo build --examples --features derive,nalgebra,rayon,serde,ros2-interfaces-jazzy
- name: Test library
run: cargo test --features derive,nalgebra,rayon
run: cargo test --features derive,nalgebra,rayon,serde

2
.gitignore vendored
View File

@ -1,3 +1,3 @@
Cargo.lock
Cargo.lock*
.idea/
target/

View File

@ -7,7 +7,7 @@
ros_pointcloud2 uses its own type for the message `PointCloud2Msg` to keep the library framework agnostic. ROS1 and ROS2 are supported with feature flags.
Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.1/) for a complete guide.
Get started with the example below, check out the other use cases in the `examples` folder or see the [Documentation](https://docs.rs/ros_pointcloud2/0.5.2/) for a complete guide.
## Quickstart
@ -70,7 +70,7 @@ Features do not work properly with `rcrls` because the messages are linked exter
```toml
[dependencies]
ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.1_rclrs" }
ros_pointcloud2 = { git = "https://github.com/stelzo/ros_pointcloud2", tag = "v0.5.2_rclrs" }
```
Also, indicate the following dependencies to your linker inside the `package.xml` of your package.

View File

@ -8,10 +8,10 @@ repository = "https://github.com/stelzo/ros_pointcloud2"
license = "MIT OR Apache-2.0"
keywords = ["ros", "pointcloud2", "pointcloud", "message"]
categories = [
"science::robotics",
"encoding",
"data-structures",
"api-bindings",
"science::robotics",
"encoding",
"data-structures",
"api-bindings",
]
[dependencies]

View File

@ -1,7 +1,7 @@
[package]
name = "rpcl2-derive"
description = "Derive macros for ros_pointcloud2 crate."
version = "0.3.0"
version = "0.4.0"
edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"]
homepage = "https://github.com/stelzo/ros_pointcloud2"

View File

@ -48,8 +48,8 @@ fn struct_field_rename_array(input: &DeriveInput) -> Vec<String> {
panic!("expected `name` attribute");
}
});
if let Err(err) = res {
panic!("Error parsing attribute: {}", err);
if let Err(e) = res {
panic!("Error parsing attribute: {e}");
}
}
});
@ -188,7 +188,7 @@ fn layout_of_type(struct_name: &Ident, data: &Data) -> proc_macro2::TokenStream
quote! {
let size = ::core::mem::size_of::<#field_ty>();
let offset = ::ros_pointcloud2::memoffset::offset_of!(#struct_name, #field_name);
let offset = ::core::mem::offset_of!(#struct_name, #field_name);
if offset > last_field_end {
fields.push((1, "", offset - last_field_end));
}

View File

@ -1,6 +1,6 @@
[package]
name = "ros_pointcloud2"
version = "0.5.1"
version = "0.5.3"
edition = "2021"
authors = ["Christopher Sieh <stelzo@steado.de>"]
description = "Customizable conversions for working with sensor_msgs/PointCloud2."
@ -8,39 +8,42 @@ repository = "https://github.com/stelzo/ros_pointcloud2"
license = "MIT OR Apache-2.0"
keywords = ["ros", "pointcloud2", "pointcloud", "message"]
categories = [
"science::robotics",
"encoding",
"data-structures",
"api-bindings",
"science::robotics",
"encoding",
"data-structures",
"api-bindings",
]
readme = "../README.md"
documentation = "https://docs.rs/ros_pointcloud2"
homepage = "https://github.com/stelzo/ros_pointcloud2"
exclude = [
"**/.github/**",
"**/tests/**",
"**/examples/**",
"**/benches/**",
"**/target/**",
"**/build/**",
"**/dist/**",
"**/docs/**",
"**/doc/**",
"**/ensure_no_std/**",
"**/.github/**",
"**/tests/**",
"**/examples/**",
"**/benches/**",
"**/target/**",
"**/build/**",
"**/dist/**",
"**/docs/**",
"**/doc/**",
"**/ensure_no_std/**",
]
rust-version = "1.63"
rust-version = "1.77"
[dependencies]
rosrust_msg = { version = "0.1", optional = true }
rosrust = { version = "0.9.11", optional = true }
rosrust_msg = { version = "0.1.8", optional = true }
rosrust = { version = "0.9.12", optional = true }
r2r = { version = "0.9", optional = true }
rayon = { version = "1", optional = true }
nalgebra = { version = "0.33", optional = true, default-features = false }
rpcl2-derive = { version = "0.3", optional = true, path = "../rpcl2-derive" }
memoffset = { version = "0.9", optional = true }
rpcl2-derive = { version = "0.4", optional = true, path = "../rpcl2-derive" }
serde = { version = "1.0", features = ["derive"], optional = true }
ros2-interfaces-jazzy = { version = "0.0.4", features = [
"sensor_msgs",
], optional = true }
[dev-dependencies]
rand = "0.8"
rand = "0.9"
criterion = { version = "0.5", features = ["html_reports"] }
pretty_assertions = "1.0"
@ -50,16 +53,18 @@ harness = false
path = "benches/roundtrip.rs"
[features]
serde = ["dep:serde", "nalgebra/serde-serialize-no-std"]
rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"]
r2r_msg = ["dep:r2r"]
ros2-interfaces-jazzy = ["dep:ros2-interfaces-jazzy"]
rayon = ["dep:rayon"]
derive = ["dep:rpcl2-derive", "dep:memoffset"]
derive = ["dep:rpcl2-derive"]
nalgebra = ["dep:nalgebra"]
std = ["nalgebra/std"]
default = ["std"]
[package.metadata.docs.rs]
features = ["derive", "nalgebra", "rayon"]
features = ["derive", "nalgebra", "rayon", "serde"]
default-target = "x86_64-unknown-linux-gnu"
rustdoc-args = ["--cfg", "docsrs"]

View File

@ -100,13 +100,13 @@ fn minus(point1: &PointXYZ, point2: &PointXYZ) -> PointXYZ {
}
pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZB> {
let mut rng = rand::thread_rng();
let mut rng = rand::rng();
let mut pointcloud = Vec::with_capacity(num_points);
for _ in 0..num_points {
let point = PointXYZB {
x: rng.gen_range(min..max),
y: rng.gen_range(min..max),
z: rng.gen_range(min..max),
x: rng.random_range(min..max),
y: rng.random_range(min..max),
z: rng.random_range(min..max),
..Default::default()
};
pointcloud.push(point);

View File

@ -140,7 +140,7 @@ fn main() {
CustomPoint::new(7.0, 8.0, 9.0, 10.0, Label::Human),
];
println!("Original cloud: {:?}", cloud);
println!("Original cloud: {cloud:?}");
let msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
@ -151,7 +151,7 @@ fn main() {
.filter(|point: &CustomPoint| point.my_custom_label == Label::Deer)
.collect::<Vec<_>>();
println!("Filtered cloud: {:?}", out);
println!("Filtered cloud: {out:?}");
assert_eq!(
vec![CustomPoint::new(1.0, 2.0, 3.0, 4.0, Label::Deer),],

View File

@ -7,13 +7,13 @@ use std::time::Duration;
use ros_pointcloud2::prelude::*;
pub fn generate_random_pointcloud(num_points: usize, min: f32, max: f32) -> Vec<PointXYZ> {
let mut rng = rand::thread_rng();
let mut rng = rand::rng();
let mut pointcloud = Vec::with_capacity(num_points);
for _ in 0..num_points {
let point = PointXYZ {
x: rng.gen_range(min..max),
y: rng.gen_range(min..max),
z: rng.gen_range(min..max),
x: rng.random_range(min..max),
y: rng.random_range(min..max),
z: rng.random_range(min..max),
};
pointcloud.push(point);
}
@ -105,31 +105,31 @@ fn main() {
let how_often = 1_000;
let dur = measure_func_avg(how_often, how_many, roundtrip);
println!("roundtrip: {:?}", dur);
println!("roundtrip: {dur:?}");
#[cfg(feature = "rayon")]
let dur = measure_func_avg(how_often, how_many, roundtrip_par);
println!("roundtrip_par: {:?}", dur);
println!("roundtrip_par: {dur:?}");
println!("200k");
let how_many = 200_000;
let how_often = 100;
let dur = measure_func_avg(how_often, how_many, roundtrip_filter);
println!("roundtrip_filter: {:?}", dur);
println!("roundtrip_filter: {dur:?}");
#[cfg(feature = "rayon")]
let dur = measure_func_avg(how_often, how_many, roundtrip_filter_par);
println!("roundtrip_filter_par: {:?}", dur);
println!("roundtrip_filter_par: {dur:?}");
println!("10m");
let how_many = 10_000_000;
let how_often = 10;
let dur = measure_func_avg(how_often, how_many, roundtrip_filter);
println!("roundtrip_filter: {:?}", dur);
println!("roundtrip_filter: {dur:?}");
#[cfg(feature = "rayon")]
let dur = measure_func_avg(how_often, how_many, roundtrip_filter_par);
println!("roundtrip_filter_par: {:?}", dur);
println!("roundtrip_filter_par: {dur:?}");
}

View File

@ -12,7 +12,7 @@ fn main() {
PointXYZ::new(3.0, 3.0, 3.0),
];
println!("Original cloud: {:?}", cloud);
println!("Original cloud: {cloud:?}");
let msg = PointCloud2Msg::try_from_iter(cloud).unwrap();
@ -25,7 +25,7 @@ fn main() {
})
.collect::<Vec<_>>();
println!("Filtered cloud: {:?}", out);
println!("Filtered cloud: {out:?}");
assert_eq!(vec![PointXYZ::new(1.0, 1.0, 1.0),], out);
}

View File

@ -139,6 +139,7 @@
#![cfg_attr(not(feature = "std"), no_std)]
// Setup an allocator with #[global_allocator]
// see: https://doc.rust-lang.org/std/alloc/trait.GlobalAlloc.html
#![allow(unexpected_cfgs)]
pub mod points;
pub mod prelude;
@ -148,12 +149,11 @@ pub mod iterator;
use crate::ros::{HeaderMsg, PointFieldMsg};
#[cfg(feature = "derive")]
#[doc(hidden)]
pub use memoffset;
use core::str::FromStr;
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
#[macro_use]
extern crate alloc;
use alloc::string::String;
@ -285,6 +285,7 @@ impl LayoutField {
/// To assert consistency, the type should be build with the [`PointCloud2MsgBuilder`].
/// See the offical [ROS message description](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud2.html) for more information on the fields.
#[derive(Clone, Debug)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointCloud2Msg {
pub header: HeaderMsg,
pub dimensions: CloudDimensions,
@ -298,6 +299,7 @@ pub struct PointCloud2Msg {
/// Endianess encoding hint for the message.
#[derive(Default, Clone, Debug, PartialEq, Copy)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub enum Endian {
Big,
#[default]
@ -306,6 +308,7 @@ pub enum Endian {
/// Density flag for the message. Writing sparse point clouds is not supported.
#[derive(Default, Clone, Debug, PartialEq, Copy)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub enum Denseness {
#[default]
Dense,
@ -462,6 +465,7 @@ impl PointCloud2MsgBuilder {
/// Dimensions of the point cloud as width and height.
#[derive(Clone, Debug, Default)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct CloudDimensions {
pub width: u32,
pub height: u32,

View File

@ -1,6 +1,9 @@
//! Predefined point types commonly used in ROS.
use crate::{LayoutDescription, LayoutField, PointConvertible, RPCL2Point};
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
/// Packed RGB color encoding as used in ROS tools.
#[derive(Clone, Copy)]
#[repr(C, align(4))]
@ -9,6 +12,29 @@ pub union RGB {
unpacked: [u8; 4], // Padding
}
#[cfg(feature = "serde")]
#[cfg_attr(docsrs, doc(cfg(feature = "serde")))]
impl<'de> Deserialize<'de> for RGB {
fn deserialize<D>(deserializer: D) -> Result<RGB, D::Error>
where
D: serde::Deserializer<'de>,
{
let packed = f32::deserialize(deserializer)?;
Ok(RGB::new_from_packed_f32(packed))
}
}
#[cfg(feature = "serde")]
#[cfg_attr(docsrs, doc(cfg(feature = "serde")))]
impl Serialize for RGB {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: serde::Serializer,
{
f32::from(*self).serialize(serializer)
}
}
unsafe impl Send for RGB {}
unsafe impl Sync for RGB {}
@ -106,6 +132,7 @@ impl From<f32> for RGB {
/// 3D point with x, y, z coordinates, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZ {
pub x: f32,
pub y: f32,
@ -113,6 +140,7 @@ pub struct PointXYZ {
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<nalgebra::Point3<f32>> for PointXYZ {
fn from(point: nalgebra::Point3<f32>) -> Self {
Self {
@ -124,12 +152,73 @@ impl From<nalgebra::Point3<f32>> for PointXYZ {
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<&nalgebra::Point3<f32>> for PointXYZ {
fn from(point: &nalgebra::Point3<f32>) -> Self {
Self {
x: point.x,
y: point.y,
z: point.z,
}
}
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<nalgebra::Point3<f64>> for PointXYZ {
fn from(point: nalgebra::Point3<f64>) -> Self {
Self {
x: point.x as f32,
y: point.y as f32,
z: point.z as f32,
}
}
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<&nalgebra::Point3<f64>> for PointXYZ {
fn from(point: &nalgebra::Point3<f64>) -> Self {
Self {
x: point.x as f32,
y: point.y as f32,
z: point.z as f32,
}
}
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<PointXYZ> for nalgebra::Point3<f32> {
fn from(point: PointXYZ) -> Self {
nalgebra::Point3::new(point.x, point.y, point.z)
}
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<&PointXYZ> for nalgebra::Point3<f32> {
fn from(point: &PointXYZ) -> Self {
nalgebra::Point3::new(point.x, point.y, point.z)
}
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<PointXYZ> for nalgebra::Point3<f64> {
fn from(point: PointXYZ) -> Self {
nalgebra::Point3::new(point.x as f64, point.y as f64, point.z as f64)
}
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
impl From<&PointXYZ> for nalgebra::Point3<f64> {
fn from(point: &PointXYZ) -> Self {
nalgebra::Point3::new(point.x as f64, point.y as f64, point.z as f64)
}
}
impl PointXYZ {
#[must_use]
pub fn new(x: f32, y: f32, z: f32) -> Self {
@ -138,9 +227,22 @@ impl PointXYZ {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
self.into()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
self.into()
}
}
@ -173,6 +275,7 @@ unsafe impl PointConvertible<3> for PointXYZ {
/// 3D point with x, y, z coordinates and an intensity value, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZI {
pub x: f32,
pub y: f32,
@ -187,10 +290,23 @@ impl PointXYZI {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
unsafe impl Send for PointXYZI {}
@ -233,6 +349,7 @@ unsafe impl PointConvertible<4> for PointXYZI {
/// 3D point with x, y, z coordinates and a label, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZL {
pub x: f32,
pub y: f32,
@ -247,10 +364,23 @@ impl PointXYZL {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
unsafe impl Send for PointXYZL {}
@ -293,6 +423,7 @@ unsafe impl PointConvertible<4> for PointXYZL {
/// 3D point with x, y, z coordinates and an RGB color value, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZRGB {
pub x: f32,
pub y: f32,
@ -324,10 +455,23 @@ impl PointXYZRGB {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
unsafe impl Send for PointXYZRGB {}
@ -371,6 +515,7 @@ unsafe impl PointConvertible<4> for PointXYZRGB {
/// The alpha channel is commonly used as padding but this crate uses every channel and no padding.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZRGBA {
pub x: f32,
pub y: f32,
@ -403,10 +548,23 @@ impl PointXYZRGBA {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
unsafe impl Send for PointXYZRGBA {}
@ -453,6 +611,7 @@ unsafe impl PointConvertible<5> for PointXYZRGBA {
/// 3D point with x, y, z coordinates, an RGB color value and a normal vector, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZRGBNormal {
pub x: f32,
pub y: f32,
@ -502,10 +661,23 @@ impl PointXYZRGBNormal {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
unsafe impl Send for PointXYZRGBNormal {}
@ -558,6 +730,7 @@ unsafe impl PointConvertible<7> for PointXYZRGBNormal {
/// 3D point with x, y, z coordinates, an intensity value and a normal vector, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZINormal {
pub x: f32,
pub y: f32,
@ -592,10 +765,23 @@ impl PointXYZINormal {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
unsafe impl Send for PointXYZINormal {}
@ -648,6 +834,7 @@ unsafe impl PointConvertible<7> for PointXYZINormal {
/// 3D point with x, y, z coordinates and a label, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZRGBL {
pub x: f32,
pub y: f32,
@ -689,10 +876,23 @@ impl PointXYZRGBL {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
impl From<RPCL2Point<5>> for PointXYZRGBL {
@ -736,6 +936,7 @@ unsafe impl PointConvertible<5> for PointXYZRGBL {
/// 3D point with x, y, z coordinates and a normal vector, commonly used in ROS with PCL.
#[derive(Clone, Debug, PartialEq, Copy, Default)]
#[repr(C, align(16))]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointXYZNormal {
pub x: f32,
pub y: f32,
@ -760,10 +961,23 @@ impl PointXYZNormal {
/// Get the coordinates as a nalgebra Point3.
#[cfg(feature = "nalgebra")]
#[deprecated(since = "0.5.2", note = "please use `xyz_f32` instead")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz(&self) -> nalgebra::Point3<f32> {
self.xyz_f32()
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f32(&self) -> nalgebra::Point3<f32> {
nalgebra::Point3::new(self.x, self.y, self.z)
}
#[cfg(feature = "nalgebra")]
#[cfg_attr(docsrs, doc(cfg(feature = "nalgebra")))]
pub fn xyz_f64(&self) -> nalgebra::Point3<f64> {
nalgebra::Point3::new(self.x as f64, self.y as f64, self.z as f64)
}
}
unsafe impl Send for PointXYZNormal {}

View File

@ -25,13 +25,27 @@
use alloc::string::String;
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
/// [Time](https://docs.ros2.org/latest/api/builtin_interfaces/msg/Time.html) representation for ROS messages.
#[derive(Clone, Debug, Default)]
#[derive(Clone, Debug, Default, Copy)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct TimeMsg {
pub sec: i32,
pub nanosec: u32,
}
#[cfg(feature = "ros2-interfaces-jazzy")]
impl From<ros2_interfaces_jazzy::builtin_interfaces::msg::Time> for TimeMsg {
fn from(time: ros2_interfaces_jazzy::builtin_interfaces::msg::Time) -> Self {
Self {
sec: time.sec,
nanosec: time.nanosec,
}
}
}
#[cfg(feature = "rosrust_msg")]
impl From<rosrust::Time> for TimeMsg {
fn from(time: rosrust::Time) -> Self {
@ -44,14 +58,27 @@ impl From<rosrust::Time> for TimeMsg {
/// Represents the [header of a ROS message](https://docs.ros2.org/latest/api/std_msgs/msg/Header.html).
#[derive(Clone, Debug, Default)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct HeaderMsg {
pub seq: u32,
pub stamp: TimeMsg,
pub frame_id: String,
}
#[cfg(feature = "ros2-interfaces-jazzy")]
impl From<ros2_interfaces_jazzy::std_msgs::msg::Header> for HeaderMsg {
fn from(header: ros2_interfaces_jazzy::std_msgs::msg::Header) -> Self {
Self {
seq: 0,
stamp: header.stamp.into(),
frame_id: header.frame_id,
}
}
}
/// Describing a point encoded in the byte buffer of a PointCloud2 message. See the [official message description](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointField.html) for more information.
#[derive(Clone, Debug)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub struct PointFieldMsg {
pub name: String,
pub offset: u32,
@ -70,6 +97,89 @@ impl Default for PointFieldMsg {
}
}
#[cfg(feature = "ros2-interfaces-jazzy")]
impl From<ros2_interfaces_jazzy::sensor_msgs::msg::PointCloud2> for crate::PointCloud2Msg {
fn from(msg: ros2_interfaces_jazzy::sensor_msgs::msg::PointCloud2) -> Self {
Self {
header: HeaderMsg {
seq: 0,
stamp: TimeMsg {
sec: msg.header.stamp.sec,
nanosec: msg.header.stamp.nanosec,
},
frame_id: msg.header.frame_id,
},
dimensions: crate::CloudDimensions {
width: msg.width,
height: msg.height,
},
fields: msg
.fields
.into_iter()
.map(|field| PointFieldMsg {
name: field.name,
offset: field.offset,
datatype: field.datatype,
count: field.count,
})
.collect(),
endian: if msg.is_bigendian {
crate::Endian::Big
} else {
crate::Endian::Little
},
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
dense: if msg.is_dense {
crate::Denseness::Dense
} else {
crate::Denseness::Sparse
},
}
}
}
#[cfg(feature = "ros2-interfaces-jazzy")]
impl From<crate::PointCloud2Msg> for ros2_interfaces_jazzy::sensor_msgs::msg::PointCloud2 {
fn from(msg: crate::PointCloud2Msg) -> Self {
ros2_interfaces_jazzy::sensor_msgs::msg::PointCloud2 {
header: ros2_interfaces_jazzy::std_msgs::msg::Header {
stamp: ros2_interfaces_jazzy::builtin_interfaces::msg::Time {
sec: msg.header.stamp.sec,
nanosec: msg.header.stamp.nanosec,
},
frame_id: msg.header.frame_id,
},
height: msg.dimensions.height,
width: msg.dimensions.width,
fields: msg
.fields
.into_iter()
.map(
|field| ros2_interfaces_jazzy::sensor_msgs::msg::PointField {
name: field.name,
offset: field.offset,
datatype: field.datatype,
count: field.count,
},
)
.collect(),
is_bigendian: match msg.endian {
crate::Endian::Big => true,
crate::Endian::Little => false,
},
point_step: msg.point_step,
row_step: msg.row_step,
data: msg.data,
is_dense: match msg.dense {
crate::Denseness::Dense => true,
crate::Denseness::Sparse => false,
},
}
}
}
#[cfg(feature = "r2r_msg")]
impl From<r2r::sensor_msgs::msg::PointCloud2> for crate::PointCloud2Msg {
fn from(msg: r2r::sensor_msgs::msg::PointCloud2) -> Self {

View File

@ -5,6 +5,10 @@
# run rustup to test with latest rust version
rustup update
if [ -e "/opt/ros/jazzy/setup.bash" ]; then
source "/opt/ros/jazzy/setup.bash"
fi
if [ -e "/opt/ros/iron/setup.bash" ]; then
source "/opt/ros/iron/setup.bash"
fi

View File

@ -5,16 +5,19 @@
# run rustup to test with latest rust version
rustup update
if [ -e "/opt/ros/jazzy/setup.bash" ]; then
. "/opt/ros/jazzy/setup.bash"
fi
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
. "/ros2_rust_build/install/local_setup.bash"
cd /ros2_rust_build/src/ros_pointcloud2_tests/ || exit
"$@"

View File

@ -0,0 +1,32 @@
#[cfg(feature = "ros2-interfaces-jazzy")]
#[test]
fn convertxyz_ros2_interfaces_jazzy_msg() {
use ros_pointcloud2::{points::PointXYZ, PointCloud2Msg};
use ros2_interfaces_jazzy::sensor_msgs::msg::PointCloud2;
let cloud = vec![
PointXYZ {
x: 1.0,
y: 2.0,
z: 3.0,
},
PointXYZ {
x: 4.0,
y: 5.0,
z: 6.0,
},
PointXYZ {
x: 7.0,
y: 8.0,
z: 9.0,
},
];
let copy = cloud.clone();
let internal_cloud = PointCloud2Msg::try_from_iter(cloud).unwrap();
let ros2_msg_cloud: PointCloud2 = internal_cloud.into();
let convert_back_internal: PointCloud2Msg = ros2_msg_cloud.into();
let to_convert = convert_back_internal.try_into_iter().unwrap();
let back_to_type = to_convert.collect::<Vec<PointXYZ>>();
assert_eq!(copy, back_to_type);
}