From 0d99a2e5c1adf4efbfd1d275086fbefaef812b05 Mon Sep 17 00:00:00 2001 From: Christopher Sieh Date: Thu, 21 Mar 2024 16:32:33 +0100 Subject: [PATCH] adds rclrs support --- .github/workflows/rclrs_humble.yml | 17 ++++++++ Cargo.toml | 7 ++- src/ros_types.rs | 70 +++++++++++++++++++++++++++++- tests/Dockerfile_rclrs_humble | 34 +++++++++++++++ tests/rclrs_msg_test.rs | 34 +++++++++++++++ tests/rclrs_test.bash | 25 +++++++++++ 6 files changed, 185 insertions(+), 2 deletions(-) create mode 100644 .github/workflows/rclrs_humble.yml create mode 100644 tests/Dockerfile_rclrs_humble create mode 100644 tests/rclrs_msg_test.rs create mode 100644 tests/rclrs_test.bash diff --git a/.github/workflows/rclrs_humble.yml b/.github/workflows/rclrs_humble.yml new file mode 100644 index 0000000..34c0ae4 --- /dev/null +++ b/.github/workflows/rclrs_humble.yml @@ -0,0 +1,17 @@ +name: rclrs_humble + +on: + push: + pull_request: + workflow_dispatch: + +env: + CARGO_TERM_COLOR: always + +jobs: + tests_humble: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble + - run: docker run r2r_humble cargo test --features rclrs_msg diff --git a/Cargo.toml b/Cargo.toml index a97e861..dc642d6 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -21,6 +21,11 @@ rosrust_msg = { version = "0.1", optional = true } rosrust = { version = "0.9.11", optional = true } r2r = { version = "0.8.4", optional = true } +sensor_msgs = { version = "*", optional = true } +std_msgs = { version = "*", optional = true } +builtin_interfaces = { version = "*", optional = true } + [features] rosrust_msg = ["dep:rosrust_msg", "dep:rosrust"] -r2r_msg = ["dep:r2r"] \ No newline at end of file +r2r_msg = ["dep:r2r"] +rclrs_msg = ["dep:sensor_msgs", "dep:std_msgs", "dep:builtin_interfaces"] \ No newline at end of file diff --git a/src/ros_types.rs b/src/ros_types.rs index 9d729b6..7f42660 100644 --- a/src/ros_types.rs +++ b/src/ros_types.rs @@ -1,4 +1,4 @@ -#[cfg(not(feature = "rosrust_msg"))] +#[cfg(not(any(feature = "rosrust_msg", feature = "rclrs_msg")))] #[derive(Clone, Debug, Default)] pub struct TimeMsg { pub sec: u32, @@ -8,6 +8,9 @@ pub struct TimeMsg { #[cfg(feature = "rosrust_msg")] pub use rosrust::Time as TimeMsg; +#[cfg(feature = "rclrs_msg")] +pub use builtin_interfaces::msg::Time as TimeMsg; + #[derive(Clone, Debug, Default)] pub struct HeaderMsg { pub seq: u32, @@ -166,3 +169,68 @@ impl Into for PointCloud2Msg { } } } + +#[cfg(feature = "rclrs_msg")] +impl From for PointCloud2Msg { + fn from(msg: sensor_msgs::msg::PointCloud2) -> Self { + Self { + header: HeaderMsg { + seq: 0, + stamp: TimeMsg { + sec: msg.header.stamp.sec as u32, + nsec: msg.header.stamp.nanosec, + }, + frame_id: msg.header.frame_id, + }, + height: msg.height, + width: msg.width, + fields: msg + .fields + .into_iter() + .map(|field| PointFieldMsg { + name: field.name, + offset: field.offset, + datatype: field.datatype, + count: field.count, + }) + .collect(), + is_bigendian: msg.is_bigendian, + point_step: msg.point_step, + row_step: msg.row_step, + data: msg.data, + is_dense: msg.is_dense, + } + } +} + +#[cfg(feature = "rclrs_msg")] +impl Into for PointCloud2Msg { + fn into(self) -> sensor_msgs::msg::PointCloud2 { + sensor_msgs::msg::PointCloud2 { + header: std_msgs::msg::Header { + stamp: builtin_interfaces::msg::Time { + sec: self.header.stamp.sec as i32, + nanosec: self.header.stamp.nsec, + }, + frame_id: self.header.frame_id, + }, + height: self.height, + width: self.width, + fields: self + .fields + .into_iter() + .map(|field| sensor_msgs::msg::PointField { + name: field.name, + offset: field.offset, + datatype: field.datatype, + count: field.count, + }) + .collect(), + is_bigendian: self.is_bigendian, + point_step: self.point_step, + row_step: self.row_step, + data: self.data, + is_dense: self.is_dense, + } + } +} \ No newline at end of file diff --git a/tests/Dockerfile_rclrs_humble b/tests/Dockerfile_rclrs_humble new file mode 100644 index 0000000..9bf10c7 --- /dev/null +++ b/tests/Dockerfile_rclrs_humble @@ -0,0 +1,34 @@ +# syntax=docker/dockerfile:1 +FROM ros:humble + +# Update default packages +RUN apt-get update + +# Get Ubuntu packages +RUN apt-get install -y \ + build-essential \ + curl \ + libclang-dev \ + git \ + python3-pip \ + python3-vcstool + +# Get ros test messages +RUN apt-get install -y ros-humble-test-msgs ros-humble-example-interfaces + +# Get Rust +RUN curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | bash -s -- -y +RUN echo 'source $HOME/.cargo/env' >> $HOME/.bashrc +RUN cargo install --debug cargo-ament-build +RUN pip install git+https://github.com/colcon/colcon-cargo.git +RUN pip install git+https://github.com/colcon/colcon-ros-cargo.git + +WORKDIR /ros2_rust_build +RUN git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust +RUN vcs import src < src/ros2_rust/ros2_rust_humble.repos +RUN . /opt/ros/humble/setup.sh && colcon build + +WORKDIR /rclrs +COPY . . +RUN chmod +x /rclrs/tests/rclrs_test.bash +ENTRYPOINT [ "/rclrs/tests/rclrs_test.bash" ] diff --git a/tests/rclrs_msg_test.rs b/tests/rclrs_msg_test.rs new file mode 100644 index 0000000..25ec71c --- /dev/null +++ b/tests/rclrs_msg_test.rs @@ -0,0 +1,34 @@ +#[cfg(feature = "rclrs_msg")] +#[test] +fn convertxyz_rclrs_msg() { + use ros_pointcloud2::fallible_iterator::FallibleIterator; + use ros_pointcloud2::pcl_utils::PointXYZ; + use ros_pointcloud2::ros_types::PointCloud2Msg; + use ros_pointcloud2::ConvertXYZ; + use 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 = ConvertXYZ::try_from(cloud).unwrap().try_into().unwrap(); + let rclrs_msg_cloud: PointCloud2 = internal_cloud.into(); + let convert_back_internal: PointCloud2Msg = rclrs_msg_cloud.into(); + let to_convert: ConvertXYZ = ConvertXYZ::try_from(convert_back_internal).unwrap(); + let back_to_type = to_convert.map(|point| Ok(point)).collect::>(); + assert_eq!(copy, back_to_type.unwrap()); +} diff --git a/tests/rclrs_test.bash b/tests/rclrs_test.bash new file mode 100644 index 0000000..709befc --- /dev/null +++ b/tests/rclrs_test.bash @@ -0,0 +1,25 @@ +#!/bin/bash + +. "$HOME/.cargo/env" + +# run rustup to test with latest rust version +rustup update + +if [ -e "/opt/ros/iron/setup.bash" ]; then + source "/opt/ros/iron/setup.bash" + source "/ros2_rust_build/install/local_setup.bash" +fi + +if [ -e "/opt/ros/humble/setup.bash" ]; then + source "/opt/ros/humble/setup.bash" + source "/ros2_rust_build/install/local_setup.bash" +fi + +if [ -e "/opt/ros/galactic/setup.bash" ]; then + source "/opt/ros/galactic/setup.bash" + source "/ros2_rust_build/install/local_setup.bash" +fi + +cd /rclrs/ || exit + +"$@"