ros2 client jazzy

.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

rpcl2/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "ros_pointcloud2"
-version = "0.5.2"
+version = "0.5.3"
 edition = "2021"
 authors = ["Christopher Sieh <[email protected]>"]
 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"]

rpcl2/src/ros.rs

@@ -36,6 +36,16 @@ pub struct TimeMsg {
     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 {
@@ -55,6 +65,17 @@ pub struct HeaderMsg {
     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))]
@@ -76,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 {

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::<Vec<PointXYZ>>();
+    assert_eq!(copy, back_to_type);
+}