From 922d3e226d1f1d86687b4e39d024275abf08180c Mon Sep 17 00:00:00 2001 From: stelzo Date: Fri, 7 Feb 2025 10:14:54 +0100 Subject: [PATCH] ros2 client jazzy --- .github/workflows/tests.yml | 2 +- rpcl2/Cargo.toml | 6 +- rpcl2/src/ros.rs | 104 ++++++++++++++++++++++ rpcl2/tests/ros2-interfaces-jazzy_test.rs | 32 +++++++ 4 files changed, 142 insertions(+), 2 deletions(-) create mode 100644 rpcl2/tests/ros2-interfaces-jazzy_test.rs diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index ca5a387..bc36d19 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -23,6 +23,6 @@ jobs: - name: Linting 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,serde + run: cargo build --examples --features derive,nalgebra,rayon,serde,ros2-interfaces-jazzy - name: Test library run: cargo test --features derive,nalgebra,rayon,serde diff --git a/rpcl2/Cargo.toml b/rpcl2/Cargo.toml index d0490ae..00a9790 100644 --- a/rpcl2/Cargo.toml +++ b/rpcl2/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "ros_pointcloud2" -version = "0.5.2" +version = "0.5.3" edition = "2021" authors = ["Christopher Sieh "] description = "Customizable conversions for working with sensor_msgs/PointCloud2." @@ -38,6 +38,9 @@ rayon = { version = "1", optional = true } nalgebra = { version = "0.33", optional = true, default-features = false } 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.9" @@ -53,6 +56,7 @@ path = "benches/roundtrip.rs" 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"] nalgebra = ["dep:nalgebra"] diff --git a/rpcl2/src/ros.rs b/rpcl2/src/ros.rs index 116f53c..f9ee221 100644 --- a/rpcl2/src/ros.rs +++ b/rpcl2/src/ros.rs @@ -36,6 +36,16 @@ pub struct TimeMsg { pub nanosec: u32, } +#[cfg(feature = "ros2-interfaces-jazzy")] +impl From 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 for TimeMsg { fn from(time: rosrust::Time) -> Self { @@ -55,6 +65,17 @@ pub struct HeaderMsg { pub frame_id: String, } +#[cfg(feature = "ros2-interfaces-jazzy")] +impl From 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))] @@ -76,6 +97,89 @@ impl Default for PointFieldMsg { } } +#[cfg(feature = "ros2-interfaces-jazzy")] +impl From 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 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 for crate::PointCloud2Msg { fn from(msg: r2r::sensor_msgs::msg::PointCloud2) -> Self { diff --git a/rpcl2/tests/ros2-interfaces-jazzy_test.rs b/rpcl2/tests/ros2-interfaces-jazzy_test.rs new file mode 100644 index 0000000..dde1df3 --- /dev/null +++ b/rpcl2/tests/ros2-interfaces-jazzy_test.rs @@ -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::>(); + assert_eq!(copy, back_to_type); +}