diff --git a/Cargo.lock b/Cargo.lock index 74b371b..cc946e9 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -4801,9 +4801,9 @@ dependencies = [ [[package]] name = "transforms" -version = "1.1.1" +version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e54745b240e84fb7064614afd5185a6243c84dc6823329ddb5dc81faf1c017c9" +checksum = "f6319e43042a9870592131406e2018fb88bb8114b282b98f99bb7f5922f03386" dependencies = [ "approx", "hashbrown 0.16.1", diff --git a/roslibrust_transforms/Cargo.toml b/roslibrust_transforms/Cargo.toml index 7147837..a790435 100644 --- a/roslibrust_transforms/Cargo.toml +++ b/roslibrust_transforms/Cargo.toml @@ -13,7 +13,7 @@ keywords = ["ROS", "robotics", "tf2", "transforms", "coordinates"] roslibrust_common = { path = "../roslibrust_common", version = "0.20" } # roslibrust is needed because the macro-generated code references ::roslibrust:: roslibrust = { path = "../roslibrust", version = "0.20", features = ["macro", "codegen"] } -transforms = "1.1" +transforms = "1.2" tokio = { workspace = true } log = { workspace = true } thiserror = "2.0" diff --git a/roslibrust_transforms/src/lib.rs b/roslibrust_transforms/src/lib.rs index 81e1e30..02fa631 100644 --- a/roslibrust_transforms/src/lib.rs +++ b/roslibrust_transforms/src/lib.rs @@ -51,10 +51,10 @@ use std::marker::PhantomData; use std::sync::Arc; use std::time::Duration; -use chrono::{DateTime, Utc}; use roslibrust_common::{Publish, RosMessageType, Subscribe, TopicProvider}; use tokio::sync::{broadcast, RwLock}; use tokio_util::sync::CancellationToken; +use transforms::time::TimePoint; /// Error types for TransformManager operations. #[derive(thiserror::Error, Debug)] @@ -69,30 +69,83 @@ pub enum TransformManagerError { Timeout(String, String, String), } +/// Conversion between ROS header timestamps and transform timestamps. +pub trait RosTimestamp: TimePoint { + /// Convert ROS sec/nsec fields into this timestamp type. + fn from_ros_time(sec: i32, nsec: u32) -> Self; + + /// Convert this timestamp type into ROS sec/nsec fields. + fn as_ros_time(self) -> (i32, u32); +} + +impl RosTimestamp for Timestamp { + fn from_ros_time(sec: i32, nsec: u32) -> Self { + Timestamp { + t: (sec as u128) * 1_000_000_000 + (nsec as u128), + } + } + + fn as_ros_time(self) -> (i32, u32) { + let secs = self.t / 1_000_000_000; + let nsecs = self.t % 1_000_000_000; + (secs as i32, nsecs as u32) + } +} + +impl RosTimestamp for std::time::SystemTime { + fn from_ros_time(sec: i32, nsec: u32) -> Self { + use std::time::UNIX_EPOCH; + + if sec >= 0 { + UNIX_EPOCH + Duration::new(sec as u64, nsec) + } else { + UNIX_EPOCH + .checked_sub(Duration::new((-sec) as u64, nsec)) + .unwrap_or(UNIX_EPOCH) + } + } + + fn as_ros_time(self) -> (i32, u32) { + use std::time::UNIX_EPOCH; + + let duration = self.duration_since(UNIX_EPOCH).unwrap_or_default(); + (duration.as_secs() as i32, duration.subsec_nanos()) + } +} + /// Trait for converting a TransformStamped message to a `transforms::Transform`. /// /// This trait abstracts over the differences between ROS1 and ROS2 TransformStamped messages. -pub trait IntoTransform { +pub trait IntoTransform +where + T: TimePoint, +{ /// Convert this message into a `transforms::Transform`. /// - /// If `is_static` is true, the timestamp should be set to `Timestamp::zero()`. - fn into_transform(self, is_static: bool) -> transforms::Transform; + /// If `is_static` is true, the timestamp should be set to the static timestamp value. + fn into_transform(self, is_static: bool) -> transforms::Transform; } /// Trait for converting a `transforms::Transform` to a TransformStamped message. /// /// This trait abstracts over the differences between ROS1 and ROS2 TransformStamped messages. -pub trait FromTransform: Sized { +pub trait FromTransform: Sized +where + T: TimePoint, +{ /// Create a TransformStamped message from a `transforms::Transform`. - fn from_transform(transform: &transforms::Transform) -> Self; + fn from_transform(transform: &transforms::Transform) -> Self; } /// Trait for TFMessage types that contain a list of TransformStamped messages. /// /// This trait abstracts over the differences between ROS1 and ROS2 TFMessage types. -pub trait TFMessageType: RosMessageType + Send + Clone + 'static { +pub trait TFMessageType: RosMessageType + Send + Clone + 'static +where + T: TimePoint, +{ /// The TransformStamped type contained in this TFMessage. - type TransformStamped: IntoTransform + FromTransform + Clone; + type TransformStamped: IntoTransform + FromTransform + Clone; /// Get the transforms from this message. fn transforms(self) -> Vec; @@ -110,8 +163,13 @@ pub trait TFMessageType: RosMessageType + Send + Clone + 'static { /// - `P`: The publisher type (inferred from the TopicProvider used to create the manager) /// /// The manager works with any roslibrust backend (ros1, rosbridge, zenoh, mock). -pub struct TransformManager + Send + Sync> { - registry: Arc>, +pub struct TransformManager +where + M: TFMessageType, + P: Publish + Send + Sync, + T: TimePoint, +{ + registry: Arc>>, buffer_duration: Duration, /// Broadcast channel to notify waiters when transforms are added transform_notify: broadcast::Sender<()>, @@ -122,7 +180,12 @@ pub struct TransformManager + Send + Sync> { _phantom: PhantomData, } -impl + Send + Sync> TransformManager { +impl TransformManager +where + M: TFMessageType, + P: Publish + Send + Sync, + T: TimePoint + Send + Sync + 'static, +{ /// Create a new TransformManager with a custom buffer duration. /// /// Typical usage: @@ -136,16 +199,16 @@ impl + Send + Sync> TransformManager { /// # Ok(()) /// # } /// ``` - pub async fn new( - ros: &T, + pub async fn new( + ros: &R, buffer_duration: Duration, - ) -> Result>, TransformManagerError> + ) -> Result, T>, TransformManagerError> where - T: TopicProvider = P> + Clone + Send + Sync + 'static, - T::Subscriber: Send + 'static, - T::Publisher: Send + Sync, + R: TopicProvider = P> + Clone + Send + Sync + 'static, + R::Subscriber: Send + 'static, + R::Publisher: Send + Sync, { - let registry = Arc::new(RwLock::new(Registry::new(buffer_duration))); + let registry = Arc::new(RwLock::new(Registry::::new(buffer_duration))); // Create broadcast channel for notifying waiters when transforms are added // Capacity of 16 should be plenty - receivers only care about the most recent notification @@ -210,7 +273,7 @@ impl + Send + Sync> TransformManager { /// Background tokio task to process incoming TF messages. async fn process_tf_messages>( mut subscriber: S, - registry: Arc>, + registry: Arc>>, notify: broadcast::Sender<()>, cancel_token: CancellationToken, is_static: bool, @@ -228,8 +291,12 @@ impl + Send + Sync> TransformManager { match result { Ok(msg) => { let mut reg = registry.write().await; - for tf in msg.transforms() { - let transform = tf.into_transform(is_static); + for tf in >::transforms(msg) { + let transform = + >::into_transform( + tf, + is_static, + ); reg.add_transform(transform); } // Notify waiters that transforms have been added @@ -299,23 +366,22 @@ impl + Send + Sync> TransformManager { &self, target_frame: &str, source_frame: &str, - time: Timestamp, - ) -> Result { + time: T, + ) -> Result, TransformManagerError> { let mut registry = self.registry.write().await; registry .get_transform(target_frame, source_frame, time) .map_err(|e| TransformManagerError::LookupError(e.to_string())) } - fn pretty_print_timestamp(time: Timestamp) -> String { - if time == Timestamp::zero() { - return "static (t=0)".to_string(); + fn pretty_print_timestamp(time: T) -> String { + if time.is_static() { + return "static".to_string(); } - let secs = (time.t / 1_000_000_000) as i64; - let nanos = (time.t % 1_000_000_000) as u32; - match DateTime::::from_timestamp(secs, nanos) { - Some(dt) => dt.format("%Y-%m-%d %H:%M:%S%.3f UTC").to_string(), - None => format!("{}.{:09}s", secs, nanos), + + match time.as_seconds() { + Ok(secs) => format!("{secs:.3}s"), + Err(_) => "".to_string(), } } @@ -365,9 +431,9 @@ impl + Send + Sync> TransformManager { &self, target_frame: &str, source_frame: &str, - time: Timestamp, + time: T, timeout: Option, - ) -> Result { + ) -> Result, TransformManagerError> { let timeout_duration = timeout.unwrap_or(self.buffer_duration); let deadline = tokio::time::Instant::now() + timeout_duration; @@ -433,10 +499,11 @@ impl + Send + Sync> TransformManager { /// This publishes the transform to the /tf topic and adds it to the local registry. pub async fn add_transform( &self, - transform: transforms::Transform, + transform: transforms::Transform, ) -> Result<(), TransformManagerError> { - let transform_stamped = M::TransformStamped::from_transform(&transform); - let msg = M::from_transforms(vec![transform_stamped]); + let transform_stamped = + >::from_transform(&transform); + let msg = >::from_transforms(vec![transform_stamped]); // Publish to /tf self.tf_publisher.publish(&msg).await?; @@ -456,20 +523,21 @@ impl + Send + Sync> TransformManager { /// Update (publish and add to registry) a static transform. /// /// This publishes the transform to the /tf_static topic and adds it to the local registry - /// with a timestamp of zero (which the transforms crate treats as a static transform). - /// If the timestamp is not zero, it will be overwritten with zero. + /// with the static timestamp value. + /// If the timestamp is not static, it will be overwritten with the static value. /// /// This function is equivalent to calling [Self::update_transform] with a timestamp of zero, but /// provided as an additional function for clarity. pub async fn update_static_transform( &self, - mut transform: transforms::Transform, + mut transform: transforms::Transform, ) -> Result<(), TransformManagerError> { // Static transforms use timestamp zero - transform.timestamp = Timestamp::zero(); + transform.timestamp = T::static_timestamp(); - let transform_stamped = M::TransformStamped::from_transform(&transform); - let msg = M::from_transforms(vec![transform_stamped]); + let transform_stamped = + >::from_transform(&transform); + let msg = >::from_transforms(vec![transform_stamped]); // Publish to /tf_static self.tf_static_publisher.publish(&msg).await?; @@ -484,9 +552,40 @@ impl + Send + Sync> TransformManager { Ok(()) } + + /// Look up a transform between two frames at different times, using a fixed frame. + /// + /// This matches tf2's "time travel" lookup. The returned transform converts points + /// from `source_frame` at `source_time` into `target_frame` at `target_time`. + /// + /// Note: this function is async to wait for access to registry, but does not wait for the transform to be available. + pub async fn get_transform_at( + &self, + target_frame: &str, + target_time: T, + source_frame: &str, + source_time: T, + fixed_frame: &str, + ) -> Result, TransformManagerError> { + let mut registry = self.registry.write().await; + registry + .get_transform_at( + target_frame, + target_time, + source_frame, + source_time, + fixed_frame, + ) + .map_err(|e| TransformManagerError::LookupError(e.to_string())) + } } -impl + Send + Sync> Drop for TransformManager { +impl Drop for TransformManager +where + M: TFMessageType, + P: Publish + Send + Sync, + T: TimePoint, +{ fn drop(&mut self) { // Cancel the background tasks when the manager is dropped self.cancel_token.cancel(); @@ -503,7 +602,10 @@ pub type Ros1TFMessage = crate::messages::ros1::TFMessage; /// ROS1 TransformStamped type alias for convenience. pub type Ros1TransformStamped = crate::messages::ros1::geometry_msgs::TransformStamped; -impl TFMessageType for Ros1TFMessage { +impl TFMessageType for Ros1TFMessage +where + T: RosTimestamp, +{ type TransformStamped = Ros1TransformStamped; fn transforms(self) -> Vec { @@ -515,14 +617,15 @@ impl TFMessageType for Ros1TFMessage { } } -impl IntoTransform for Ros1TransformStamped { - fn into_transform(self, is_static: bool) -> transforms::Transform { +impl IntoTransform for Ros1TransformStamped +where + T: RosTimestamp, +{ + fn into_transform(self, is_static: bool) -> transforms::Transform { let timestamp = if is_static { - Timestamp::zero() + T::static_timestamp() } else { - let nanoseconds = (self.header.stamp.secs as u128) * 1_000_000_000 - + (self.header.stamp.nsecs as u128); - Timestamp { t: nanoseconds } + T::from_ros_time(self.header.stamp.secs, self.header.stamp.nsecs as u32) }; transforms::Transform { @@ -544,18 +647,18 @@ impl IntoTransform for Ros1TransformStamped { } } -impl FromTransform for Ros1TransformStamped { - fn from_transform(transform: &transforms::Transform) -> Self { +impl FromTransform for Ros1TransformStamped +where + T: RosTimestamp, +{ + fn from_transform(transform: &transforms::Transform) -> Self { use crate::messages::ros1::{geometry_msgs, std_msgs}; - // Year 2038 problem anyone? - let secs = transform.timestamp.t / 1_000_000_000; - let nsecs = transform.timestamp.t % 1_000_000_000; - if secs > i32::MAX as u128 || nsecs > i32::MAX as u128 { + let (secs, nsecs) = transform.timestamp.as_ros_time(); + if nsecs > i32::MAX as u32 { panic!("Timestamp overflow when converting to Ros1TransformStamped"); } - let secs = secs as i32; let nsecs = nsecs as i32; Ros1TransformStamped { @@ -592,7 +695,10 @@ pub type Ros2TFMessage = crate::messages::ros2::TFMessage; /// ROS2 TransformStamped type alias for convenience. pub type Ros2TransformStamped = crate::messages::ros2::geometry_msgs::TransformStamped; -impl TFMessageType for Ros2TFMessage { +impl TFMessageType for Ros2TFMessage +where + T: RosTimestamp, +{ type TransformStamped = Ros2TransformStamped; fn transforms(self) -> Vec { @@ -604,14 +710,15 @@ impl TFMessageType for Ros2TFMessage { } } -impl IntoTransform for Ros2TransformStamped { - fn into_transform(self, is_static: bool) -> transforms::Transform { +impl IntoTransform for Ros2TransformStamped +where + T: RosTimestamp, +{ + fn into_transform(self, is_static: bool) -> transforms::Transform { let timestamp = if is_static { - Timestamp::zero() + T::static_timestamp() } else { - let nanoseconds = (self.header.stamp.sec as u128) * 1_000_000_000 - + (self.header.stamp.nanosec as u128); - Timestamp { t: nanoseconds } + T::from_ros_time(self.header.stamp.sec, self.header.stamp.nanosec) }; transforms::Transform { @@ -633,18 +740,14 @@ impl IntoTransform for Ros2TransformStamped { } } -impl FromTransform for Ros2TransformStamped { - fn from_transform(transform: &transforms::Transform) -> Self { +impl FromTransform for Ros2TransformStamped +where + T: RosTimestamp, +{ + fn from_transform(transform: &transforms::Transform) -> Self { use crate::messages::ros2::{builtin_interfaces, geometry_msgs, std_msgs}; - // Year 2038 problem anyone? - let sec = transform.timestamp.t / 1_000_000_000; - let nanosec = transform.timestamp.t % 1_000_000_000; - if sec > i32::MAX as u128 || nanosec > u32::MAX as u128 { - panic!("Timestamp overflow when converting to Ros2TransformStamped"); - } - let sec = sec as i32; - let nanosec = nanosec as u32; + let (sec, nanosec) = transform.timestamp.as_ros_time(); Ros2TransformStamped { header: std_msgs::Header { diff --git a/roslibrust_transforms/tests/mocked_tests.rs b/roslibrust_transforms/tests/mocked_tests.rs index e2a8146..9bce355 100644 --- a/roslibrust_transforms/tests/mocked_tests.rs +++ b/roslibrust_transforms/tests/mocked_tests.rs @@ -4,9 +4,76 @@ use std::time::Duration; use roslibrust_common::{Publish, TopicProvider}; use roslibrust_mock::MockRos; +use transforms::time::{TimeError, TimePoint}; use roslibrust_transforms::messages::ros1::{geometry_msgs, std_msgs, TFMessage}; -use roslibrust_transforms::{Ros1TFMessage, Timestamp, TransformManager}; +use roslibrust_transforms::{ + Quaternion, Ros1TFMessage, RosTimestamp, Timestamp, TransformManager, Vector3, +}; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +struct MockTimestamp { + t: u128, +} + +impl TimePoint for MockTimestamp { + fn static_timestamp() -> Self { + Self { t: 0 } + } + + fn duration_since(self, earlier: Self) -> Result { + if self.t < earlier.t { + return Err(TimeError::DurationUnderflow); + } + + let diff = self.t - earlier.t; + let secs = diff / 1_000_000_000; + let nanos = diff % 1_000_000_000; + if secs > u64::MAX as u128 { + return Err(TimeError::DurationOverflow); + } + + Ok(Duration::new(secs as u64, nanos as u32)) + } + + fn checked_add(self, rhs: Duration) -> Result { + let rhs_nanos = rhs.as_nanos(); + self.t + .checked_add(rhs_nanos) + .map(|t| Self { t }) + .ok_or(TimeError::DurationOverflow) + } + + fn checked_sub(self, rhs: Duration) -> Result { + let rhs_nanos = rhs.as_nanos(); + self.t + .checked_sub(rhs_nanos) + .map(|t| Self { t }) + .ok_or(TimeError::DurationUnderflow) + } + + fn as_seconds(self) -> Result { + Ok(self.t as f64 / 1_000_000_000.0) + } +} + +impl RosTimestamp for MockTimestamp { + fn from_ros_time(sec: i32, nsec: u32) -> Self { + Self { + t: (sec as u128) * 1_000_000_000 + (nsec as u128), + } + } + + fn as_ros_time(self) -> (i32, u32) { + let secs = self.t / 1_000_000_000; + let nsecs = self.t % 1_000_000_000; + if secs > i32::MAX as u128 || nsecs > u32::MAX as u128 { + panic!("Timestamp overflow when converting to ROS time"); + } + + (secs as i32, nsecs as u32) + } +} /// Helper function to create a TFMessage with a single transform. fn create_tf_message( @@ -39,18 +106,74 @@ fn create_tf_message( } } -#[tokio::test(flavor = "multi_thread")] -async fn test_transform_listener_creation() { +#[tokio::test] +async fn test_transform_listener_with_custom_timestamp() { + tokio::time::pause(); let mock_ros = MockRos::new(); - let manager = - TransformManager::::new(&mock_ros, std::time::Duration::from_secs(10)) - .await; - assert!(manager.is_ok(), "Failed to create TransformManager"); + // Create a publisher for /tf topic + let tf_publisher = mock_ros + .advertise::("/tf") + .await + .expect("Failed to create /tf publisher"); + + // Create the manager with a custom timestamp type + let manager = TransformManager::::new( + &mock_ros, + std::time::Duration::from_secs(10), + ) + .await + .expect("Failed to create TransformManager"); + + // Give the listener time to subscribe + tokio::time::sleep(Duration::from_millis(50)).await; + + // Publish a transform and verify conversion into custom timestamp type + let tf_msg = create_tf_message("world", "custom_frame", 1.0, 2.0, 3.0, 3, 0); + tf_publisher + .publish(&tf_msg) + .await + .expect("Failed to publish transform"); + + // Give the listener time to process the message + tokio::time::sleep(Duration::from_millis(100)).await; + + let lookup_time = MockTimestamp { t: 3_000_000_000 }; + let transform = manager + .get_transform("world", "custom_frame", lookup_time) + .await + .expect("Failed to look up transform with custom timestamp"); + assert_eq!(transform.timestamp, lookup_time); + + // Add another transform through the manager and verify conversion from custom timestamp + let transform = transforms::Transform { + parent: "world".to_string(), + child: "custom_from_manager".to_string(), + translation: Vector3::new(4.0, 5.0, 6.0), + rotation: Quaternion::identity(), + timestamp: MockTimestamp { t: 4_000_000_000 }, + }; + manager + .add_transform(transform) + .await + .expect("Failed to add transform with custom timestamp"); + + let retrieved = manager + .get_transform( + "world", + "custom_from_manager", + MockTimestamp { t: 4_000_000_000 }, + ) + .await + .expect("Failed to retrieve transform added with custom timestamp"); + assert!((retrieved.translation.x - 4.0).abs() < 1e-6); + assert!((retrieved.translation.y - 5.0).abs() < 1e-6); + assert!((retrieved.translation.z - 6.0).abs() < 1e-6); } -#[tokio::test(flavor = "multi_thread")] +#[tokio::test] async fn test_transform_listener_receives_tf_messages() { + tokio::time::pause(); use crate::Timestamp; let mock_ros = MockRos::new(); @@ -101,8 +224,9 @@ async fn test_transform_listener_receives_tf_messages() { ); } -#[tokio::test(flavor = "multi_thread")] +#[tokio::test] async fn test_transform_listener_static_transforms() { + tokio::time::pause(); let mock_ros = MockRos::new(); // Create a publisher for /tf_static topic @@ -141,8 +265,9 @@ async fn test_transform_listener_static_transforms() { ); } -#[tokio::test(flavor = "multi_thread")] +#[tokio::test] async fn test_lookup_transform_values() { + tokio::time::pause(); let mock_ros = MockRos::new(); // Create a publisher for /tf_static topic @@ -218,8 +343,9 @@ async fn test_lookup_transform_values() { ); } -#[tokio::test(flavor = "multi_thread")] +#[tokio::test] async fn test_wait_for_transform_success() { + tokio::time::pause(); let mock_ros = MockRos::new(); // Create the manager @@ -248,7 +374,7 @@ async fn test_wait_for_transform_success() { }); // Wait for the transform - it should succeed after the delayed publish - let start = std::time::Instant::now(); + let start = tokio::time::Instant::now(); let result = manager .wait_for_transform( "world", @@ -277,8 +403,9 @@ async fn test_wait_for_transform_success() { assert!((transform.translation.z - 3.0).abs() < 1e-6); } -#[tokio::test(flavor = "multi_thread")] +#[tokio::test] async fn test_wait_for_transform_timeout() { + tokio::time::pause(); use roslibrust_transforms::TransformManagerError; let mock_ros = MockRos::new(); @@ -290,7 +417,7 @@ async fn test_wait_for_transform_timeout() { .expect("Failed to create TransformManager"); // Wait for a transform that will never be published - let start = std::time::Instant::now(); + let start = tokio::time::Instant::now(); let result = manager .wait_for_transform( "nonexistent_parent", @@ -322,8 +449,9 @@ async fn test_wait_for_transform_timeout() { } } -#[tokio::test(flavor = "multi_thread")] +#[tokio::test] async fn test_wait_for_transform_immediate_success() { + tokio::time::pause(); let mock_ros = MockRos::new(); // Create a publisher for /tf_static topic @@ -371,8 +499,9 @@ async fn test_wait_for_transform_immediate_success() { ); } -#[tokio::test(flavor = "multi_thread")] +#[tokio::test] async fn test_wait_for_transform_default_timeout() { + tokio::time::pause(); let mock_ros = MockRos::new(); // Create the manager with a short buffer duration (used as default timeout) @@ -382,7 +511,7 @@ async fn test_wait_for_transform_default_timeout() { .expect("Failed to create TransformManager"); // Wait for a transform that will never be published, using default timeout (None) - let start = std::time::Instant::now(); + let start = tokio::time::Instant::now(); let result = manager .wait_for_transform("missing_parent", "missing_child", Timestamp::zero(), None) .await; @@ -401,3 +530,78 @@ async fn test_wait_for_transform_default_timeout() { "Should not have waited much longer than the buffer duration" ); } + +#[tokio::test] +async fn test_get_transform_at_different_times() { + tokio::time::pause(); + let mock_ros = MockRos::new(); + + // Create a publisher for /tf topic + let tf_publisher = mock_ros + .advertise::("/tf") + .await + .expect("Failed to create /tf publisher"); + + // Create the manager + let manager = + TransformManager::::new(&mock_ros, std::time::Duration::from_secs(10)) + .await + .expect("Failed to create TransformManager"); + + // Give the listener time to subscribe + tokio::time::sleep(Duration::from_millis(50)).await; + + // fixed -> a at t=1s + let fixed_to_a_t1 = create_tf_message("fixed", "a", 1.0, 0.0, 0.0, 1, 0); + tf_publisher + .publish(&fixed_to_a_t1) + .await + .expect("Failed to publish fixed->a at t=1s"); + + // fixed -> a at t=2s + let fixed_to_a_t2 = create_tf_message("fixed", "a", 2.0, 0.0, 0.0, 2, 0); + tf_publisher + .publish(&fixed_to_a_t2) + .await + .expect("Failed to publish fixed->a at t=2s"); + + // a -> b at t=1s + let a_to_b_t1 = create_tf_message("a", "b", 0.0, 1.0, 0.0, 1, 0); + tf_publisher + .publish(&a_to_b_t1) + .await + .expect("Failed to publish a->b at t=1s"); + + // Give the listener time to process the messages + tokio::time::sleep(Duration::from_millis(100)).await; + + let t1 = Timestamp { t: 1_000_000_000 }; + let t2 = Timestamp { t: 2_000_000_000 }; + + let transform = manager + .get_transform_at("a", t2, "b", t1, "fixed") + .await + .expect("Failed to look up transform at different times"); + + assert_eq!(transform.parent, "a"); + assert_eq!(transform.child, "b"); + assert_eq!(transform.timestamp, t2); + + // b at t=1s in fixed is (1, 1, 0), while a at t=2s in fixed is (2, 0, 0) + // so b at t=1s expressed in a at t=2s is (-1, 1, 0) + assert!( + (transform.translation.x + 1.0).abs() < 1e-6, + "Expected x=-1.0, got {}", + transform.translation.x + ); + assert!( + (transform.translation.y - 1.0).abs() < 1e-6, + "Expected y=1.0, got {}", + transform.translation.y + ); + assert!( + transform.translation.z.abs() < 1e-6, + "Expected z=0.0, got {}", + transform.translation.z + ); +}