diff --git a/CHANGELOG.md b/CHANGELOG.md index fb3198c..31e6a45 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,12 +9,16 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +- roslibrust_transforms gained `latest_common_time()` for looking up the newest time a transform can be served, and `remove_frame()` for removing a frame from the local buffer (e.g. to allow re-parenting it). + ### Fixed - @JesseGuillory-CM removed several panics and poor error handling from rosbridge client. ### Changed +- Upgraded roslibrust_transforms to transforms v2.1. Static transforms are now represented by `Stamp::Static` instead of a zero timestamp, transforms are built with `Transform::new` / `Transform::static_between` instead of struct literals, and `add_transform()` publishes static transforms to /tf_static automatically, replacing `update_static_transform()`. Invalid or conflicting transforms received over the wire are now dropped with a warning instead of silently corrupting the buffer. + ## 0.21.0 - May 19th, 2026 ### Added diff --git a/Cargo.lock b/Cargo.lock index 63decad..1b0a5ab 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1524,6 +1524,17 @@ dependencies = [ "foldhash 0.2.0", ] +[[package]] +name = "hashbrown" +version = "0.17.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +dependencies = [ + "allocator-api2", + "equivalent", + "foldhash 0.2.0", +] + [[package]] name = "heck" version = "0.5.0" @@ -3673,7 +3684,6 @@ dependencies = [ name = "roslibrust_transforms" version = "0.1.0" dependencies = [ - "chrono", "diffy", "env_logger 0.11.8", "log", @@ -5025,12 +5035,14 @@ dependencies = [ [[package]] name = "transforms" -version = "1.2.0" +version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f6319e43042a9870592131406e2018fb88bb8114b282b98f99bb7f5922f03386" +checksum = "e5eaf21707966bfcbfbb0e31bfd2ba076179dadf87c7c623893320377980976c" dependencies = [ "approx", - "hashbrown 0.16.1", + "hashbrown 0.17.1", + "libm", + "serde", "thiserror 2.0.18", ] @@ -5455,7 +5467,7 @@ version = "0.1.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c2a7b1c03c876122aa43f3020e6c3c3ee5c05081c9a00739faf7503aeba10d22" dependencies = [ - "windows-sys 0.48.0", + "windows-sys 0.61.2", ] [[package]] diff --git a/roslibrust_transforms/Cargo.toml b/roslibrust_transforms/Cargo.toml index 7ce15a3..55927fc 100644 --- a/roslibrust_transforms/Cargo.toml +++ b/roslibrust_transforms/Cargo.toml @@ -13,13 +13,12 @@ keywords = ["ROS", "robotics", "tf2", "transforms", "coordinates"] roslibrust_common = { path = "../roslibrust_common", version = "0.21" } # roslibrust is needed because the macro-generated code references ::roslibrust:: roslibrust = { path = "../roslibrust", version = "0.21", features = ["macro", "codegen"] } -transforms = "1.2" +transforms = "2.1" tokio = { workspace = true } log = { workspace = true } thiserror = "2.0" serde = { workspace = true } tokio-util = "0.7" -chrono = "0.4" [dev-dependencies] env_logger = "0.11" diff --git a/roslibrust_transforms/README.md b/roslibrust_transforms/README.md index ceff211..234a175 100644 --- a/roslibrust_transforms/README.md +++ b/roslibrust_transforms/README.md @@ -21,17 +21,17 @@ roslibrust_transforms = "0.1" ### Basic Example ```rust -use roslibrust_transforms::{TransformManager, Ros1TFMessage}; +use roslibrust_transforms::{TransformManager, Ros1TFMessage, Timestamp}; async fn example(ros: impl roslibrust_common::TopicProvider + Clone + Send + Sync + 'static) { // Create a TransformManager (subscribes to /tf and /tf_static automatically) - let manager = TransformManager::::new(&ros).await.unwrap(); + let manager = TransformManager::::new(&ros, std::time::Duration::from_secs(10)).await.unwrap(); // Look up a transform - let transform = manager.lookup_latest_transform("base_link", "camera_link").await.unwrap(); + let transform = manager.get_transform("base_link", "camera_link", Timestamp::now()).await.unwrap(); - println!("Translation: {:?}", transform.translation); - println!("Rotation: {:?}", transform.rotation); + println!("Translation: {:?}", transform.translation()); + println!("Rotation: {:?}", transform.rotation()); } ``` @@ -41,38 +41,37 @@ The only difference is the message type parameter: ```rust // ROS1 -let manager = TransformManager::::new(&ros).await?; +let manager = TransformManager::::new(&ros, std::time::Duration::from_secs(10)).await?; // ROS2 -let manager = TransformManager::::new(&ros).await?; +let manager = TransformManager::::new(&ros, std::time::Duration::from_secs(10)).await?; ``` ### Publishing Transforms ```rust -use roslibrust_transforms::{TransformManager, Ros1TFMessage, Transform, Timestamp}; +use roslibrust_transforms::{TransformManager, Ros1TFMessage, Stamp, Timestamp, Transform, Quaternion, Vector3}; async fn broadcast_example(ros: impl roslibrust_common::TopicProvider + Clone + Send + Sync + 'static) { - let manager = TransformManager::::new(&ros).await.unwrap(); - - // Create and publish a dynamic transform - let transform = Transform { - parent_frame: "world".to_string(), - child_frame: "robot".to_string(), - translation: Default::default(), - rotation: Default::default(), - timestamp: Timestamp::now(), - }; - manager.update_transform(transform).await.unwrap(); - - // Or publish a static transform - let static_tf = Transform { - parent_frame: "robot".to_string(), - child_frame: "sensor".to_string(), - translation: Default::default(), - rotation: Default::default(), - timestamp: Timestamp::now(), // Will be set to zero internally - }; - manager.update_static_transform(static_tf).await.unwrap(); + let manager = TransformManager::::new(&ros, std::time::Duration::from_secs(10)).await.unwrap(); + + // Create and publish a dynamic transform, published on /tf + let transform = Transform::new( + "world", + "robot", + Vector3::new(1.0, 0.0, 0.0), + Quaternion::identity(), + Stamp::At(Timestamp::now()), + ).unwrap(); + manager.add_transform(transform).await.unwrap(); + + // Static transforms are valid for all time, and are published on /tf_static + let static_tf = Transform::static_between( + "robot", + "sensor", + Vector3::new(0.1, 0.0, 0.5), + Quaternion::identity(), + ).unwrap(); + manager.add_transform(static_tf).await.unwrap(); } ``` diff --git a/roslibrust_transforms/examples/ros1.rs b/roslibrust_transforms/examples/ros1.rs index aaded0f..9b45b86 100644 --- a/roslibrust_transforms/examples/ros1.rs +++ b/roslibrust_transforms/examples/ros1.rs @@ -22,7 +22,7 @@ use std::time::Duration; -use roslibrust_transforms::{Ros1TFMessage, Timestamp, TransformManager}; +use roslibrust_transforms::{Ros1TFMessage, Stamp, Timestamp, TransformManager}; use log::*; @@ -57,11 +57,12 @@ async fn main() -> Result<(), Box> { // Try to look up a transform from "world" to "base_link" match manager.get_transform("world", "base_link", Timestamp::now()).await { Ok(transform) => { + let translation = transform.translation(); info!( "Transform world -> base_link: translation=({:.3}, {:.3}, {:.3})", - transform.translation.x, - transform.translation.y, - transform.translation.z + translation.x, + translation.y, + translation.z ); } Err(e) => { @@ -69,18 +70,26 @@ async fn main() -> Result<(), Box> { } } - // Also try looking up with a specific timestamp (for static transforms, use zero) - match manager.get_transform("world", "base_link", Timestamp::zero()).await { - Ok(transform) => { - info!( - "Static transform world -> base_link: translation=({:.3}, {:.3}, {:.3})", - transform.translation.x, - transform.translation.y, - transform.translation.z - ); - } - Err(_) => { - // Static transform not available yet + // Also look up the newest transform the local buffer can serve + if let Ok(latest) = manager.latest_common_time("world", "base_link").await { + let time = match latest { + Stamp::At(time) => time, + // Frames connected by static transforms only can be looked up at any time + Stamp::Static => Timestamp::now(), + }; + match manager.get_transform("world", "base_link", time).await { + Ok(transform) => { + let translation = transform.translation(); + info!( + "Latest transform world -> base_link: translation=({:.3}, {:.3}, {:.3})", + translation.x, + translation.y, + translation.z + ); + } + Err(e) => { + warn!("Could not look up latest transform: {}", e); + } } } } diff --git a/roslibrust_transforms/examples/ros2_rosbridge.rs b/roslibrust_transforms/examples/ros2_rosbridge.rs index 12a02f0..d1fa57d 100644 --- a/roslibrust_transforms/examples/ros2_rosbridge.rs +++ b/roslibrust_transforms/examples/ros2_rosbridge.rs @@ -23,7 +23,7 @@ use std::time::Duration; use roslibrust_rosbridge::ClientHandle; -use roslibrust_transforms::{Ros2TFMessage, Timestamp, TransformManager}; +use roslibrust_transforms::{Ros2TFMessage, Stamp, Timestamp, TransformManager}; #[tokio::main] async fn main() -> Result<(), Box> { @@ -55,11 +55,12 @@ async fn main() -> Result<(), Box> { // Try to look up a transform from "world" to "base_link" match manager.get_transform("world", "base_link", Timestamp::now()).await { Ok(transform) => { + let translation = transform.translation(); log::info!( "Transform world -> base_link: translation=({:.3}, {:.3}, {:.3})", - transform.translation.x, - transform.translation.y, - transform.translation.z + translation.x, + translation.y, + translation.z ); } Err(e) => { @@ -67,18 +68,26 @@ async fn main() -> Result<(), Box> { } } - // Also try looking up with a specific timestamp (for static transforms, use zero) - match manager.get_transform("world", "base_link", Timestamp::zero()).await { - Ok(transform) => { - log::info!( - "Static transform world -> base_link: translation=({:.3}, {:.3}, {:.3})", - transform.translation.x, - transform.translation.y, - transform.translation.z - ); - } - Err(_) => { - // Static transform not available yet + // Also look up the newest transform the local buffer can serve + if let Ok(latest) = manager.latest_common_time("world", "base_link").await { + let time = match latest { + Stamp::At(time) => time, + // Frames connected by static transforms only can be looked up at any time + Stamp::Static => Timestamp::now(), + }; + match manager.get_transform("world", "base_link", time).await { + Ok(transform) => { + let translation = transform.translation(); + log::info!( + "Latest transform world -> base_link: translation=({:.3}, {:.3}, {:.3})", + translation.x, + translation.y, + translation.z + ); + } + Err(e) => { + log::warn!("Could not look up latest transform: {}", e); + } } } } diff --git a/roslibrust_transforms/examples/shared_manager.rs b/roslibrust_transforms/examples/shared_manager.rs index 2859f35..52e71b2 100644 --- a/roslibrust_transforms/examples/shared_manager.rs +++ b/roslibrust_transforms/examples/shared_manager.rs @@ -10,7 +10,7 @@ use log::*; // Example uses the mock backend for simplicity in running / testing, but any backend can be used use roslibrust::mock::MockRos; use roslibrust_transforms::{ - Quaternion, Ros1TFMessage, Timestamp, Transform, TransformManager, Vector3, + Quaternion, Ros1TFMessage, Stamp, Timestamp, Transform, TransformManager, Vector3, }; #[tokio::main] @@ -34,16 +34,15 @@ async fn main() -> Result<(), Box> { info!("Publishing transform..."); // Because of internal mutability, add_transform() doesn't require `&mut self` // So you don't need to use Mutex or RefCell to share the manager - tf_mgr2 - .add_transform(Transform { - parent: "world".to_string(), - child: "robot".to_string(), - translation: Vector3::new(x, 0.0, 0.0), - rotation: Quaternion::identity(), - timestamp: Timestamp::now(), - }) - .await - .unwrap(); + let transform = Transform::new( + "world", + "robot", + Vector3::new(x, 0.0, 0.0), + Quaternion::identity(), + Stamp::At(Timestamp::now()), + ) + .unwrap(); + tf_mgr2.add_transform(transform).await.unwrap(); x += 1.0; } }); @@ -65,10 +64,11 @@ async fn main() -> Result<(), Box> { xform = tf_mgr.wait_for_transform("world", "robot", lookup, None) => { match xform { Ok(xform) => { + let translation = xform.translation(); info!("Transform: translation=({:.3}, {:.3}, {:.3})", - xform.translation.x, - xform.translation.y, - xform.translation.z + translation.x, + translation.y, + translation.z ); } Err(e) => { diff --git a/roslibrust_transforms/src/lib.rs b/roslibrust_transforms/src/lib.rs index 02fa631..b0fa0b7 100644 --- a/roslibrust_transforms/src/lib.rs +++ b/roslibrust_transforms/src/lib.rs @@ -8,19 +8,19 @@ //! - Generic over roslibrust backends (ros1, rosbridge, zenoh, mock) //! - Supports both ROS1 and ROS2 message formats //! - Automatic subscription to `/tf` and `/tf_static` topics -//! - Ability to publish transforms via `update_transform()` and `update_static_transform()` +//! - Ability to publish transforms via `add_transform()` //! //! # ROS1 vs ROS2 //! //! The `TransformManager` is generic over the message type. Use the appropriate type alias //! for your ROS version: //! -//! - ROS1: `TransformManager::::new(&ros)` -//! - ROS2: `TransformManager::::new(&ros)` +//! - ROS1: `TransformManager::::new(&ros, buffer_duration)` +//! - ROS2: `TransformManager::::new(&ros, buffer_duration)` //! //! # Example //! ```no_run -//! use roslibrust_transforms::{TransformManager, Ros1TFMessage, Timestamp}; +//! use roslibrust_transforms::{Quaternion, Ros1TFMessage, Stamp, Timestamp, Transform, TransformManager, Vector3}; //! use roslibrust::traits::Ros; //! //! // Generic over any roslibrust backend @@ -29,32 +29,38 @@ //! let manager = TransformManager::::new(&ros, std::time::Duration::from_secs(10)).await.unwrap(); //! //! // Look up a transform -//! let mut transform = manager.get_transform("base_link", "camera_link", Timestamp::now()).await.unwrap(); +//! let transform = manager.get_transform("base_link", "camera_link", Timestamp::now()).await.unwrap(); //! -//! // Modify the transform -//! transform.translation.x += 1.0; -//! transform.timestamp = transforms::time::Timestamp::now(); +//! // Build an updated transform from its components +//! let updated = Transform::new( +//! transform.parent(), +//! transform.child(), +//! transform.translation() + Vector3::new(1.0, 0.0, 0.0), +//! transform.rotation(), +//! Stamp::At(Timestamp::now()), +//! ) +//! .unwrap(); //! //! // Update the value in the buffer, and publish it's update to other nodes -//! manager.add_transform(transform).await.unwrap(); +//! manager.add_transform(updated).await.unwrap(); //! } //! ``` pub mod messages; // Re-export useful types from the transforms crate -pub use transforms::geometry::{Quaternion, Transform, Vector3}; -pub use transforms::time::Timestamp; -pub use transforms::Registry; +pub use transforms::errors::TransformError; +pub use transforms::geometry::{Quaternion, Vector3}; +pub use transforms::time::{Stamp, TimeError, TimePoint, Timestamp}; +pub use transforms::{Registry, Transform}; use std::marker::PhantomData; use std::sync::Arc; use std::time::Duration; use roslibrust_common::{Publish, RosMessageType, Subscribe, TopicProvider}; -use tokio::sync::{broadcast, RwLock}; +use tokio::sync::{watch, RwLock}; use tokio_util::sync::CancellationToken; -use transforms::time::TimePoint; /// Error types for TransformManager operations. #[derive(thiserror::Error, Debug)] @@ -62,6 +68,9 @@ pub enum TransformManagerError { #[error("Transform lookup failed: {0}")] LookupError(String), + #[error("Transform rejected by the registry: {0}")] + RejectedTransform(String), + #[error("ROS communication error: {0}")] RosError(#[from] roslibrust_common::Error), @@ -80,14 +89,18 @@ pub trait RosTimestamp: TimePoint { 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), + // Timestamp stores u64 nanoseconds since the unix epoch, so times before the epoch + // have no representation, clamp them to zero + match u64::try_from(sec) { + Ok(sec) => Timestamp::from_nanos(sec * 1_000_000_000 + (nsec as u64)), + Err(_) => Timestamp::zero(), } } fn as_ros_time(self) -> (i32, u32) { - let secs = self.t / 1_000_000_000; - let nsecs = self.t % 1_000_000_000; + let nanos = self.as_nanos(); + let secs = nanos / 1_000_000_000; + let nsecs = nanos % 1_000_000_000; (secs as i32, nsecs as u32) } } @@ -122,8 +135,12 @@ where { /// Convert this message into a `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; + /// If `is_static` is true, the message's timestamp is ignored and the resulting transform + /// carries `Stamp::Static`, making it valid for all time. + /// + /// Returns an error if the message does not describe a valid transform, e.g. its rotation + /// is not a unit quaternion or one of its components is NaN or infinite. + fn into_transform(self, is_static: bool) -> Result, TransformError>; } /// Trait for converting a `transforms::Transform` to a TransformStamped message. @@ -134,6 +151,8 @@ where T: TimePoint, { /// Create a TransformStamped message from a `transforms::Transform`. + /// + /// Static transforms carry no instant and are stamped with time zero in the resulting message. fn from_transform(transform: &transforms::Transform) -> Self; } @@ -171,8 +190,8 @@ where { registry: Arc>>, buffer_duration: Duration, - /// Broadcast channel to notify waiters when transforms are added - transform_notify: broadcast::Sender<()>, + /// Watch channel to notify waiters when transforms are added + transform_notify: watch::Sender<()>, /// Cancellation token to shut down background tasks when dropped cancel_token: CancellationToken, tf_publisher: P, @@ -208,11 +227,11 @@ where 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::::with_max_age(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 - let (transform_notify, _) = broadcast::channel(16); + // Create watch channel for notifying waiters when transforms are added + // Notifications coalesce - receivers only care that something changed since they last checked + let (transform_notify, _) = watch::channel(()); // Create cancellation token for shutting down background tasks let cancel_token = CancellationToken::new(); @@ -274,17 +293,15 @@ where async fn process_tf_messages>( mut subscriber: S, registry: Arc>>, - notify: broadcast::Sender<()>, + notify: watch::Sender<()>, cancel_token: CancellationToken, is_static: bool, ) { + let topic = if is_static { "/tf_static" } else { "/tf" }; loop { tokio::select! { _ = cancel_token.cancelled() => { - log::debug!( - "Shutting down {} listener task", - if is_static { "/tf_static" } else { "/tf" } - ); + log::debug!("Shutting down {topic} listener task"); break; } result = subscriber.next() => { @@ -293,22 +310,33 @@ where let mut reg = registry.write().await; for tf in >::transforms(msg) { let transform = - >::into_transform( - tf, - is_static, + match >::into_transform( + tf, is_static, + ) { + Ok(transform) => transform, + Err(e) => { + log::warn!( + "Dropping invalid transform received on {topic}: {e}" + ); + continue; + } + }; + // Clone the frame names so the warning below can still name them + // after the transform is moved into the registry + let parent = transform.parent().to_owned(); + let child = transform.child().to_owned(); + if let Err(e) = reg.add_transform(transform) { + log::warn!( + "Dropping transform from '{parent}' to '{child}' rejected by the registry on {topic}: {e}" ); - reg.add_transform(transform); + } } // Notify waiters that transforms have been added // Ignore errors - they just mean no one is currently listening let _ = notify.send(()); } Err(e) => { - log::warn!( - "Error receiving {} message: {}", - if is_static { "/tf_static" } else { "/tf" }, - e - ); + log::warn!("Error receiving {topic} message: {e}"); // Continue trying to receive messages } } @@ -324,7 +352,7 @@ where /// /// Example: /// ``` - /// use roslibrust_transforms::{TransformManager, Ros1TFMessage}; + /// use roslibrust_transforms::{Quaternion, Ros1TFMessage, Stamp, Timestamp, Transform, TransformManager, Vector3}; /// use roslibrust::traits::Ros; /// #[tokio::main] /// async fn main() -> Result<(), Box> { @@ -334,24 +362,24 @@ where /// /// // Camera has moved between t=0 and t=5 /// // These updates would be automatically received over the /tf topic if something was publishing them - /// let t0 = roslibrust_transforms::Timestamp::now(); - /// let x0 = transforms::Transform { - /// parent: "base_link".to_string(), - /// child: "camera_link".to_string(), - /// translation: roslibrust_transforms::Vector3::new(0.0, 0.0, 0.0), - /// rotation: roslibrust_transforms::Quaternion::identity(), - /// timestamp: t0, - /// }; + /// let t0 = Timestamp::now(); + /// let x0 = Transform::new( + /// "base_link", + /// "camera_link", + /// Vector3::new(0.0, 0.0, 0.0), + /// Quaternion::identity(), + /// Stamp::At(t0), + /// )?; /// manager.add_transform(x0).await?; /// /// let t5 = (t0 + std::time::Duration::from_secs(5)).unwrap(); - /// let x5 = transforms::Transform { - /// parent: "base_link".to_string(), - /// child: "camera_link".to_string(), - /// translation: roslibrust_transforms::Vector3::new(1.0, 0.0, 0.0), - /// rotation: roslibrust_transforms::Quaternion::identity(), - /// timestamp: t5, - /// }; + /// let x5 = Transform::new( + /// "base_link", + /// "camera_link", + /// Vector3::new(1.0, 0.0, 0.0), + /// Quaternion::identity(), + /// Stamp::At(t5), + /// )?; /// manager.add_transform(x5).await?; /// /// // We care to know where the camera was at t=3 @@ -359,7 +387,7 @@ where /// let transform = manager.get_transform("base_link", "camera_link", t3).await?; /// /// // Linear interpolation was performed behind the scenes to get the transform at t=3 - /// assert_eq!(transform.translation.x, 0.6); + /// assert_eq!(transform.translation().x, 0.6); /// Ok(()) /// } pub async fn get_transform( @@ -368,21 +396,14 @@ where source_frame: &str, time: T, ) -> Result, TransformManagerError> { - let mut registry = self.registry.write().await; + let registry = self.registry.read().await; registry .get_transform(target_frame, source_frame, time) .map_err(|e| TransformManagerError::LookupError(e.to_string())) } fn pretty_print_timestamp(time: T) -> String { - if time.is_static() { - return "static".to_string(); - } - - match time.as_seconds() { - Ok(secs) => format!("{secs:.3}s"), - Err(_) => "".to_string(), - } + format!("{:.3}s", time.as_seconds_lossy()) } /// Wait for a transform to become available between two frames at a specific time. @@ -443,7 +464,7 @@ where loop { // Try to get the transform { - let mut registry = self.registry.write().await; + let registry = self.registry.read().await; if let Ok(transform) = registry.get_transform(target_frame, source_frame, time) { return Ok(transform); } @@ -463,7 +484,7 @@ where tokio::select! { _ = tokio::time::sleep(remaining) => { // Timeout expired - do one final check then return error - let mut registry = self.registry.write().await; + let registry = self.registry.read().await; if let Ok(transform) = registry.get_transform(target_frame, source_frame, time) { return Ok(transform); } @@ -473,30 +494,27 @@ where Self::pretty_print_timestamp(time), )); } - result = receiver.recv() => { + result = receiver.changed() => { // Got a notification - check for the transform on next loop iteration - // Handle lagged receivers by just continuing - we'll check the registry anyway - match result { - Ok(()) | Err(broadcast::error::RecvError::Lagged(_)) => { - // Continue to next iteration to check for transform - } - Err(broadcast::error::RecvError::Closed) => { - // Channel closed, shouldn't happen but treat as timeout - return Err(TransformManagerError::Timeout( - target_frame.to_string(), - source_frame.to_string(), - Self::pretty_print_timestamp(time), - )); - } + // Notifications coalesce, so a burst of transforms only triggers one check + if result.is_err() { + // Channel closed, shouldn't happen but treat as timeout + return Err(TransformManagerError::Timeout( + target_frame.to_string(), + source_frame.to_string(), + Self::pretty_print_timestamp(time), + )); } } } } } - /// Update (publish and add to registry) a dynamic transform. + /// Update (publish and add to registry) a transform. /// - /// This publishes the transform to the /tf topic and adds it to the local registry. + /// The transform's stamp picks the topic: dynamic transforms (`Stamp::At`, built with + /// [Transform::new]) are published to the /tf topic, static transforms (`Stamp::Static`, + /// built with [Transform::static_between]) are published to the /tf_static topic. pub async fn add_transform( &self, transform: transforms::Transform, @@ -504,51 +522,26 @@ where let transform_stamped = >::from_transform(&transform); let msg = >::from_transforms(vec![transform_stamped]); + let is_static = transform.timestamp().is_static(); - // Publish to /tf - self.tf_publisher.publish(&msg).await?; - - // Update registry + // Update the registry first so that a transform it rejects (e.g. one that would + // re-parent a frame) is never published { let mut registry = self.registry.write().await; - registry.add_transform(transform); + registry + .add_transform(transform) + .map_err(|e| TransformManagerError::RejectedTransform(e.to_string()))?; } - // Notify waiters that a transform has been added + // Notify waiters that a transform has been added, even if the publish below fails let _ = self.transform_notify.send(()); - Ok(()) - } - - /// 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 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, - ) -> Result<(), TransformManagerError> { - // Static transforms use timestamp zero - transform.timestamp = T::static_timestamp(); - - let transform_stamped = - >::from_transform(&transform); - let msg = >::from_transforms(vec![transform_stamped]); - - // Publish to /tf_static - self.tf_static_publisher.publish(&msg).await?; - - // Update registry - let mut registry = self.registry.write().await; - registry.add_transform(transform); - drop(registry); - - // Notify waiters that a transform has been added - let _ = self.transform_notify.send(()); + // Publish to the topic matching the transform's kind + if is_static { + self.tf_static_publisher.publish(&msg).await?; + } else { + self.tf_publisher.publish(&msg).await?; + } Ok(()) } @@ -567,7 +560,7 @@ where source_time: T, fixed_frame: &str, ) -> Result, TransformManagerError> { - let mut registry = self.registry.write().await; + let registry = self.registry.read().await; registry .get_transform_at( target_frame, @@ -578,6 +571,76 @@ where ) .map_err(|e| TransformManagerError::LookupError(e.to_string())) } + + /// Get the newest time at which a transform between two frames can be served. + /// + /// Returns `Stamp::At` with the newest time [Self::get_transform] can serve for the pair, + /// or `Stamp::Static` if the frames are connected entirely by static transforms, in which + /// case a transform is available at any time. + /// + /// Note: transforms keep arriving in the background, so the returned time is a snapshot. + /// It can be stale by the time a follow-up [Self::get_transform] runs, and if buffer cleanup + /// evicts it the follow-up lookup returns an error rather than a wrong transform. + /// + /// This is the equivalent of tf2's "latest available transform" lookup: + /// ``` + /// use roslibrust_transforms::{Quaternion, Ros1TFMessage, Stamp, Timestamp, Transform, TransformManager, Vector3}; + /// # #[tokio::main] + /// # async fn main() -> Result<(), Box> { + /// // Creating a fake ros instance for this example + /// let ros = roslibrust::mock::MockRos::new(); + /// let manager = TransformManager::::new(&ros, std::time::Duration::from_secs(10)).await?; + /// + /// let stamp = Timestamp::now(); + /// let transform = Transform::new( + /// "map", + /// "robot", + /// Vector3::new(1.0, 0.0, 0.0), + /// Quaternion::identity(), + /// Stamp::At(stamp), + /// )?; + /// manager.add_transform(transform).await?; + /// + /// // The newest time a lookup between the two frames can be served is the sample just added + /// let latest = manager.latest_common_time("map", "robot").await?; + /// assert_eq!(latest, Stamp::At(stamp)); + /// + /// // Look up the transform at the newest available time + /// let time = match latest { + /// Stamp::At(time) => time, + /// // Frames connected by static transforms only can be looked up at any time + /// Stamp::Static => Timestamp::now(), + /// }; + /// let transform = manager.get_transform("map", "robot", time).await?; + /// # Ok(()) + /// # } + /// ``` + pub async fn latest_common_time( + &self, + target_frame: &str, + source_frame: &str, + ) -> Result, TransformManagerError> { + let registry = self.registry.read().await; + registry + .latest_common_time(target_frame, source_frame) + .map_err(|e| TransformManagerError::LookupError(e.to_string())) + } + + /// Remove a frame and all of its transforms from the local buffer. + /// + /// The transforms crate does not support re-parenting: once a child frame is in the buffer, + /// transforms for the same child frame under a different parent are rejected (the subscriber + /// tasks log them as warnings). Removing the frame allows it to be re-added under its new + /// parent. Note that removing a frame in the middle of the tree strands its descendants + /// until the removed frame is received or added again. + /// + /// This only affects the local buffer, the tf buffers of other nodes are unaffected. + /// + /// Returns `true` if the frame existed. + pub async fn remove_frame(&self, frame: &str) -> bool { + let mut registry = self.registry.write().await; + registry.remove_frame(frame) + } } impl Drop for TransformManager @@ -621,29 +684,32 @@ impl IntoTransform for Ros1TransformStamped where T: RosTimestamp, { - fn into_transform(self, is_static: bool) -> transforms::Transform { + fn into_transform(self, is_static: bool) -> Result, TransformError> { let timestamp = if is_static { - T::static_timestamp() + Stamp::Static } else { - T::from_ros_time(self.header.stamp.secs, self.header.stamp.nsecs as u32) + Stamp::At(T::from_ros_time( + self.header.stamp.secs, + self.header.stamp.nsecs as u32, + )) }; - transforms::Transform { - translation: Vector3::new( + transforms::Transform::new( + &self.header.frame_id, + &self.child_frame_id, + Vector3::new( self.transform.translation.x, self.transform.translation.y, self.transform.translation.z, ), - rotation: Quaternion { + Quaternion { w: self.transform.rotation.w, x: self.transform.rotation.x, y: self.transform.rotation.y, z: self.transform.rotation.z, }, timestamp, - parent: self.header.frame_id, - child: self.child_frame_id, - } + ) } } @@ -654,31 +720,37 @@ where fn from_transform(transform: &transforms::Transform) -> Self { use crate::messages::ros1::{geometry_msgs, std_msgs}; - let (secs, nsecs) = transform.timestamp.as_ros_time(); + // Static transforms carry no instant, stamp them with time zero on the wire + let (secs, nsecs) = match transform.timestamp() { + Stamp::At(time) => time.as_ros_time(), + Stamp::Static => (0, 0), + }; if nsecs > i32::MAX as u32 { panic!("Timestamp overflow when converting to Ros1TransformStamped"); } let nsecs = nsecs as i32; + let translation = transform.translation(); + let rotation = transform.rotation(); Ros1TransformStamped { header: std_msgs::Header { seq: 0, stamp: roslibrust::codegen::integral_types::Time { secs, nsecs }, - frame_id: transform.parent.clone(), + frame_id: transform.parent().to_string(), }, - child_frame_id: transform.child.clone(), + child_frame_id: transform.child().to_string(), transform: geometry_msgs::Transform { translation: geometry_msgs::Vector3 { - x: transform.translation.x, - y: transform.translation.y, - z: transform.translation.z, + x: translation.x, + y: translation.y, + z: translation.z, }, rotation: geometry_msgs::Quaternion { - x: transform.rotation.x, - y: transform.rotation.y, - z: transform.rotation.z, - w: transform.rotation.w, + x: rotation.x, + y: rotation.y, + z: rotation.z, + w: rotation.w, }, }, } @@ -714,29 +786,32 @@ impl IntoTransform for Ros2TransformStamped where T: RosTimestamp, { - fn into_transform(self, is_static: bool) -> transforms::Transform { + fn into_transform(self, is_static: bool) -> Result, TransformError> { let timestamp = if is_static { - T::static_timestamp() + Stamp::Static } else { - T::from_ros_time(self.header.stamp.sec, self.header.stamp.nanosec) + Stamp::At(T::from_ros_time( + self.header.stamp.sec, + self.header.stamp.nanosec, + )) }; - transforms::Transform { - translation: Vector3::new( + transforms::Transform::new( + &self.header.frame_id, + &self.child_frame_id, + Vector3::new( self.transform.translation.x, self.transform.translation.y, self.transform.translation.z, ), - rotation: Quaternion { + Quaternion { w: self.transform.rotation.w, x: self.transform.rotation.x, y: self.transform.rotation.y, z: self.transform.rotation.z, }, timestamp, - parent: self.header.frame_id, - child: self.child_frame_id, - } + ) } } @@ -747,25 +822,32 @@ where fn from_transform(transform: &transforms::Transform) -> Self { use crate::messages::ros2::{builtin_interfaces, geometry_msgs, std_msgs}; - let (sec, nanosec) = transform.timestamp.as_ros_time(); + // Static transforms carry no instant, stamp them with time zero on the wire + let (sec, nanosec) = match transform.timestamp() { + Stamp::At(time) => time.as_ros_time(), + Stamp::Static => (0, 0), + }; + + let translation = transform.translation(); + let rotation = transform.rotation(); Ros2TransformStamped { header: std_msgs::Header { stamp: builtin_interfaces::Time { sec, nanosec }, - frame_id: transform.parent.clone(), + frame_id: transform.parent().to_string(), }, - child_frame_id: transform.child.clone(), + child_frame_id: transform.child().to_string(), transform: geometry_msgs::Transform { translation: geometry_msgs::Vector3 { - x: transform.translation.x, - y: transform.translation.y, - z: transform.translation.z, + x: translation.x, + y: translation.y, + z: translation.z, }, rotation: geometry_msgs::Quaternion { - x: transform.rotation.x, - y: transform.rotation.y, - z: transform.rotation.z, - w: transform.rotation.w, + x: rotation.x, + y: rotation.y, + z: rotation.z, + w: rotation.w, }, }, } diff --git a/roslibrust_transforms/tests/mocked_tests.rs b/roslibrust_transforms/tests/mocked_tests.rs index 9bce355..70c0f1e 100644 --- a/roslibrust_transforms/tests/mocked_tests.rs +++ b/roslibrust_transforms/tests/mocked_tests.rs @@ -2,13 +2,13 @@ use std::time::Duration; -use roslibrust_common::{Publish, TopicProvider}; +use roslibrust_common::{Publish, Subscribe, TopicProvider}; use roslibrust_mock::MockRos; -use transforms::time::{TimeError, TimePoint}; use roslibrust_transforms::messages::ros1::{geometry_msgs, std_msgs, TFMessage}; use roslibrust_transforms::{ - Quaternion, Ros1TFMessage, RosTimestamp, Timestamp, TransformManager, Vector3, + Quaternion, Ros1TFMessage, RosTimestamp, Stamp, TimeError, TimePoint, Timestamp, Transform, + TransformManager, Vector3, }; #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] @@ -17,10 +17,6 @@ struct MockTimestamp { } 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); @@ -36,14 +32,6 @@ impl TimePoint for MockTimestamp { 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 @@ -52,8 +40,8 @@ impl TimePoint for MockTimestamp { .ok_or(TimeError::DurationUnderflow) } - fn as_seconds(self) -> Result { - Ok(self.t as f64 / 1_000_000_000.0) + fn as_seconds_lossy(self) -> f64 { + self.t as f64 / 1_000_000_000.0 } } @@ -75,6 +63,16 @@ impl RosTimestamp for MockTimestamp { } } +#[test] +fn test_from_ros_time_clamps_pre_epoch_times() { + // Timestamp cannot represent times before the unix epoch, they clamp to zero + assert_eq!(Timestamp::from_ros_time(-1, 500_000_000), Timestamp::zero()); + assert_eq!( + Timestamp::from_ros_time(1, 500_000_000), + Timestamp::from_nanos(1_500_000_000) + ); +} + /// Helper function to create a TFMessage with a single transform. fn create_tf_message( parent_frame: &str, @@ -143,16 +141,17 @@ async fn test_transform_listener_with_custom_timestamp() { .get_transform("world", "custom_frame", lookup_time) .await .expect("Failed to look up transform with custom timestamp"); - assert_eq!(transform.timestamp, lookup_time); + assert_eq!(transform.timestamp(), Stamp::At(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 }, - }; + let transform = Transform::new( + "world", + "custom_from_manager", + Vector3::new(4.0, 5.0, 6.0), + Quaternion::identity(), + Stamp::At(MockTimestamp { t: 4_000_000_000 }), + ) + .expect("Failed to build transform with custom timestamp"); manager .add_transform(transform) .await @@ -166,9 +165,9 @@ async fn test_transform_listener_with_custom_timestamp() { ) .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); + 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] @@ -201,10 +200,8 @@ async fn test_transform_listener_receives_tf_messages() { let nsecs = now.subsec_nanos() as i32; let tf_msg = create_tf_message("world", "base_link", 1.0, 2.0, 3.0, secs, nsecs); - // Calculate the exact timestamp for lookup (same as what convert_transform_stamped uses) - let lookup_timestamp = Timestamp { - t: (secs as u128) * 1_000_000_000 + (nsecs as u128), - }; + // Calculate the exact timestamp for lookup (same as what from_ros_time uses) + let lookup_timestamp = Timestamp::from_nanos((secs as u64) * 1_000_000_000 + (nsecs as u64)); tf_publisher .publish(&tf_msg) @@ -244,7 +241,7 @@ async fn test_transform_listener_static_transforms() { // Give the listener time to subscribe tokio::time::sleep(Duration::from_millis(50)).await; - // Publish a static transform (timestamp doesn't matter for static transforms) + // Publish a static transform (its timestamp is ignored for static transforms) let tf_msg = create_tf_message("base_link", "camera_link", 0.5, 0.0, 0.3, 0, 0); tf_static_publisher @@ -255,9 +252,9 @@ async fn test_transform_listener_static_transforms() { // Give the listener time to process the message tokio::time::sleep(Duration::from_millis(100)).await; - // Check that the transform is available + // Check that the transform is available, static transforms are valid at any lookup time let can_transform = manager - .get_transform("base_link", "camera_link", Timestamp::zero()) + .get_transform("base_link", "camera_link", Timestamp::now()) .await; assert!( can_transform.is_ok(), @@ -297,49 +294,51 @@ async fn test_lookup_transform_values() { tokio::time::sleep(Duration::from_millis(100)).await; // Look up the transform and verify its values - // Static transforms use Timestamp::zero() + // Static transforms are valid at any lookup time let transform = manager - .get_transform("world", "sensor", Timestamp::zero()) + .get_transform("world", "sensor", Timestamp::now()) .await .expect("Failed to look up transform"); // Verify translation values + let translation = transform.translation(); assert!( - (transform.translation.x - 1.5).abs() < 1e-6, + (translation.x - 1.5).abs() < 1e-6, "Expected x=1.5, got {}", - transform.translation.x + translation.x ); assert!( - (transform.translation.y - 2.5).abs() < 1e-6, + (translation.y - 2.5).abs() < 1e-6, "Expected y=2.5, got {}", - transform.translation.y + translation.y ); assert!( - (transform.translation.z - 3.5).abs() < 1e-6, + (translation.z - 3.5).abs() < 1e-6, "Expected z=3.5, got {}", - transform.translation.z + translation.z ); // Verify rotation is identity (w=1, x=y=z=0) + let rotation = transform.rotation(); assert!( - (transform.rotation.w - 1.0).abs() < 1e-6, + (rotation.w - 1.0).abs() < 1e-6, "Expected rotation.w=1.0, got {}", - transform.rotation.w + rotation.w ); assert!( - transform.rotation.x.abs() < 1e-6, + rotation.x.abs() < 1e-6, "Expected rotation.x=0.0, got {}", - transform.rotation.x + rotation.x ); assert!( - transform.rotation.y.abs() < 1e-6, + rotation.y.abs() < 1e-6, "Expected rotation.y=0.0, got {}", - transform.rotation.y + rotation.y ); assert!( - transform.rotation.z.abs() < 1e-6, + rotation.z.abs() < 1e-6, "Expected rotation.z=0.0, got {}", - transform.rotation.z + rotation.z ); } @@ -374,12 +373,13 @@ async fn test_wait_for_transform_success() { }); // Wait for the transform - it should succeed after the delayed publish + // Static transforms are valid at any lookup time let start = tokio::time::Instant::now(); let result = manager .wait_for_transform( "world", "delayed_frame", - Timestamp::zero(), + Timestamp::now(), Some(Duration::from_secs(2)), ) .await; @@ -397,10 +397,10 @@ async fn test_wait_for_transform_success() { ); // Verify the transform values - let transform = result.unwrap(); - assert!((transform.translation.x - 1.0).abs() < 1e-6); - assert!((transform.translation.y - 2.0).abs() < 1e-6); - assert!((transform.translation.z - 3.0).abs() < 1e-6); + let translation = result.unwrap().translation(); + assert!((translation.x - 1.0).abs() < 1e-6); + assert!((translation.y - 2.0).abs() < 1e-6); + assert!((translation.z - 3.0).abs() < 1e-6); } #[tokio::test] @@ -480,12 +480,13 @@ async fn test_wait_for_transform_immediate_success() { tokio::time::sleep(Duration::from_millis(50)).await; // Wait for the transform - it should return immediately since it's already available + // Static transforms are valid at any lookup time let start = std::time::Instant::now(); let result = manager .wait_for_transform( "world", "immediate_frame", - Timestamp::zero(), + Timestamp::now(), Some(Duration::from_secs(5)), ) .await; @@ -575,33 +576,255 @@ async fn test_get_transform_at_different_times() { // 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 t1 = Timestamp::from_nanos(1_000_000_000); + let t2 = Timestamp::from_nanos(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); + assert_eq!(transform.parent(), "a"); + assert_eq!(transform.child(), "b"); + assert_eq!(transform.timestamp(), Stamp::At(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) + let translation = transform.translation(); assert!( - (transform.translation.x + 1.0).abs() < 1e-6, + (translation.x + 1.0).abs() < 1e-6, "Expected x=-1.0, got {}", - transform.translation.x + translation.x ); assert!( - (transform.translation.y - 1.0).abs() < 1e-6, + (translation.y - 1.0).abs() < 1e-6, "Expected y=1.0, got {}", - transform.translation.y + translation.y ); assert!( - transform.translation.z.abs() < 1e-6, + translation.z.abs() < 1e-6, "Expected z=0.0, got {}", - transform.translation.z + translation.z + ); +} + +#[tokio::test] +async fn test_invalid_transforms_are_dropped() { + 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; + + // Publish a transform with a denormalized rotation, which fails validation during conversion + // and is dropped by the listener + let mut bad_msg = create_tf_message("world", "bad_frame", 1.0, 0.0, 0.0, 1, 0); + bad_msg.transforms[0].transform.rotation.w = 2.0; + tf_publisher + .publish(&bad_msg) + .await + .expect("Failed to publish invalid transform"); + + // Publish a valid transform afterwards to verify the listener keeps processing + let good_msg = create_tf_message("world", "good_frame", 1.0, 0.0, 0.0, 1, 0); + tf_publisher + .publish(&good_msg) + .await + .expect("Failed to publish valid transform"); + + // Give the listener time to process the messages + tokio::time::sleep(Duration::from_millis(100)).await; + + let t1 = Timestamp::from_nanos(1_000_000_000); + let result = manager.get_transform("world", "bad_frame", t1).await; + assert!( + result.is_err(), + "Invalid transform should not have been added to the buffer" + ); + + let result = manager.get_transform("world", "good_frame", t1).await; + assert!( + result.is_ok(), + "Valid transform should still be processed after an invalid one" + ); +} + +#[tokio::test] +async fn test_add_static_transform_publishes_to_tf_static() { + tokio::time::pause(); + let mock_ros = MockRos::new(); + + // Create a subscriber on /tf_static to observe what the manager publishes + let mut tf_static_subscriber = mock_ros + .subscribe::("/tf_static") + .await + .expect("Failed to create /tf_static subscriber"); + + // 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; + + // Static transforms are published to /tf_static instead of /tf + let transform = Transform::static_between( + "base_link", + "imu_link", + Vector3::new(0.1, 0.0, 0.2), + Quaternion::identity(), + ) + .expect("Failed to build static transform"); + manager + .add_transform(transform) + .await + .expect("Failed to add static transform"); + + let msg = tf_static_subscriber + .next() + .await + .expect("Failed to receive message on /tf_static"); + assert_eq!(msg.transforms.len(), 1); + assert_eq!(msg.transforms[0].header.frame_id, "base_link"); + assert_eq!(msg.transforms[0].child_frame_id, "imu_link"); + + // The static transform is also available in the local buffer at any lookup time + let transform = manager + .get_transform("base_link", "imu_link", Timestamp::now()) + .await + .expect("Failed to look up static transform"); + assert!((transform.translation().x - 0.1).abs() < 1e-6); + assert!((transform.translation().z - 0.2).abs() < 1e-6); +} + +#[tokio::test] +async fn test_latest_common_time() { + 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; + + // world -> a is available at t=1s and t=2s, but a -> b only at t=1s + let world_to_a_t1 = create_tf_message("world", "a", 1.0, 0.0, 0.0, 1, 0); + tf_publisher + .publish(&world_to_a_t1) + .await + .expect("Failed to publish world->a at t=1s"); + let world_to_a_t2 = create_tf_message("world", "a", 2.0, 0.0, 0.0, 2, 0); + tf_publisher + .publish(&world_to_a_t2) + .await + .expect("Failed to publish world->a at t=2s"); + 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; + + // The newest time the whole chain can serve is bounded by the lagging a -> b hop + let t1 = Timestamp::from_nanos(1_000_000_000); + let latest = manager + .latest_common_time("world", "b") + .await + .expect("Failed to get latest common time"); + assert_eq!(latest, Stamp::At(t1)); + + // And the returned time is servable + let result = manager.get_transform("world", "b", t1).await; + assert!( + result.is_ok(), + "Transform should be available at the latest common time" + ); +} + +#[tokio::test] +async fn test_reparenting_is_rejected_until_frame_removed() { + 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; + + // tool is first received as a child of world, pinning its parent + let world_to_tool = create_tf_message("world", "tool", 1.0, 0.0, 0.0, 1, 0); + tf_publisher + .publish(&world_to_tool) + .await + .expect("Failed to publish world->tool"); + + // A transform re-parenting tool under gripper is rejected and dropped with a warning + let gripper_to_tool = create_tf_message("gripper", "tool", 0.0, 1.0, 0.0, 2, 0); + tf_publisher + .publish(&gripper_to_tool) + .await + .expect("Failed to publish gripper->tool"); + + // Give the listener time to process the messages + tokio::time::sleep(Duration::from_millis(100)).await; + + let t1 = Timestamp::from_nanos(1_000_000_000); + let t2 = Timestamp::from_nanos(2_000_000_000); + assert!( + manager.get_transform("world", "tool", t1).await.is_ok(), + "Original parent should still serve" + ); + assert!( + manager.get_transform("gripper", "tool", t2).await.is_err(), + "Re-parented transform should have been dropped" + ); + + // remove_frame() is the escape hatch that allows the frame to be re-added under a new parent + assert!(manager.remove_frame("tool").await); + tf_publisher + .publish(&gripper_to_tool) + .await + .expect("Failed to re-publish gripper->tool"); + tokio::time::sleep(Duration::from_millis(100)).await; + + assert!( + manager.get_transform("gripper", "tool", t2).await.is_ok(), + "Frame should be re-added under its new parent after remove_frame" ); }