load a toml file full of transforms and publish them repeatedly with …

…updated header stamps, like the old tf static_transform_publisher- next make a real static pulisher and only publish them once latched

tf_roslibrust/Cargo.toml

@@ -19,9 +19,11 @@ chrono = "0.4.38"
 nalgebra.workspace = true
 serde = { version = "1.0", features = ["derive"] }
 serde-big-array = "0.5.1"
+serde_derive = "1.0.210"
 smart-default = "0.7.1"
 thiserror.workspace = true
 tokio = "1.38.0"
+toml = "0.8.19"
 
 [dependencies.roslibrust]
 git = "https://github.com/Carter12s/roslibrust.git"

tf_roslibrust/README.md

@@ -13,3 +13,9 @@ Get the transform from one frame to another like tf2_utils echo.py:
 ```
 echo frame1 frame2
 ```
+
+# tf publisher
+
+```
+ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo run --release --bin tf_publisher -- examples/transforms.toml
+```

tf_roslibrust/examples/transforms.toml

@@ -0,0 +1,13 @@
+[[tf]]
+frame = "odom"
+child_frame = "test0"
+x = 1.0
+y = 2.0
+z = 3.0
+roll = 0.34
+
+[[tf]]
+frame = "odom"
+child_frame = "test1"
+x = -1.0
+pitch = 1.3

tf_roslibrust/src/bin/publisher.rs

@@ -1,8 +1,93 @@
+use serde_derive::Deserialize;
+use std::collections::HashMap;
 use tf_roslibrust::tf_util;
 
 roslibrust_codegen_macro::find_and_generate_ros_messages!();
 
 /// Load a toml file of a list of trnasforms 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 = {
+            if let Some(roll) = tfr.roll {
+                roll
+            } else {
+                0.0
+            }
+        };
+        let pitch = {
+            if let Some(pitch) = tfr.pitch {
+                pitch
+            } else {
+                0.0
+            }
+        };
+        let yaw = {
+            if let Some(yaw) = tfr.yaw {
+                yaw
+            } else {
+                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> {
@@ -27,6 +112,9 @@ async fn main() -> Result<(), anyhow::Error> {
     let full_node_name = &format!("/{ns}/tf_publisher").replace("//", "/");
     // println!("{}", format!("full ns and node name: {full_node_name}"));
 
+    let config_file = &args2[1];
+    let mut tfm = get_transforms(config_file)?;
+
     let nh = NodeHandle::new(&std::env::var("ROS_MASTER_URI")?, full_node_name)
         .await
         .unwrap();
@@ -49,24 +137,13 @@ async fn main() -> Result<(), anyhow::Error> {
 
     // TODO(lucasw) optional time limit
     loop {
-        // TODO(lucasw) if there is a rosnode kill the listener will stop receiving messages
-        // but this loop keeps going
         update_interval.tick().await;
         let stamp = tf_util::stamp_now();
 
-        let tfm = {
-            let mut transform = geometry_msgs::TransformStamped::default();
-            transform.header.stamp = stamp;
-            transform.header.frame_id = "odom".to_string();
-            transform.child_frame_id = "test".to_string();
-            transform.transform.translation.z = 1.0;
-            transform.transform.rotation.w = 1.0;
-
-            let mut tfm = tf2_msgs::TFMessage::default();
-            tfm.transforms.push(transform);
-            tfm
-        };
-        // println!("{tfm:?}");
+        for tf in &mut tfm.transforms {
+            tf.header.stamp = stamp.clone();
+        }
+
         tf_publisher.publish(&tfm).await?;
     }