move loading transforms out of toml into tf_util so other crates can …

…use it
lucasw · Sep 16, 2024 · c7ef97f · c7ef97f
1 parent a3a73d9
commit c7ef97f
Show file tree

Hide file tree

Showing 2 changed files with 72 additions and 70 deletions.
diff --git a/tf_roslibrust/src/bin/publisher.rs b/tf_roslibrust/src/bin/publisher.rs
@@ -1,76 +1,9 @@
-use serde_derive::Deserialize;
-use std::collections::HashMap;
 use tf_roslibrust::tf_util;
+use tf_roslibrust::tf_util::tf2_msgs;
 
-roslibrust_codegen_macro::find_and_generate_ros_messages!();
-
-/// Load a toml file of a list of trnasforms and publish them
+/// Load a toml file of a list of transforms and publish them
 /// tf_publisher examples/transforms.toml
 
-#[derive(Deserialize, Debug)]
-struct TransformRaw {
-    frame: String,
-    child_frame: String,
-    x: Option<f64>,
-    y: Option<f64>,
-    z: Option<f64>,
-    roll: Option<f64>,
-    pitch: Option<f64>,
-    yaw: Option<f64>,
-}
-
-fn get_transforms(filename: &str) -> Result<tf2_msgs::TFMessage, anyhow::Error> {
-    let tf_data = {
-        let contents = match std::fs::read_to_string(filename) {
-            Ok(contents) => contents,
-            // Handle the `error` case.
-            Err(err) => {
-                panic!("Could not read file '{filename}', {err}");
-            }
-        };
-        // println!("{contents}");
-        let tf_data: HashMap<String, Vec<TransformRaw>> = toml::from_str(&contents)?;
-        // println!("{tf_data:?}");
-
-        tf_data
-    };
-
-    let mut tfm = tf2_msgs::TFMessage::default();
-    for tfr in tf_data.get("tf").ok_or(anyhow::anyhow!("no tfs"))? {
-        let mut transform = geometry_msgs::TransformStamped::default();
-        // transform.header.stamp = stamp;
-        transform.header.frame_id = tfr.frame.clone();
-        transform.child_frame_id = tfr.child_frame.clone();
-        transform.transform.rotation.w = 1.0;
-
-        // TODO(lucasw) try out a macro
-        if let Some(x) = tfr.x {
-            transform.transform.translation.x = x;
-        }
-        if let Some(y) = tfr.y {
-            transform.transform.translation.y = y;
-        }
-        if let Some(z) = tfr.z {
-            transform.transform.translation.z = z;
-        }
-
-        let roll = tfr.roll.unwrap_or(0.0);
-        let pitch = tfr.pitch.unwrap_or(0.0);
-        let yaw = tfr.yaw.unwrap_or(0.0);
-
-        let unit_quat = nalgebra::UnitQuaternion::from_euler_angles(roll, pitch, yaw);
-        let quat = unit_quat.quaternion();
-        transform.transform.rotation.x = quat.coords[0];
-        transform.transform.rotation.y = quat.coords[1];
-        transform.transform.rotation.z = quat.coords[2];
-        transform.transform.rotation.w = quat.coords[3];
-
-        tfm.transforms.push(transform);
-    }
-
-    Ok(tfm)
-}
-
 #[tokio::main]
 async fn main() -> Result<(), anyhow::Error> {
     use roslibrust::ros1::NodeHandle;
@@ -95,7 +28,7 @@ async fn main() -> Result<(), anyhow::Error> {
     // println!("{}", format!("full ns and node name: {full_node_name}"));
 
     let config_file = &args2[1];
-    let mut tfm = get_transforms(config_file)?;
+    let mut tfm = tf_util::get_transforms_from_toml(config_file)?;
 
     let nh = NodeHandle::new(&std::env::var("ROS_MASTER_URI")?, full_node_name)
         .await

diff --git a/tf_roslibrust/src/tf_util.rs b/tf_roslibrust/src/tf_util.rs
@@ -1,7 +1,11 @@
 use chrono::TimeDelta;
 use roslibrust_codegen::Time;
+use serde_derive::Deserialize;
+use std::collections::HashMap;
 use std::time::SystemTime;
 
+roslibrust_codegen_macro::find_and_generate_ros_messages!();
+
 pub fn to_stamp(secs: u32, nsecs: u32) -> Time {
     roslibrust_codegen::Time { secs, nsecs }
 }
@@ -33,3 +37,68 @@ pub fn duration_to_f64(time: TimeDelta) -> f64 {
 pub fn stamp_to_f64(stamp: Time) -> f64 {
     stamp.secs as f64 + (stamp.nsecs as f64) / 1e9
 }
+
+/// use for loading from a toml
+#[derive(Deserialize, Debug)]
+struct TransformRaw {
+    frame: String,
+    child_frame: String,
+    x: Option<f64>,
+    y: Option<f64>,
+    z: Option<f64>,
+    roll: Option<f64>,
+    pitch: Option<f64>,
+    yaw: Option<f64>,
+}
+
+pub fn get_transforms_from_toml(filename: &str) -> Result<tf2_msgs::TFMessage, anyhow::Error> {
+    let tf_data = {
+        let contents = match std::fs::read_to_string(filename) {
+            Ok(contents) => contents,
+            // Handle the `error` case.
+            Err(err) => {
+                panic!("Could not read file '{filename}', {err}");
+            }
+        };
+        // println!("{contents}");
+        let tf_data: HashMap<String, Vec<TransformRaw>> = toml::from_str(&contents)?;
+        // println!("{tf_data:?}");
+
+        tf_data
+    };
+
+    let mut tfm = tf2_msgs::TFMessage::default();
+    for tfr in tf_data.get("tf").ok_or(anyhow::anyhow!("no tfs"))? {
+        let mut transform = geometry_msgs::TransformStamped::default();
+        // transform.header.stamp = stamp;
+        transform.header.frame_id = tfr.frame.clone();
+        transform.child_frame_id = tfr.child_frame.clone();
+        transform.transform.rotation.w = 1.0;
+
+        // TODO(lucasw) try out a macro
+        if let Some(x) = tfr.x {
+            transform.transform.translation.x = x;
+        }
+        if let Some(y) = tfr.y {
+            transform.transform.translation.y = y;
+        }
+        if let Some(z) = tfr.z {
+            transform.transform.translation.z = z;
+        }
+
+        let roll = tfr.roll.unwrap_or(0.0);
+        let pitch = tfr.pitch.unwrap_or(0.0);
+        let yaw = tfr.yaw.unwrap_or(0.0);
+
+        let unit_quat = nalgebra::UnitQuaternion::from_euler_angles(roll, pitch, yaw);
+        let quat = unit_quat.quaternion();
+        transform.transform.rotation.x = quat.coords[0];
+        transform.transform.rotation.y = quat.coords[1];
+        transform.transform.rotation.z = quat.coords[2];
+        transform.transform.rotation.w = quat.coords[3];
+
+        tfm.transforms.push(transform);
+    }
+
+    Ok(tfm)
+}