From 2344134a571838a965df1544a141181924f28aca Mon Sep 17 00:00:00 2001
From: Lucas Walter <wsacul@gmail.com>
Date: Sun, 22 Sep 2024 09:47:53 -0700
Subject: [PATCH] tf2tf implementation lives here now

---
 tf_roslibrust/src/tf_util.rs | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/tf_roslibrust/src/tf_util.rs b/tf_roslibrust/src/tf_util.rs
index f7c3f7a8..d5c245c1 100644
--- a/tf_roslibrust/src/tf_util.rs
+++ b/tf_roslibrust/src/tf_util.rs
@@ -138,3 +138,32 @@ pub fn get_transforms_from_toml(filename: &str) -> Result<tf2_msgs::TFMessage, a
 
     Ok(tfm)
 }
+
+// adapted from 'old_tf_to_new_tf'
+pub fn tf2tf(tf_listener: &crate::tf_listener::TfListener,
+    lookup_parent: &str, lookup_child: &str, broadcast_parent: &str, broadcast_child: &str,
+    zero_x: bool, zero_y: bool, zero_z: bool, zero_rotation: bool,
+) -> Result<geometry_msgs::TransformStamped, anyhow::Error> {
+    // get the most recent parent child transform, zero out x,y,z and/or rotation
+    // TODO(lucasw) does tf listener handle looping time?  No it doesn't
+    let mut tfs = tf_listener.lookup_transform(lookup_parent, lookup_child, None)?;
+
+    tfs.header.frame_id = broadcast_parent.to_string();
+    tfs.child_frame_id = broadcast_child.to_string();
+    if zero_x {
+        tfs.transform.translation.x = 0.0;
+    }
+    if zero_y {
+        tfs.transform.translation.y = 0.0;
+    }
+    if zero_z {
+        tfs.transform.translation.z = 0.0;
+    }
+    if zero_rotation {
+        tfs.transform.rotation.x = 0.0;
+        tfs.transform.rotation.y = 0.0;
+        tfs.transform.rotation.z = 0.0;
+        tfs.transform.rotation.w = 1.0;
+    }
+    Ok(tfs)
+}