From 22ba38af430b88b00f74263ac03e05a82b48fd67 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 20 Jul 2024 19:38:47 -0700 Subject: [PATCH 001/139] interim checkin of roslibrust version of tf lookup --- Cargo.toml | 6 +- tf_roslibrust/Cargo.toml | 40 + tf_roslibrust/README.md | 3 + tf_roslibrust/src/lib.rs | 32 + tf_roslibrust/src/tf_broadcaster.rs | 68 ++ tf_roslibrust/src/tf_buffer.rs | 881 ++++++++++++++++++ tf_roslibrust/src/tf_error.rs | 27 + tf_roslibrust/src/tf_graph_node.rs | 59 ++ .../src/tf_individual_transform_chain.rs | 111 +++ tf_roslibrust/src/tf_listener.rs | 100 ++ tf_roslibrust/src/tf_util.rs | 9 + tf_roslibrust/src/transforms.rs | 225 +++++ 12 files changed, 1560 insertions(+), 1 deletion(-) create mode 100644 tf_roslibrust/Cargo.toml create mode 100644 tf_roslibrust/README.md create mode 100644 tf_roslibrust/src/lib.rs create mode 100644 tf_roslibrust/src/tf_broadcaster.rs create mode 100644 tf_roslibrust/src/tf_buffer.rs create mode 100644 tf_roslibrust/src/tf_error.rs create mode 100644 tf_roslibrust/src/tf_graph_node.rs create mode 100644 tf_roslibrust/src/tf_individual_transform_chain.rs create mode 100644 tf_roslibrust/src/tf_listener.rs create mode 100644 tf_roslibrust/src/tf_util.rs create mode 100644 tf_roslibrust/src/transforms.rs diff --git a/Cargo.toml b/Cargo.toml index 5749749d0..8e3f0cd24 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,10 @@ [workspace] resolver = "2" -members = ["tf_rosrust", "tf_r2r"] +members = [ + "tf_r2r", + "tf_roslibrust", + "tf_rosrust", +] [workspace.package] edition = "2021" diff --git a/tf_roslibrust/Cargo.toml b/tf_roslibrust/Cargo.toml new file mode 100644 index 000000000..ca7e556e4 --- /dev/null +++ b/tf_roslibrust/Cargo.toml @@ -0,0 +1,40 @@ +[package] +name = "tf_roslibrust" +version = "0.1.0" +edition = "2021" +rust-version = "1.75" +license = "BSD 3-Clause" +publish = false +description = """ +This is a rust port of the [ROS tf library](http://wiki.ros.org/tf). +It is intended for being used in robots to help keep track of multiple coordinate +frames and is part of a larger suite of rust libraries that provide support for +various robotics related functionality. +This supports ROS1 using the roslibrust crate. +""" + +[dependencies] +anyhow = "1.0.86" +nalgebra.workspace = true +serde = { version = "1.0", features = ["derive"] } +serde-big-array = "0.5.1" +smart-default = "0.7.1" +thiserror.workspace = true +tokio = "1.38.0" + +[dependencies.roslibrust] +git = "https://github.com/Carter12s/roslibrust.git" +# tag v0.10.1 on github is confirmed to work +# version="0.9.0" +# rev = "883057f8d159b0298c243744d65971e746ec5dbc" +branch = "master" +features = ["ros1"] + +[dependencies.roslibrust_codegen] +git = "https://github.com/Carter12s/roslibrust.git" +branch = "master" + +[dependencies.roslibrust_codegen_macro] +git = "https://github.com/Carter12s/roslibrust.git" +branch = "master" + diff --git a/tf_roslibrust/README.md b/tf_roslibrust/README.md new file mode 100644 index 000000000..ee9c92ec1 --- /dev/null +++ b/tf_roslibrust/README.md @@ -0,0 +1,3 @@ +``` +ROS_PACKAGE_PATH=`rospack find geometry_msgs`:`rospack find tf2_msgs`:`rospack find std_msgs`:`rospack find actionlib_msgs` cargo build +``` diff --git a/tf_roslibrust/src/lib.rs b/tf_roslibrust/src/lib.rs new file mode 100644 index 000000000..e7dd1b8a3 --- /dev/null +++ b/tf_roslibrust/src/lib.rs @@ -0,0 +1,32 @@ +//! This is a rust port of the [ROS tf library](http://wiki.ros.org/tf). It is intended for being used in robots to help keep track of +//! multiple coordinate frames and is part of a larger suite of rust libraries that provide support for various robotics related functionality. +//! +//! Example usage: +//! +//! ```no_run +//! use tf_roslibrust::TfListener; +//! +//! rosrust::init("listener"); +//! let listener = TfListener::new(); +//! +//! let rate = rosrust::rate(1.0); +//! while rosrust::is_ok() { +//! let tf = listener.lookup_transform("camera", "base_link", rosrust::Time::new()); +//! println!("{tf:?}"); +//! rate.sleep(); +//! } +//!``` + +mod tf_broadcaster; +mod tf_buffer; +mod tf_error; +mod tf_graph_node; +mod tf_individual_transform_chain; +pub mod transforms; +pub use transforms::geometry_msgs::TransformStamped; +mod tf_listener; +pub use tf_broadcaster::TfBroadcaster; +pub use tf_buffer::TfBuffer; +pub use tf_error::TfError; +pub use tf_listener::TfListener; +mod tf_util; diff --git a/tf_roslibrust/src/tf_broadcaster.rs b/tf_roslibrust/src/tf_broadcaster.rs new file mode 100644 index 000000000..81253f1ae --- /dev/null +++ b/tf_roslibrust/src/tf_broadcaster.rs @@ -0,0 +1,68 @@ +use crate::{ + tf_error::TfError, + transforms::{geometry_msgs::TransformStamped, tf2_msgs::TFMessage}, +}; +use roslibrust::ros1::NodeHandle; +use roslibrust::ros1::Publisher; + + +/// Broadcast tf messages +/// +/// Example usage: +/// +/// TODO(lucasw) update this to use roslibrust +/// ```no_run +/// use tf_rosrust::{TfBroadcaster, TransformStamped}; +/// +/// rosrust::init("broadcaster"); +/// let broadcaster = TfBroadcaster::new(nh); +/// +/// let rate = rosrust::rate(100.0); +/// let mut tf = TransformStamped::default(); +/// tf.header.frame_id = "map".to_string(); +/// tf.child_frame_id = "tf_rosrust".to_string(); +/// tf.transform.rotation.w = 1.0; +/// let mut theta = 0.01_f64; +/// while rosrust::is_ok() { +/// theta += 0.01; +/// tf.header.stamp = rosrust::now(); +/// tf.transform.translation.x = theta.sin(); +/// tf.transform.translation.y = theta.cos(); +/// broadcaster.send_transform(tf.clone()).unwrap(); +/// println!("{tf:?}"); +/// rate.sleep(); +/// } +/// ``` +pub struct TfBroadcaster { + publisher: Publisher, +} + +impl TfBroadcaster { + /// Create a new TfBroadcaster + pub async fn new(nh: &NodeHandle) -> Self { + Self { + publisher: nh.advertise("/tf", 1000).await.unwrap(), + } + } + + // TODO(lucasw) need be able to send list of transforms + /// Broadcast transform + pub async fn send_transform(&self, tf: TransformStamped) -> Result<(), TfError> { + let tf_message = TFMessage { + transforms: vec![tf], + }; + // TODO: handle error correctly + self.publisher + .publish(&tf_message) + .await + .map_err(|err| TfError::Rosrust(err.description().to_string())) + } +} + +/* +impl Default for TfBroadcaster { + fn default(nh: &NodeHandle) -> Self { + TfBroadcaster::new(nh) + } +} +*/ diff --git a/tf_roslibrust/src/tf_buffer.rs b/tf_roslibrust/src/tf_buffer.rs new file mode 100644 index 000000000..8f077791f --- /dev/null +++ b/tf_roslibrust/src/tf_buffer.rs @@ -0,0 +1,881 @@ +use std::collections::{hash_map::Entry, HashMap, HashSet, VecDeque}; + +use roslibrust_codegen::Time; +use tokio::time::Duration; + +use crate::{ + tf_error::TfError, + tf_graph_node::TfGraphNode, + tf_individual_transform_chain::TfIndividualTransformChain, + transforms::{ + chain_transforms, + geometry_msgs::{Transform, TransformStamped}, + get_inverse, + std_msgs::Header, + tf2_msgs::TFMessage, + to_transform_stamped, + }, +}; + +#[derive(Clone, Debug)] +pub struct TfBuffer { + child_transform_index: HashMap>, + transform_data: HashMap, + cache_duration: Duration, +} + +const DEFAULT_CACHE_DURATION_SECONDS: i32 = 10; + +impl TfBuffer { + pub(crate) fn new() -> Self { + Self::new_with_duration(Duration::from_secs(DEFAULT_CACHE_DURATION_SECONDS)) + } + + pub fn new_with_duration(cache_duration: Duration) -> Self { + TfBuffer { + child_transform_index: HashMap::new(), + transform_data: HashMap::new(), + cache_duration, + } + } + + pub(crate) fn handle_incoming_transforms(&mut self, transforms: TFMessage, static_tf: bool) { + for transform in transforms.transforms { + self.add_transform(&transform, static_tf); + self.add_transform(&get_inverse(&transform), static_tf); + } + } + + fn add_transform(&mut self, transform: &TransformStamped, static_tf: bool) { + //TODO: Detect is new transform will create a loop + self.child_transform_index + .entry(transform.header.frame_id.clone()) + .or_default() + .insert(transform.child_frame_id.clone()); + + let key = TfGraphNode { + child: transform.child_frame_id.clone(), + parent: transform.header.frame_id.clone(), + }; + + match self.transform_data.entry(key) { + Entry::Occupied(e) => e.into_mut(), + Entry::Vacant(e) => e.insert(TfIndividualTransformChain::new( + static_tf, + self.cache_duration, + )), + } + .add_to_buffer(transform.clone()); + } + + /// Retrieves the transform path + fn retrieve_transform_path( + &self, + from: String, + to: String, + time: Time, + ) -> Result, TfError> { + let mut res = vec![]; + let mut frontier: VecDeque = VecDeque::new(); + let mut visited: HashSet = HashSet::new(); + let mut parents: HashMap = HashMap::new(); + visited.insert(from.clone()); + frontier.push_front(from.clone()); + + while !frontier.is_empty() { + let current_node = frontier.pop_front().unwrap(); + if current_node == to { + break; + } + if let Some(children) = self.child_transform_index.get(¤t_node) { + for v in children { + if visited.contains(v) { + continue; + } + + if self + .transform_data + .get(&TfGraphNode { + child: v.clone(), + parent: current_node.clone(), + }) + .map_or(false, |chain| chain.has_valid_transform(time)) + { + parents.insert(v.to_string(), current_node.clone()); + frontier.push_front(v.to_string()); + visited.insert(v.to_string()); + } + } + } + } + let mut r = to.clone(); + while r != from { + res.push(r.clone()); + let parent = parents.get(&r); + + match parent { + Some(x) => r = x.to_string(), + None => { + return Err(TfError::CouldNotFindTransform( + from, + to, + self.child_transform_index.clone(), + )) + } + } + } + res.reverse(); + Ok(res) + } + + /// Looks up a transform within the tree at a given time. + pub fn lookup_transform( + &self, + from: &str, + to: &str, + time: Time, + ) -> Result { + let from = from.to_string(); + let to = to.to_string(); + let path = self.retrieve_transform_path(from.clone(), to.clone(), time); + + match path { + Ok(path) => { + let mut tf_list: Vec = Vec::new(); + let mut first = from.clone(); + for intermediate in path { + let node = TfGraphNode { + child: intermediate.clone(), + parent: first.clone(), + }; + let time_cache = self.transform_data.get(&node).unwrap(); + let transform = time_cache.get_closest_transform(time); + match transform { + Err(e) => return Err(e), + Ok(x) => { + tf_list.push(x.transform); + } + } + first.clone_from(&intermediate); + } + let final_tf = chain_transforms(&tf_list); + let msg = TransformStamped { + child_frame_id: to, + header: Header { + frame_id: from, + stamp: time, + seq: 1, + }, + transform: final_tf, + }; + Ok(msg) + } + Err(x) => Err(x), + } + } + + pub(crate) fn lookup_transform_with_time_travel( + &self, + to: &str, + time2: Time, + from: &str, + time1: Time, + fixed_frame: &str, + ) -> Result { + let tf1 = self.lookup_transform(from, fixed_frame, time1)?; + let tf2 = self.lookup_transform(to, fixed_frame, time2)?; + let transforms = get_inverse(&tf1); + let result = chain_transforms(&[tf2.transform, transforms.transform]); + Ok(to_transform_stamped( + result, + from.to_string(), + to.to_string(), + time1, + )) + } +} + +#[cfg(test)] +mod test { + use Time; + + use super::*; + use crate::transforms::geometry_msgs::{Quaternion, Vector3}; + + const PARENT: &str = "parent"; + const CHILD0: &str = "child0"; + const CHILD1: &str = "child1"; + + /// This function builds a tree consisting of the following items: + /// * a world coordinate frame + /// * an item in the world frame at (1,0,0) + /// * base_link of a robot starting at (0,0,0) and progressing at (0,t,0) where t is time in seconds + /// * a camera which is (0.5, 0, 0) from the base_link + fn build_test_tree(buffer: &mut TfBuffer, time: f64) { + let nsecs = ((time - ((time.floor() as i64) as f64)) * 1E9) as u32; + + let world_to_item = TransformStamped { + child_frame_id: "item".to_string(), + header: Header { + frame_id: "world".to_string(), + stamp: Time { + secs: time.floor() as u32, + nsecs: nsecs, + }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0f64, + y: 0f64, + z: 0f64, + w: 1f64, + }, + translation: Vector3 { + x: 1f64, + y: 0f64, + z: 0f64, + }, + }, + }; + buffer.add_transform(&world_to_item, true); + buffer.add_transform(&get_inverse(&world_to_item), true); + + let world_to_base_link = TransformStamped { + child_frame_id: "base_link".to_string(), + header: Header { + frame_id: "world".to_string(), + stamp: Time { + secs: time.floor() as u32, + nsecs: nsecs, + }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0f64, + y: 0f64, + z: 0f64, + w: 1f64, + }, + translation: Vector3 { + x: 0f64, + y: time, + z: 0f64, + }, + }, + }; + buffer.add_transform(&world_to_base_link, false); + buffer.add_transform(&get_inverse(&world_to_base_link), false); + + let base_link_to_camera = TransformStamped { + child_frame_id: "camera".to_string(), + header: Header { + frame_id: "base_link".to_string(), + stamp: Time { + secs: time.floor() as u32, + nsecs: nsecs, + }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0f64, + y: 0f64, + z: 0f64, + w: 1f64, + }, + translation: Vector3 { + x: 0.5f64, + y: 0f64, + z: 0f64, + }, + }, + }; + buffer.add_transform(&base_link_to_camera, true); + buffer.add_transform(&get_inverse(&base_link_to_camera), true); + } + + /// Tests a basic lookup + #[test] + fn test_basic_tf_lookup() { + let mut tf_buffer = TfBuffer::new(); + build_test_tree(&mut tf_buffer, 0f64); + let res = tf_buffer.lookup_transform("camera", "item", Time { secs: 0, nsecs: 0 }); + let expected = TransformStamped { + child_frame_id: "item".to_string(), + header: Header { + frame_id: "camera".to_string(), + stamp: Time { secs: 0, nsecs: 0 }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0f64, + y: 0f64, + z: 0f64, + w: 1f64, + }, + translation: Vector3 { + x: 0.5f64, + y: 0f64, + z: 0f64, + }, + }, + }; + assert_eq!(res.unwrap(), expected); + } + + /// Tests an interpolated lookup. + #[test] + fn test_basic_tf_interpolation() { + let mut tf_buffer = TfBuffer::new(); + build_test_tree(&mut tf_buffer, 0f64); + build_test_tree(&mut tf_buffer, 1f64); + let res = tf_buffer.lookup_transform( + "camera", + "item", + Time { + secs: 0, + nsecs: 700_000_000, + }, + ); + let expected = TransformStamped { + child_frame_id: "item".to_string(), + header: Header { + frame_id: "camera".to_string(), + stamp: Time { + secs: 0, + nsecs: 700_000_000, + }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0f64, + y: 0f64, + z: 0f64, + w: 1f64, + }, + translation: Vector3 { + x: 0.5f64, + y: -0.7f64, + z: 0f64, + }, + }, + }; + assert_eq!(res.unwrap(), expected); + } + + /// Tests an interpolated lookup. + #[test] + fn test_basic_tf_time_travel() { + let mut tf_buffer = TfBuffer::new(); + build_test_tree(&mut tf_buffer, 0f64); + build_test_tree(&mut tf_buffer, 1f64); + let res = tf_buffer.lookup_transform_with_time_travel( + "camera", + Time { + secs: 0, + nsecs: 400_000_000, + }, + "camera", + Time { + secs: 0, + nsecs: 700_000_000, + }, + "item", + ); + let expected = TransformStamped { + child_frame_id: "camera".to_string(), + header: Header { + frame_id: "camera".to_string(), + stamp: Time { + secs: 0, + nsecs: 700_000_000, + }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0f64, + y: 0f64, + z: 0f64, + w: 1f64, + }, + translation: Vector3 { + x: 0f64, + y: 0.3f64, + z: 0f64, + }, + }, + }; + assert_approx_eq(res.unwrap(), expected); + } + + #[test] + fn test_add_transform() { + let mut tf_buffer = TfBuffer::new(); + let transform00 = TransformStamped { + header: Header { + frame_id: PARENT.to_string(), + stamp: Time { secs: 0, nsecs: 0 }, + ..Default::default() + }, + child_frame_id: CHILD0.to_string(), + ..Default::default() + }; + let transform01 = TransformStamped { + header: Header { + frame_id: PARENT.to_string(), + stamp: Time { secs: 1, nsecs: 0 }, + ..Default::default() + }, + child_frame_id: CHILD0.to_string(), + ..Default::default() + }; + let transform1 = TransformStamped { + header: Header { + frame_id: PARENT.to_string(), + ..Default::default() + }, + child_frame_id: CHILD1.to_string(), + ..Default::default() + }; + let transform0_key = TfGraphNode { + child: CHILD0.to_owned(), + parent: PARENT.to_owned(), + }; + let transform1_key = TfGraphNode { + child: CHILD1.to_owned(), + parent: PARENT.to_owned(), + }; + let static_tf = true; + tf_buffer.add_transform(&transform00, static_tf); + assert_eq!(tf_buffer.child_transform_index.len(), 1); + assert!(tf_buffer.child_transform_index.contains_key(PARENT)); + let children = tf_buffer.child_transform_index.get(PARENT).unwrap(); + assert_eq!(children.len(), 1); + assert!(children.contains(CHILD0)); + assert_eq!(tf_buffer.transform_data.len(), 1); + assert!(tf_buffer.transform_data.contains_key(&transform0_key)); + let data = tf_buffer.transform_data.get(&transform0_key); + assert!(data.is_some()); + assert_eq!(data.unwrap().transform_chain.len(), 1); + + tf_buffer.add_transform(&transform01, static_tf); + assert_eq!(tf_buffer.child_transform_index.len(), 1); + assert!(tf_buffer.child_transform_index.contains_key(PARENT)); + let children = tf_buffer.child_transform_index.get(PARENT).unwrap(); + assert_eq!(children.len(), 1); + assert!(children.contains(CHILD0)); + assert_eq!(tf_buffer.transform_data.len(), 1); + assert!(tf_buffer.transform_data.contains_key(&transform0_key)); + let data = tf_buffer.transform_data.get(&transform0_key); + assert!(data.is_some()); + assert_eq!(data.unwrap().transform_chain.len(), 2); + + tf_buffer.add_transform(&transform1, static_tf); + assert_eq!(tf_buffer.child_transform_index.len(), 1); + assert!(tf_buffer.child_transform_index.contains_key(PARENT)); + let children = tf_buffer.child_transform_index.get(PARENT).unwrap(); + assert_eq!(children.len(), 2); + assert!(children.contains(CHILD0)); + assert!(children.contains(CHILD1)); + assert_eq!(tf_buffer.transform_data.len(), 2); + assert!(tf_buffer.transform_data.contains_key(&transform0_key)); + assert!(tf_buffer.transform_data.contains_key(&transform1_key)); + let data = tf_buffer.transform_data.get(&transform0_key); + assert!(data.is_some()); + assert_eq!(data.unwrap().transform_chain.len(), 2); + let data = tf_buffer.transform_data.get(&transform1_key); + assert!(data.is_some()); + assert_eq!(data.unwrap().transform_chain.len(), 1); + } + + #[test] + fn test_cache_duration() { + let mut tf_buffer = TfBuffer::new_with_duration(Duration::from_secs(1)); + let transform00 = TransformStamped { + header: Header { + frame_id: PARENT.to_string(), + stamp: Time { secs: 0, nsecs: 0 }, + ..Default::default() + }, + child_frame_id: CHILD0.to_string(), + ..Default::default() + }; + let transform01 = TransformStamped { + header: Header { + frame_id: PARENT.to_string(), + stamp: Time { secs: 1, nsecs: 0 }, + ..Default::default() + }, + child_frame_id: CHILD0.to_string(), + ..Default::default() + }; + let transform02 = TransformStamped { + header: Header { + frame_id: PARENT.to_string(), + stamp: Time { secs: 2, nsecs: 0 }, + ..Default::default() + }, + child_frame_id: CHILD0.to_string(), + ..Default::default() + }; + let transform0_key = TfGraphNode { + child: CHILD0.to_owned(), + parent: PARENT.to_owned(), + }; + + let static_tf = true; + tf_buffer.add_transform(&transform00, static_tf); + assert_eq!(tf_buffer.child_transform_index.len(), 1); + assert_eq!(tf_buffer.transform_data.len(), 1); + assert!(tf_buffer.transform_data.contains_key(&transform0_key)); + let data = tf_buffer.transform_data.get(&transform0_key); + assert!(data.is_some()); + assert_eq!(data.unwrap().transform_chain.len(), 1); + assert_eq!( + data.unwrap().transform_chain[0].header.stamp, + Time::from_nanos(0) + ); + + tf_buffer.add_transform(&transform01, static_tf); + assert_eq!(tf_buffer.child_transform_index.len(), 1); + assert_eq!(tf_buffer.transform_data.len(), 1); + assert!(tf_buffer.transform_data.contains_key(&transform0_key)); + let data = tf_buffer.transform_data.get(&transform0_key); + assert!(data.is_some()); + assert_eq!(data.unwrap().transform_chain.len(), 2); + assert_eq!( + data.unwrap().transform_chain[0].header.stamp, + Time::from_nanos(0) + ); + assert_eq!( + data.unwrap().transform_chain[1].header.stamp, + Time::from_nanos(1_000_000_000) + ); + + tf_buffer.add_transform(&transform02, static_tf); + assert_eq!(tf_buffer.child_transform_index.len(), 1); + assert_eq!(tf_buffer.transform_data.len(), 1); + assert!(tf_buffer.transform_data.contains_key(&transform0_key)); + let data = tf_buffer.transform_data.get(&transform0_key); + assert!(data.is_some()); + assert_eq!(data.unwrap().transform_chain.len(), 2); + assert_eq!( + data.unwrap().transform_chain[0].header.stamp, + Time::from_nanos(1_000_000_000) + ); + assert_eq!( + data.unwrap().transform_chain[1].header.stamp, + Time::from_nanos(2_000_000_000) + ); + } + + fn assert_approx_eq(msg1: TransformStamped, msg2: TransformStamped) { + assert_eq!(msg1.header, msg2.header); + assert_eq!(msg1.child_frame_id, msg2.child_frame_id); + + assert!((msg1.transform.rotation.x - msg2.transform.rotation.x).abs() < 1e-9); + assert!((msg1.transform.rotation.y - msg2.transform.rotation.y).abs() < 1e-9); + assert!((msg1.transform.rotation.z - msg2.transform.rotation.z).abs() < 1e-9); + assert!((msg1.transform.rotation.w - msg2.transform.rotation.w).abs() < 1e-9); + + assert!((msg1.transform.translation.x - msg2.transform.translation.x).abs() < 1e-9); + assert!((msg1.transform.translation.y - msg2.transform.translation.y).abs() < 1e-9); + assert!((msg1.transform.translation.z - msg2.transform.translation.z).abs() < 1e-9); + } + + /// Tests a case in which the tree structure changes dynamically + /// time 1-2(sec): [base] -> [camera1] -> [marker] -> [target] + /// time 3-4(sec): [base] -> [camera2] -> [marker] -> [target] + /// time 5-6(sec): [base] -> [camera1] -> [marker] -> [target] + #[test] + fn test_dynamic_tree() { + let mut tf_buffer = TfBuffer::new(); + + let base_to_camera1 = TransformStamped { + child_frame_id: "camera1".to_string(), + header: Header { + frame_id: "base".to_string(), + stamp: Time { secs: 1, nsecs: 0 }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + translation: Vector3 { + x: 1.0, + y: 0.0, + z: 0.0, + }, + }, + }; + tf_buffer.add_transform(&base_to_camera1, true); + tf_buffer.add_transform(&get_inverse(&base_to_camera1), true); + + let base_to_camera2 = TransformStamped { + child_frame_id: "camera2".to_string(), + header: Header { + frame_id: "base".to_string(), + stamp: Time { secs: 1, nsecs: 0 }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 1.0, + w: 0.0, + }, + translation: Vector3 { + x: -1.0, + y: 0.0, + z: 0.0, + }, + }, + }; + tf_buffer.add_transform(&base_to_camera2, true); + tf_buffer.add_transform(&get_inverse(&base_to_camera2), true); + + let marker_to_target = TransformStamped { + child_frame_id: "target".to_string(), + header: Header { + frame_id: "marker".to_string(), + stamp: Time { secs: 1, nsecs: 0 }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + translation: Vector3 { + x: -0.5, + y: 0.0, + z: 0.0, + }, + }, + }; + tf_buffer.add_transform(&marker_to_target, true); + tf_buffer.add_transform(&get_inverse(&marker_to_target), true); + + let mut camera1_to_marker = TransformStamped { + child_frame_id: "marker".to_string(), + header: Header { + frame_id: "camera1".to_string(), + stamp: Time { secs: 1, nsecs: 0 }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + translation: Vector3 { + x: 1.0, + y: 1.0, + z: 0.0, + }, + }, + }; + tf_buffer.add_transform(&camera1_to_marker, false); + tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false); + + camera1_to_marker.header.stamp.sec = 2; + camera1_to_marker.header.seq += 1; + camera1_to_marker.transform.translation.y = -1.0; + tf_buffer.add_transform(&camera1_to_marker, false); + tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false); + + let mut camera2_to_marker = TransformStamped { + child_frame_id: "marker".to_string(), + header: Header { + frame_id: "camera2".to_string(), + stamp: Time { secs: 3, nsecs: 0 }, + seq: 1, + }, + transform: Transform { + rotation: Quaternion { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, + translation: Vector3 { + x: 1.0, + y: 1.0, + z: 0.0, + }, + }, + }; + tf_buffer.add_transform(&camera2_to_marker, false); + tf_buffer.add_transform(&get_inverse(&camera2_to_marker), false); + + camera2_to_marker.header.stamp.sec = 4; + camera2_to_marker.header.seq += 1; + camera2_to_marker.transform.translation.y = -1.0; + tf_buffer.add_transform(&camera2_to_marker, false); + tf_buffer.add_transform(&get_inverse(&camera2_to_marker), false); + + let result = + tf_buffer.lookup_transform("base", "target", Time { secs: 1, nsecs: 0 }); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: 1.5, + y: 1.0, + z: 0.0 + } + ); + + let result = tf_buffer.lookup_transform( + "base", + "target", + Time { + secs: 1, + nsecs: 500_000_000, + }, + ); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: 1.5, + y: 0.0, + z: 0.0 + } + ); + + let result = + tf_buffer.lookup_transform("base", "target", Time { secs: 2, nsecs: 0 }); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: 1.5, + y: -1.0, + z: 0.0 + } + ); + + let result = tf_buffer.lookup_transform( + "base", + "target", + Time { + secs: 2, + nsecs: 500_000_000, + }, + ); + assert!(result.is_err()); + + let result = + tf_buffer.lookup_transform("base", "target", Time { secs: 3, nsecs: 0 }); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: -1.5, + y: -1.0, + z: 0.0 + } + ); + + let result = tf_buffer.lookup_transform( + "base", + "target", + Time { + secs: 3, + nsecs: 500_000_000, + }, + ); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: -1.5, + y: -0.0, + z: 0.0 + } + ); + + let result = + tf_buffer.lookup_transform("base", "target", Time { secs: 4, nsecs: 0 }); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: -1.5, + y: 1.0, + z: 0.0 + } + ); + + let result = tf_buffer.lookup_transform( + "base", + "target", + Time { + secs: 4, + nsecs: 500_000_000, + }, + ); + assert!(result.is_err()); + + camera1_to_marker.header.stamp.sec = 5; + camera1_to_marker.header.seq += 1; + camera1_to_marker.transform.translation.x = 0.5; + camera1_to_marker.transform.translation.y = 1.0; + tf_buffer.add_transform(&camera1_to_marker, false); + tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false); + + camera1_to_marker.header.stamp.sec = 6; + camera1_to_marker.header.seq += 1; + camera1_to_marker.transform.translation.y = -1.0; + tf_buffer.add_transform(&camera1_to_marker, false); + tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false); + + let result = + tf_buffer.lookup_transform("base", "target", Time { secs: 5, nsecs: 0 }); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: 1.0, + y: 1.0, + z: 0.0 + } + ); + + let result = tf_buffer.lookup_transform( + "base", + "target", + Time { + secs: 5, + nsecs: 500_000_000, + }, + ); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: 1.0, + y: 0.0, + z: 0.0 + } + ); + + let result = + tf_buffer.lookup_transform("base", "target", Time { secs: 6, nsecs: 0 }); + assert_eq!( + result.unwrap().transform.translation, + Vector3 { + x: 1.0, + y: -1.0, + z: 0.0 + } + ); + } +} diff --git a/tf_roslibrust/src/tf_error.rs b/tf_roslibrust/src/tf_error.rs new file mode 100644 index 000000000..0e3b52865 --- /dev/null +++ b/tf_roslibrust/src/tf_error.rs @@ -0,0 +1,27 @@ +use std::collections::{HashMap, HashSet}; + +use roslibrust_codegen::Time; +use thiserror::Error; + +use crate::transforms::geometry_msgs::TransformStamped; + +/// Enumerates the different types of errors +#[derive(Clone, Debug, 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)] + AttemptedLookupInPast(Time, Box), + /// Error due to the transform not yet being available. + #[error("tf_rosrust: AttemptedLookupInFuture {:?} < {:?}",.0, .1)] + AttemptedLookUpInFuture(Box, Time), + /// There is no path between the from and to frame. + #[error("tf_rosrust: CouldNotFindTransform {} -> {} ({:?})", .0, .1, .2)] + CouldNotFindTransform(String, String, HashMap>), + /// In the event that a write is simultaneously happening with a read of the same tf buffer + #[error("tf_rosrust: CouldNotAcquireLock")] + CouldNotAcquireLock, + /// Error of rosrust + #[error("tf_rosrust: rosrust error {:?}", .0)] + Rosrust(String), +} diff --git a/tf_roslibrust/src/tf_graph_node.rs b/tf_roslibrust/src/tf_graph_node.rs new file mode 100644 index 000000000..5a59dfd63 --- /dev/null +++ b/tf_roslibrust/src/tf_graph_node.rs @@ -0,0 +1,59 @@ +#[derive(Clone, Debug, Hash, Eq, PartialEq)] +pub(crate) struct TfGraphNode { + pub(crate) child: String, + pub(crate) parent: String, +} + +#[cfg(test)] +mod test { + use std::collections::HashMap; + + use crate::tf_graph_node::TfGraphNode; + + #[test] + fn test() { + let mut hash_map = HashMap::new(); + hash_map.insert( + TfGraphNode { + child: "child0".to_owned(), + parent: "parent".to_owned(), + }, + 0, + ); + assert_eq!(hash_map.len(), 1); + hash_map.insert( + TfGraphNode { + child: "child0".to_owned(), + parent: "parent".to_owned(), + }, + 1, + ); + assert_eq!(hash_map.len(), 1); + + let mut hash_map = HashMap::new(); + hash_map.insert( + TfGraphNode { + child: "child0".to_owned(), + parent: "parent0".to_owned(), + }, + 0, + ); + assert_eq!(hash_map.len(), 1); + hash_map.insert( + TfGraphNode { + child: "parent0".to_owned(), + parent: "child0".to_owned(), + }, + 0, + ); + assert_eq!(hash_map.len(), 2); + hash_map.insert( + TfGraphNode { + child: "child0".to_owned(), + parent: "parent1".to_owned(), + }, + 0, + ); + assert_eq!(hash_map.len(), 3); + } +} diff --git a/tf_roslibrust/src/tf_individual_transform_chain.rs b/tf_roslibrust/src/tf_individual_transform_chain.rs new file mode 100644 index 000000000..26e532091 --- /dev/null +++ b/tf_roslibrust/src/tf_individual_transform_chain.rs @@ -0,0 +1,111 @@ +use roslibrust_codegen::Duration; +use roslibrust_codegen::Time; + +use crate::{ + tf_error::TfError, + transforms::{geometry_msgs::TransformStamped, interpolate, to_transform_stamped}, +}; + +fn get_nanos(dur: Duration) -> i64 { + i64::from(dur.as_secs()) * 1_000_000_000 + i64::from(dur.as_nanos()) +} + +fn binary_search_time(chain: &[TransformStamped], time: Time) -> Result { + chain.binary_search_by(|element| element.header.stamp.cmp(&time)) +} + +#[derive(Clone, Debug)] +pub(crate) struct TfIndividualTransformChain { + cache_duration: f64, + static_tf: bool, + // TODO: Implement a circular buffer. Current method is slow. + pub(crate) transform_chain: Vec, +} + +impl TfIndividualTransformChain { + pub(crate) fn new(static_tf: bool, cache_duration: f64) -> Self { + Self { + cache_duration, + transform_chain: Vec::new(), + static_tf, + } + } + + fn newest_stamp(&self) -> Option