[tf2_r2r] Various fixes

.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,6 +1,20 @@
 [workspace]
-members = ["tf_rosrust", "tf2_r2r"]
 resolver = "2"
+members = ["tf_rosrust", "tf2_r2r"]
+
+[workspace.package]
+version = "0.1.0"
+edition = "2021"
+license = "MIT OR Apache-2.0"
+repository = "https://github.com/smilerobotics/tf_rosrust"
+
+[workspace.dependencies]
+futures = "0.3"
+nalgebra = "0.30"
+r2r = "0.8"
+rosrust = "0.9"
+thiserror = "1.0"
+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

@@ -1,15 +1,21 @@
 [package]
 name = "tf2_r2r"
-version = "0.1.0"
-edition = "2021"
-
-# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+version.workspace = true
+edition.workspace = true
+license.workspace = true
+repository.workspace = true
+keywords = ["transforms", "robotics", "ROS", "tf"]
+categories = ["science::robotics"]
 
 [features]
+ros2 = ["r2r"]
 
 [dependencies]
-nalgebra = "0.30"
-r2r = "0.7"
-thiserror = "1"
-tokio = { version = "1", features = ["full"] }
-futures = "0.3"
+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

@@ -1,11 +1,13 @@
+#![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;
-pub mod utils;
+mod utils;
 
 pub use tf_broadcaster::TfBroadcaster;
 pub use tf_buffer::TfBuffer;

tf2_r2r/src/tf_broadcaster.rs

@@ -1,18 +1,17 @@
-use crate::tf_error::TfError;
 use r2r::{geometry_msgs::msg::TransformStamped, tf2_msgs::msg::TFMessage, QosProfile};
 
+use crate::tf_error::TfError;
+
 pub struct TfBroadcaster {
     publisher: r2r::Publisher<TFMessage>,
-    _node: r2r::Node,
 }
 
 impl TfBroadcaster {
     /// Create a new TfBroadcaster
-    pub fn new(ctx: r2r::Context) -> Self {
-        let mut node = r2r::Node::create(ctx, "tf_broadcaster", "tf2_r2r").unwrap();
+    #[track_caller]
+    pub fn new(node: &mut r2r::Node) -> Self {
         Self {
             publisher: node.create_publisher("/tf", QosProfile::default()).unwrap(),
-            _node: node,
         }
     }
 

tf2_r2r/src/tf_buffer.rs

@@ -99,7 +99,7 @@ impl TfBuffer {
                             child: v.clone(),
                             parent: current_node.clone(),
                         })
-                        .map_or(false, |chain| chain.has_valid_transform(&time))
+                        .map_or(false, |chain| chain.has_valid_transform(time))
                     {
                         parents.insert(v.to_string(), current_node.clone());
                         frontier.push_front(v.to_string());
@@ -137,7 +137,7 @@ impl TfBuffer {
     ) -> Result<TransformStamped, TfError> {
         let from = from.to_string();
         let to = to.to_string();
-        let path = self.retrieve_transform_path(from.clone(), to.clone(), &time);
+        let path = self.retrieve_transform_path(from.clone(), to.clone(), time);
 
         match path {
             Ok(path) => {
@@ -149,7 +149,7 @@ impl TfBuffer {
                         parent: first.clone(),
                     };
                     let time_cache = self.transform_data.get(&node).unwrap();
-                    let transform = time_cache.get_closest_transform(&time);
+                    let transform = time_cache.get_closest_transform(time);
                     match transform {
                         Err(e) => return Err(e),
                         Ok(x) => {
@@ -196,15 +196,14 @@ impl TfBuffer {
 
 #[cfg(test)]
 mod test {
-    use crate::utils::time_from_nanosec;
-
-    use super::*;
-
     use r2r::{
         builtin_interfaces::msg::Time,
         geometry_msgs::msg::{Quaternion, Vector3},
     };
 
+    use super::*;
+    use crate::utils::time_from_nanosec;
+
     const PARENT: &str = "parent";
     const CHILD0: &str = "child0";
     const CHILD1: &str = "child1";
@@ -398,7 +397,6 @@ mod test {
             header: Header {
                 frame_id: PARENT.to_string(),
                 stamp: time_from_nanosec(0),
-                ..Default::default()
             },
             child_frame_id: CHILD0.to_string(),
             ..Default::default()
@@ -407,7 +405,6 @@ mod test {
             header: Header {
                 frame_id: PARENT.to_string(),
                 stamp: Time { sec: 1, nanosec: 0 },
-                ..Default::default()
             },
             child_frame_id: CHILD0.to_string(),
             ..Default::default()
@@ -478,7 +475,6 @@ mod test {
             header: Header {
                 frame_id: PARENT.to_string(),
                 stamp: time_from_nanosec(0),
-                ..Default::default()
             },
             child_frame_id: CHILD0.to_string(),
             ..Default::default()
@@ -487,7 +483,6 @@ mod test {
             header: Header {
                 frame_id: PARENT.to_string(),
                 stamp: Time { sec: 1, nanosec: 0 },
-                ..Default::default()
             },
             child_frame_id: CHILD0.to_string(),
             ..Default::default()
@@ -496,7 +491,6 @@ mod test {
             header: Header {
                 frame_id: PARENT.to_string(),
                 stamp: Time { sec: 2, nanosec: 0 },
-                ..Default::default()
             },
             child_frame_id: CHILD0.to_string(),
             ..Default::default()

tf2_r2r/src/tf_error.rs

@@ -8,18 +8,18 @@ use thiserror::Error;
 #[non_exhaustive]
 pub enum TfError {
     /// Error due to looking up too far in the past. I.E the information is no longer available in the TF Cache.
-    #[error("tf_rosrust: AttemptedLookupInPast {:?} < {:?}",.0, .1)]
+    #[error("tf2_r2r: AttemptedLookupInPast {:?} < {:?}",.0, .1)]
     AttemptedLookupInPast(Time, Box<TransformStamped>),
     /// Error due to the transform not yet being available.
-    #[error("tf_rosrust: AttemptedLookupInFuture {:?} < {:?}",.0, .1)]
+    #[error("tf2_r2r: AttemptedLookupInFuture {:?} < {:?}",.0, .1)]
     AttemptedLookUpInFuture(Box<TransformStamped>, Time),
     /// There is no path between the from and to frame.
-    #[error("tf_rosrust: CouldNotFindTransform {} -> {} ({:?})", .0, .1, .2)]
+    #[error("tf2_r2r: CouldNotFindTransform {} -> {} ({:?})", .0, .1, .2)]
     CouldNotFindTransform(String, String, HashMap<String, HashSet<String>>),
     /// In the event that a write is simultaneously happening with a read of the same tf buffer
-    #[error("tf_rosrust: CouldNotAcquireLock")]
+    #[error("tf2_r2r: CouldNotAcquireLock")]
     CouldNotAcquireLock,
-    /// Error of rosrust
-    #[error("tf_rosrust: rosrust error {:?}", .0)]
+    /// Error of r2r
+    #[error("tf2_r2r: r2r error {:?}", .0)]
     R2r(String),
 }

tf2_r2r/src/tf_individual_transform_chain.rs

@@ -15,32 +15,32 @@ fn get_nanos(dur: Duration) -> i64 {
 
 fn binary_search_time(chain: &[TransformStamped], time: &Time) -> Result<usize, usize> {
     chain.binary_search_by(|element| {
-        time_as_ns_i64(&element.header.stamp).cmp(&time_as_ns_i64(&time))
+        time_as_ns_i64(&element.header.stamp).cmp(&time_as_ns_i64(time))
     })
 }
 
 #[derive(Clone, Debug)]
 pub(crate) struct TfIndividualTransformChain {
     cache_duration: Duration,
     static_tf: bool,
-    //TODO:  Implement a circular buffer. Current method is slow.
+    // TODO: Implement a circular buffer. Current method is slow.
     pub(crate) transform_chain: Vec<TransformStamped>,
 }
 
 impl TfIndividualTransformChain {
-    pub fn new(static_tf: bool, cache_duration: Duration) -> Self {
+    pub(crate) fn new(static_tf: bool, cache_duration: Duration) -> Self {
         Self {
             cache_duration,
             transform_chain: Vec::new(),
             static_tf,
         }
     }
 
-    pub fn newest_stamp(&self) -> Option<Time> {
+    fn newest_stamp(&self) -> Option<Time> {
         self.transform_chain.last().map(|x| x.header.stamp.clone())
     }
 
-    pub fn add_to_buffer(&mut self, msg: TransformStamped) {
+    pub(crate) fn add_to_buffer(&mut self, msg: TransformStamped) {
         let index = binary_search_time(&self.transform_chain, &msg.header.stamp)
             .unwrap_or_else(|index| index);
         self.transform_chain.insert(index, msg.clone());
@@ -59,7 +59,7 @@ impl TfIndividualTransformChain {
     }
 
     /// If timestamp is zero, return the latest transform.
-    pub fn get_closest_transform(&self, time: &Time) -> Result<TransformStamped, TfError> {
+    pub(crate) fn get_closest_transform(&self, time: &Time) -> Result<TransformStamped, TfError> {
         if time_as_ns_i64(time) == 0 {
             return Ok(self.transform_chain.last().unwrap().clone());
         }
@@ -68,7 +68,7 @@ impl TfIndividualTransformChain {
             return Ok(self.transform_chain.last().unwrap().clone());
         }
 
-        match binary_search_time(&self.transform_chain, &time) {
+        match binary_search_time(&self.transform_chain, time) {
             Ok(x) => return Ok(self.transform_chain.get(x).unwrap().clone()),
             Err(x) => {
                 if x == 0 {
@@ -96,16 +96,16 @@ impl TfIndividualTransformChain {
                 let header = self.transform_chain.get(x).unwrap().header.clone();
                 let child_frame = self.transform_chain.get(x).unwrap().child_frame_id.clone();
                 let total_duration = get_nanos(sub_time_and_time(&time2, &time1)) as f64;
-                let desired_duration = get_nanos(sub_time_and_time(&time, &time1)) as f64;
+                let desired_duration = get_nanos(sub_time_and_time(time, &time1)) as f64;
                 let weight = 1.0 - desired_duration / total_duration;
                 let final_tf = interpolate(tf1, tf2, weight);
-                let ros_msg = to_transform_stamped(final_tf, header.frame_id, child_frame, &time);
+                let ros_msg = to_transform_stamped(final_tf, header.frame_id, child_frame, time);
                 Ok(ros_msg)
             }
         }
     }
 
-    pub fn has_valid_transform(&self, time: &Time) -> bool {
+    pub(crate) fn has_valid_transform(&self, time: &Time) -> bool {
         if self.transform_chain.is_empty() {
             return false;
         }
@@ -118,6 +118,6 @@ impl TfIndividualTransformChain {
         let last = self.transform_chain.last().unwrap();
 
         time_as_ns_i64(time) == 0
-            || is_time_in_range_eq(&time, &first.header.stamp, &last.header.stamp)
+            || is_time_in_range_eq(time, &first.header.stamp, &last.header.stamp)
     }
 }