From a8d9fc6ca2dec9573722c6da0e94e39c53a6bb84 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Tue, 10 Sep 2024 15:17:36 -0700 Subject: [PATCH] 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 | 2 + tf_roslibrust/README.md | 6 ++ tf_roslibrust/examples/transforms.toml | 13 +++ tf_roslibrust/src/bin/publisher.rs | 107 +++++++++++++++++++++---- 4 files changed, 113 insertions(+), 15 deletions(-) create mode 100644 tf_roslibrust/examples/transforms.toml diff --git a/tf_roslibrust/Cargo.toml b/tf_roslibrust/Cargo.toml index 39b20c71..04ea771d 100644 --- a/tf_roslibrust/Cargo.toml +++ b/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" diff --git a/tf_roslibrust/README.md b/tf_roslibrust/README.md index 4c7695ee..d5c76220 100644 --- a/tf_roslibrust/README.md +++ b/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 +``` diff --git a/tf_roslibrust/examples/transforms.toml b/tf_roslibrust/examples/transforms.toml new file mode 100644 index 00000000..91bd3581 --- /dev/null +++ b/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 diff --git a/tf_roslibrust/src/bin/publisher.rs b/tf_roslibrust/src/bin/publisher.rs index 6b663a9f..d882becc 100644 --- a/tf_roslibrust/src/bin/publisher.rs +++ b/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, + y: Option, + z: Option, + roll: Option, + pitch: Option, + yaw: Option, +} + +fn get_transforms(filename: &str) -> Result { + 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> = 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?; }