Add tf2_r2r

.github/.cspell/project-dictionary.txt

@@ -1,7 +1,11 @@
 arjo
 deque
+DISTRO
+dpkg
+keyrings
 Maerz
 nanos
+nanosec
 nsec
 nsecs
 rosmsg

.github/workflows/ci.yml

@@ -26,18 +26,12 @@ concurrency:
   cancel-in-progress: true
 
 jobs:
-  macos:
-    name: Build on macOS
-    runs-on: macos-latest
-    timeout-minutes: 60
-    steps:
-      - uses: actions/checkout@v4
-      - uses: dtolnay/rust-toolchain@stable
-      - run: cargo build
-
-  linux:
-    name: Build on Linux
-    runs-on: ubuntu-latest
+  build:
+    strategy:
+      fail-fast: false
+      matrix:
+        os: [ubuntu-latest, macos-latest]
+    runs-on: ${{ matrix.os }}
     timeout-minutes: 60
     steps:
       - uses: actions/checkout@v4
@@ -51,7 +45,19 @@ jobs:
     steps:
       - uses: actions/checkout@v4
       - uses: dtolnay/rust-toolchain@stable
-      - run: cargo test
+      - run: echo "ROS2_DISTRO=humble" >>"${GITHUB_ENV}"
+      - name: Install ROS2
+        run: |
+          # for tf2_r2r
+          sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg
+          echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+          sudo apt-get update
+          sudo apt-get install -y ros-$ROS2_DISTRO-ros-core ros-$ROS2_DISTRO-geometry-msgs ros-$ROS2_DISTRO-tf2-msgs
+      - name: cargo test
+        run: |
+          # for tf2_r2r
+          source /opt/ros/$ROS2_DISTRO/setup.bash
+          cargo test --all-features
 
   fmt:
     name: Rustfmt
@@ -73,7 +79,19 @@ jobs:
       - uses: dtolnay/rust-toolchain@stable
         with:
           components: clippy
-      - run: cargo clippy --all-targets
+      - run: echo "ROS2_DISTRO=humble" >>"${GITHUB_ENV}"
+      - name: Install ROS2
+        run: |
+          # for tf2_r2r
+          sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg
+          echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+          sudo apt-get update
+          sudo apt-get install -y ros-$ROS2_DISTRO-ros-core ros-$ROS2_DISTRO-geometry-msgs ros-$ROS2_DISTRO-tf2-msgs
+      - name: cargo clippy
+        run: |
+          # for tf2_r2r
+          source /opt/ros/$ROS2_DISTRO/setup.bash
+          cargo clippy --all-features --all-targets
 
   spell-check:
     runs-on: ubuntu-latest

Cargo.toml

@@ -1,25 +1,20 @@
-[package]
-name = "tf_rosrust"
+[workspace]
+resolver = "2"
+members = ["tf_rosrust", "tf2_r2r"]
+
+[workspace.package]
 version = "0.1.0"
-authors = [
-    "Arjo Chakravarty <arjo129@gmail.com>",
-    "Michael Maerz <maerz@magazino.eu>",
-    "Mitsuharu Kojima <kojima@smilerobotics.com>",
-]
 edition = "2021"
-description = "This is a rust port of the [ROS tf library](http://wiki.ros.org/tf). It is intended for being used in robots to help keep track of multiple coordinate frames and is part of a larger suite of rust libraries that provide support for various robotics related functionality."
 license = "MIT OR Apache-2.0"
-keywords = ["transforms", "robotics", "ROS", "tf"]
 repository = "https://github.com/smilerobotics/tf_rosrust"
-homepage = "https://github.com/smilerobotics/tf_rosrust"
 
-[dependencies]
-rosrust = "0.9"
+[workspace.dependencies]
+futures = "0.3"
 nalgebra = "0.30"
+r2r = "0.8"
+rosrust = "0.9"
 thiserror = "1.0"
-
-[lints]
-workspace = true
+tokio = "1"
 
 [workspace.lints.rust]
 # missing_debug_implementations = "warn" # TODO: Most rosrust types don't implement Debug

README.md

@@ -38,7 +38,7 @@ After installing ROS, you may simply add this crate as a dependency to your carg
 
 ```toml
 [dependencies]
-tf_rosrust = "0.0.5"
+tf_rosrust = "0.1"
 ```
 
 ## Third party software

tf2_r2r/Cargo.toml

@@ -0,0 +1,21 @@
+[package]
+name = "tf2_r2r"
+version.workspace = true
+edition.workspace = true
+license.workspace = true
+repository.workspace = true
+keywords = ["transforms", "robotics", "ROS", "tf"]
+categories = ["science::robotics"]
+
+[features]
+ros2 = ["r2r"]
+
+[dependencies]
+futures.workspace = true
+nalgebra.workspace = true
+r2r = { workspace = true, optional = true }
+thiserror.workspace = true
+tokio = { workspace = true, features = ["full"] }
+
+[lints]
+workspace = true

tf2_r2r/src/lib.rs

@@ -0,0 +1,15 @@
+#![cfg(feature = "ros2")]
+
+mod tf_broadcaster;
+mod tf_buffer;
+mod tf_error;
+mod tf_graph_node;
+mod tf_individual_transform_chain;
+mod tf_listener;
+pub mod transforms;
+mod utils;
+
+pub use tf_broadcaster::TfBroadcaster;
+pub use tf_buffer::TfBuffer;
+pub use tf_error::TfError;
+pub use tf_listener::TfListener;

tf2_r2r/src/tf_broadcaster.rs

@@ -0,0 +1,28 @@
+use r2r::{geometry_msgs::msg::TransformStamped, tf2_msgs::msg::TFMessage, QosProfile};
+
+use crate::tf_error::TfError;
+
+pub struct TfBroadcaster {
+    publisher: r2r::Publisher<TFMessage>,
+}
+
+impl TfBroadcaster {
+    /// Create a new TfBroadcaster
+    #[track_caller]
+    pub fn new(node: &mut r2r::Node) -> Self {
+        Self {
+            publisher: node.create_publisher("/tf", QosProfile::default()).unwrap(),
+        }
+    }
+
+    /// Broadcast transform
+    pub fn send_transform(&self, tf: TransformStamped) -> Result<(), TfError> {
+        let tf_message = TFMessage {
+            transforms: vec![tf],
+        };
+        // TODO: handle error correctly
+        self.publisher
+            .publish(&tf_message)
+            .map_err(|err| TfError::R2r(err.to_string()))
+    }
+}