Struct TransformTree

Source

pub struct TransformTree<T: Copy + Debug + Default + 'static> { /* private fields */ }

Implementations§

Source §

impl<T> TransformTree<T>
where Transform3D<T>: Clone + HasInverse<T> + Mul<Output = Transform3D<T>>, T: Add<Output = T> + Sub<Output = T> + Mul<Output = T> + Div<Output = T> + AddAssign + SubAssign + NumCast + Copy + Debug + Default + One + Serialize + DeserializeOwned + 'static + Neg<Output = T>,

Source

pub fn new() -> Self

Create a new transform tree with default settings

Examples found in repository ?

examples/payload_usage.rs (line 12)

6fn main() {
7    println!("Cu Transform - CuMsg Pattern Demo");
8    println!("=================================");
9    println!();
10
11    // Create a transform tree
12    let mut tree = TransformTree::<f32>::new();
13    let clock = RobotClock::default();
14
15    // Create transforms using the new CuMsg pattern
16    println!("Creating transforms with CuMsg...");
17
18    // World to base transform
19    let transform1 = Transform3D::from_matrix([
20        [1.0f32, 0.0, 0.0, 0.0], // Column-major: each inner array is a column
21        [0.0, 1.0, 0.0, 0.0],
22        [0.0, 0.0, 1.0, 0.0],
23        [1.0, 0.0, 0.0, 1.0], // Translation (1, 0, 0)
24    ]);
25
26    let frame_transform = FrameTransform::new(
27        transform1,
28        FrameIdString::from("world").expect("Frame name too long"),
29        FrameIdString::from("base").expect("Frame name too long"),
30    );
31    let mut sft = StampedFrameTransform::new(Some(frame_transform));
32    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
33
34    // Add to tree using the new API
35    tree.add_transform(&sft).expect("Failed to add transform");
36    println!("  Added world->base transform at t=1s");
37
38    // Base to arm transform
39    let transform2 = Transform3D::from_matrix([
40        [0.0, 1.0, 0.0, 0.0], // 90-degree rotation around Z
41        [-1.0, 0.0, 0.0, 0.0],
42        [0.0, 0.0, 1.0, 0.0],
43        [0.5, 0.0, 0.0, 1.0], // Translation (0.5, 0, 0)
44    ]);
45
46    let msg2 = FrameTransform::new(
47        transform2,
48        FrameIdString::from("base").expect("Frame name too long"),
49        FrameIdString::from("arm").expect("Frame name too long"),
50    );
51    let mut sft = StampedFrameTransform::new(Some(msg2));
52    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
53
54    tree.add_transform(&sft).expect("Failed to add transform");
55    println!("  Added base->arm transform at t=1s");
56
57    // Demonstrate using time ranges for a broadcast
58    println!("\nBroadcasting multiple transforms with time range...");
59
60    // Create multiple transforms at different times
61    let times = [
62        CuDuration(2_000_000_000), // 2 seconds
63        CuDuration(2_100_000_000), // 2.1 seconds
64        CuDuration(2_200_000_000),
65    ];
66
67    for (i, &time) in times.iter().enumerate() {
68        let x_translation = 1.0 + (i as f32) * 0.1;
69        let transform = Transform3D::from_matrix([
70            [1.0, 0.0, 0.0, 0.0],
71            [0.0, 1.0, 0.0, 0.0],
72            [0.0, 0.0, 1.0, 0.0],
73            [x_translation, 0.0, 0.0, 1.0],
74        ]);
75
76        let msg = FrameTransform::new(
77            transform,
78            FrameIdString::from("world").expect("Frame name too long"),
79            FrameIdString::from("base").expect("Frame name too long"),
80        );
81        let mut sft = StampedFrameTransform::new(Some(msg));
82
83        // For a broadcast with multiple transforms, use Range
84        if i == 0 {
85            sft.tov = Tov::Range(cu29::clock::CuTimeRange {
86                start: times[0],
87                end: *times.last().unwrap(),
88            });
89        } else {
90            sft.tov = Tov::Time(time);
91        }
92
93        tree.add_transform(&sft).expect("Failed to add transform");
94    }
95
96    println!("  Added 3 transforms with time range 2.0s - 2.2s");
97
98    // Query the tree
99    println!("\nQuerying transforms...");
100
101    let result = tree.lookup_transform("world", "arm", CuDuration(1_000_000_000), &clock);
102    match result {
103        Ok(transform) => {
104            let mat = transform.to_matrix();
105            println!(
106                "  World->arm at t=1s: translation=({:.2}, {:.2}, {:.2})",
107                mat[3][0], mat[3][1], mat[3][2]
108            );
109        }
110        Err(e) => println!("  Error: {e}"),
111    }
112
113    // Query velocity (requires transforms at multiple times)
114    let velocity = tree.lookup_velocity("world", "base", CuDuration(2_100_000_000), &clock);
115    match velocity {
116        Ok(vel) => {
117            println!(
118                "  World->base velocity at t=2.1s: linear=({:.2}, {:.2}, {:.2}) m/s",
119                vel.linear[0], vel.linear[1], vel.linear[2]
120            );
121        }
122        Err(e) => println!("  Error computing velocity: {e}"),
123    }
124
125    println!("\nKey advantages of CuMsg pattern:");
126    println!("- Integrates with Copper message system");
127    println!("- Timestamps handled by CuMsg metadata (Tov)");
128    println!("- Supports time ranges for broadcasts");
129}

More examples

Hide additional examples

examples/basic_usage.rs (line 140)

9fn main() {
10    // Example using the typed transform approach
11    println!("Cu Transform - New Typed Approach Demo");
12    println!("=====================================");
13
14    // Create a buffer for world -> robot transforms
15    let mut world_to_robot_buffer: TypedTransformBuffer<f32, WorldFrame, RobotFrame, 10> =
16        TypedTransformBuffer::new();
17
18    // Create a transform message
19    let transform = Transform3D::from_matrix([
20        [1.0, 0.0, 0.0, 1.0], // X translation
21        [0.0, 1.0, 0.0, 2.0], // Y translation
22        [0.0, 0.0, 1.0, 0.0],
23        [0.0, 0.0, 0.0, 1.0],
24    ]);
25
26    let world_to_robot_msg = TypedTransform::new(transform, CuDuration(1000));
27
28    println!(
29        "Created transform from {} to {}",
30        world_to_robot_msg.parent_name(),
31        world_to_robot_msg.child_name()
32    );
33    if let Some(t) = world_to_robot_msg.transform() {
34        let mat = t.to_matrix();
35        println!(
36            "  Translation: [{}, {}, {}]",
37            mat[0][3], mat[1][3], mat[2][3]
38        );
39    }
40
41    // Add to buffer
42    world_to_robot_buffer.add_transform(world_to_robot_msg);
43
44    // Create second transform
45    let transform2 = Transform3D::from_matrix([
46        [1.0, 0.0, 0.0, 2.0], // X translation
47        [0.0, 1.0, 0.0, 4.0], // Y translation
48        [0.0, 0.0, 1.0, 0.0],
49        [0.0, 0.0, 0.0, 1.0],
50    ]);
51
52    let world_to_robot_msg2 = TypedTransform::new(transform2, CuDuration(2000));
53    world_to_robot_buffer.add_transform(world_to_robot_msg2);
54
55    // Query the buffer
56    if let Some(latest) = world_to_robot_buffer.get_latest_transform() {
57        println!(
58            "\nLatest transform at time {}:",
59            latest.timestamp().unwrap().as_nanos()
60        );
61        if let Some(t) = latest.transform() {
62            let mat = t.to_matrix();
63            println!(
64                "  Translation: [{}, {}, {}]",
65                mat[0][3], mat[1][3], mat[2][3]
66            );
67        }
68    }
69
70    // Query closest to a specific time
71    if let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1500)) {
72        println!("\nClosest transform to time 1500:");
73        println!("  Actual time: {}", closest.timestamp().unwrap().as_nanos());
74        if let Some(t) = closest.transform() {
75            let mat = t.to_matrix();
76            println!(
77                "  Translation: [{}, {}, {}]",
78                mat[0][3], mat[1][3], mat[2][3]
79            );
80        }
81    }
82
83    // Demonstrate time range
84    if let Some(range) = world_to_robot_buffer.get_time_range() {
85        println!(
86            "\nTime range: {} to {}",
87            range.start.as_nanos(),
88            range.end.as_nanos()
89        );
90    }
91
92    // Demonstrate velocity computation
93    if let Some(latest) = world_to_robot_buffer.get_latest_transform()
94        && let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1000))
95        && let Some(velocity) = latest.compute_velocity(closest)
96    {
97        println!("\nVelocity computation:");
98        println!(
99            "  Linear velocity: [{}, {}, {}]",
100            velocity.linear[0], velocity.linear[1], velocity.linear[2]
101        );
102    }
103
104    // Demonstrate the stringly typed version of the API.
105    println!("\n\nConstant-Size Buffer Demo");
106    println!("===========================================");
107
108    let mut const_buffer: ConstTransformBuffer<f32, 5> = ConstTransformBuffer::new();
109
110    // Add some stamped transforms
111    let stamped_transform = StampedTransform {
112        transform,
113        stamp: CuDuration(1000),
114        parent_frame: "world".try_into().unwrap(),
115        child_frame: "robot".try_into().unwrap(),
116    };
117
118    const_buffer.add_transform(stamped_transform);
119
120    if let Some(latest_stamped) = const_buffer.get_latest_transform() {
121        println!("Latest transform in constant buffer:");
122        println!(
123            "  From: {} to: {}",
124            latest_stamped.parent_frame, latest_stamped.child_frame
125        );
126        println!("  Time: {}", latest_stamped.stamp.as_nanos());
127        let mat = latest_stamped.transform.to_matrix();
128        println!(
129            "  Translation: [{}, {}, {}]",
130            mat[0][3], mat[1][3], mat[2][3]
131        );
132    }
133
134    println!("\nThis buffer is stack-allocated with capacity 5 - no heap allocation!");
135
136    // Demonstrate the StampedFrameTransform pattern with TransformTree
137    println!("\n\nStampedFrameTransform Pattern Demo");
138    println!("================================");
139
140    let mut tree = TransformTree::<f32>::new();
141
142    // Create a CuMsg with TransformMsg
143    let frame_transform = FrameTransform::new(
144        transform,
145        FrameIdString::from("world").expect("Frame name too long"),
146        FrameIdString::from("robot").expect("Frame name too long"),
147    );
148
149    let mut sft = StampedFrameTransform::new(Some(frame_transform));
150    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
151
152    // Add using the new API
153    tree.add_transform(&sft).expect("Failed to add transform");
154    println!("Added transform using CuMsg<TransformMsg> pattern");
155
156    // Query the transform
157    let robot_clock = cu29::clock::RobotClock::default();
158    let result = tree.lookup_transform("world", "robot", CuDuration(1_000_000_000), &robot_clock);
159
160    match result {
161        Ok(transform) => {
162            let mat = transform.to_matrix();
163            println!(
164                "Retrieved transform: translation=({:.2}, {:.2}, {:.2})",
165                mat[3][0], mat[3][1], mat[3][2]
166            );
167        }
168        Err(e) => println!("Error: {e}"),
169    }
170}

Source

pub fn with_cache_settings(cache_size: usize, cache_age: Duration) -> Self

Create a new transform tree with custom cache settings

Source

pub fn clear_cache(&self)

Clear the transform cache

Source

pub fn cleanup_cache(&self, robot_clock: &RobotClock)

Perform scheduled cache cleanup operation

Source

pub fn add_transform( &mut self, sft: &StampedFrameTransform<T>, ) -> TransformResult<()>
where T: Encode + Decode<()>,

add a transform to the tree.

Examples found in repository ?

examples/payload_usage.rs (line 35)

6fn main() {
7    println!("Cu Transform - CuMsg Pattern Demo");
8    println!("=================================");
9    println!();
10
11    // Create a transform tree
12    let mut tree = TransformTree::<f32>::new();
13    let clock = RobotClock::default();
14
15    // Create transforms using the new CuMsg pattern
16    println!("Creating transforms with CuMsg...");
17
18    // World to base transform
19    let transform1 = Transform3D::from_matrix([
20        [1.0f32, 0.0, 0.0, 0.0], // Column-major: each inner array is a column
21        [0.0, 1.0, 0.0, 0.0],
22        [0.0, 0.0, 1.0, 0.0],
23        [1.0, 0.0, 0.0, 1.0], // Translation (1, 0, 0)
24    ]);
25
26    let frame_transform = FrameTransform::new(
27        transform1,
28        FrameIdString::from("world").expect("Frame name too long"),
29        FrameIdString::from("base").expect("Frame name too long"),
30    );
31    let mut sft = StampedFrameTransform::new(Some(frame_transform));
32    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
33
34    // Add to tree using the new API
35    tree.add_transform(&sft).expect("Failed to add transform");
36    println!("  Added world->base transform at t=1s");
37
38    // Base to arm transform
39    let transform2 = Transform3D::from_matrix([
40        [0.0, 1.0, 0.0, 0.0], // 90-degree rotation around Z
41        [-1.0, 0.0, 0.0, 0.0],
42        [0.0, 0.0, 1.0, 0.0],
43        [0.5, 0.0, 0.0, 1.0], // Translation (0.5, 0, 0)
44    ]);
45
46    let msg2 = FrameTransform::new(
47        transform2,
48        FrameIdString::from("base").expect("Frame name too long"),
49        FrameIdString::from("arm").expect("Frame name too long"),
50    );
51    let mut sft = StampedFrameTransform::new(Some(msg2));
52    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
53
54    tree.add_transform(&sft).expect("Failed to add transform");
55    println!("  Added base->arm transform at t=1s");
56
57    // Demonstrate using time ranges for a broadcast
58    println!("\nBroadcasting multiple transforms with time range...");
59
60    // Create multiple transforms at different times
61    let times = [
62        CuDuration(2_000_000_000), // 2 seconds
63        CuDuration(2_100_000_000), // 2.1 seconds
64        CuDuration(2_200_000_000),
65    ];
66
67    for (i, &time) in times.iter().enumerate() {
68        let x_translation = 1.0 + (i as f32) * 0.1;
69        let transform = Transform3D::from_matrix([
70            [1.0, 0.0, 0.0, 0.0],
71            [0.0, 1.0, 0.0, 0.0],
72            [0.0, 0.0, 1.0, 0.0],
73            [x_translation, 0.0, 0.0, 1.0],
74        ]);
75
76        let msg = FrameTransform::new(
77            transform,
78            FrameIdString::from("world").expect("Frame name too long"),
79            FrameIdString::from("base").expect("Frame name too long"),
80        );
81        let mut sft = StampedFrameTransform::new(Some(msg));
82
83        // For a broadcast with multiple transforms, use Range
84        if i == 0 {
85            sft.tov = Tov::Range(cu29::clock::CuTimeRange {
86                start: times[0],
87                end: *times.last().unwrap(),
88            });
89        } else {
90            sft.tov = Tov::Time(time);
91        }
92
93        tree.add_transform(&sft).expect("Failed to add transform");
94    }
95
96    println!("  Added 3 transforms with time range 2.0s - 2.2s");
97
98    // Query the tree
99    println!("\nQuerying transforms...");
100
101    let result = tree.lookup_transform("world", "arm", CuDuration(1_000_000_000), &clock);
102    match result {
103        Ok(transform) => {
104            let mat = transform.to_matrix();
105            println!(
106                "  World->arm at t=1s: translation=({:.2}, {:.2}, {:.2})",
107                mat[3][0], mat[3][1], mat[3][2]
108            );
109        }
110        Err(e) => println!("  Error: {e}"),
111    }
112
113    // Query velocity (requires transforms at multiple times)
114    let velocity = tree.lookup_velocity("world", "base", CuDuration(2_100_000_000), &clock);
115    match velocity {
116        Ok(vel) => {
117            println!(
118                "  World->base velocity at t=2.1s: linear=({:.2}, {:.2}, {:.2}) m/s",
119                vel.linear[0], vel.linear[1], vel.linear[2]
120            );
121        }
122        Err(e) => println!("  Error computing velocity: {e}"),
123    }
124
125    println!("\nKey advantages of CuMsg pattern:");
126    println!("- Integrates with Copper message system");
127    println!("- Timestamps handled by CuMsg metadata (Tov)");
128    println!("- Supports time ranges for broadcasts");
129}

More examples

Hide additional examples

examples/basic_usage.rs (line 153)

9fn main() {
10    // Example using the typed transform approach
11    println!("Cu Transform - New Typed Approach Demo");
12    println!("=====================================");
13
14    // Create a buffer for world -> robot transforms
15    let mut world_to_robot_buffer: TypedTransformBuffer<f32, WorldFrame, RobotFrame, 10> =
16        TypedTransformBuffer::new();
17
18    // Create a transform message
19    let transform = Transform3D::from_matrix([
20        [1.0, 0.0, 0.0, 1.0], // X translation
21        [0.0, 1.0, 0.0, 2.0], // Y translation
22        [0.0, 0.0, 1.0, 0.0],
23        [0.0, 0.0, 0.0, 1.0],
24    ]);
25
26    let world_to_robot_msg = TypedTransform::new(transform, CuDuration(1000));
27
28    println!(
29        "Created transform from {} to {}",
30        world_to_robot_msg.parent_name(),
31        world_to_robot_msg.child_name()
32    );
33    if let Some(t) = world_to_robot_msg.transform() {
34        let mat = t.to_matrix();
35        println!(
36            "  Translation: [{}, {}, {}]",
37            mat[0][3], mat[1][3], mat[2][3]
38        );
39    }
40
41    // Add to buffer
42    world_to_robot_buffer.add_transform(world_to_robot_msg);
43
44    // Create second transform
45    let transform2 = Transform3D::from_matrix([
46        [1.0, 0.0, 0.0, 2.0], // X translation
47        [0.0, 1.0, 0.0, 4.0], // Y translation
48        [0.0, 0.0, 1.0, 0.0],
49        [0.0, 0.0, 0.0, 1.0],
50    ]);
51
52    let world_to_robot_msg2 = TypedTransform::new(transform2, CuDuration(2000));
53    world_to_robot_buffer.add_transform(world_to_robot_msg2);
54
55    // Query the buffer
56    if let Some(latest) = world_to_robot_buffer.get_latest_transform() {
57        println!(
58            "\nLatest transform at time {}:",
59            latest.timestamp().unwrap().as_nanos()
60        );
61        if let Some(t) = latest.transform() {
62            let mat = t.to_matrix();
63            println!(
64                "  Translation: [{}, {}, {}]",
65                mat[0][3], mat[1][3], mat[2][3]
66            );
67        }
68    }
69
70    // Query closest to a specific time
71    if let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1500)) {
72        println!("\nClosest transform to time 1500:");
73        println!("  Actual time: {}", closest.timestamp().unwrap().as_nanos());
74        if let Some(t) = closest.transform() {
75            let mat = t.to_matrix();
76            println!(
77                "  Translation: [{}, {}, {}]",
78                mat[0][3], mat[1][3], mat[2][3]
79            );
80        }
81    }
82
83    // Demonstrate time range
84    if let Some(range) = world_to_robot_buffer.get_time_range() {
85        println!(
86            "\nTime range: {} to {}",
87            range.start.as_nanos(),
88            range.end.as_nanos()
89        );
90    }
91
92    // Demonstrate velocity computation
93    if let Some(latest) = world_to_robot_buffer.get_latest_transform()
94        && let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1000))
95        && let Some(velocity) = latest.compute_velocity(closest)
96    {
97        println!("\nVelocity computation:");
98        println!(
99            "  Linear velocity: [{}, {}, {}]",
100            velocity.linear[0], velocity.linear[1], velocity.linear[2]
101        );
102    }
103
104    // Demonstrate the stringly typed version of the API.
105    println!("\n\nConstant-Size Buffer Demo");
106    println!("===========================================");
107
108    let mut const_buffer: ConstTransformBuffer<f32, 5> = ConstTransformBuffer::new();
109
110    // Add some stamped transforms
111    let stamped_transform = StampedTransform {
112        transform,
113        stamp: CuDuration(1000),
114        parent_frame: "world".try_into().unwrap(),
115        child_frame: "robot".try_into().unwrap(),
116    };
117
118    const_buffer.add_transform(stamped_transform);
119
120    if let Some(latest_stamped) = const_buffer.get_latest_transform() {
121        println!("Latest transform in constant buffer:");
122        println!(
123            "  From: {} to: {}",
124            latest_stamped.parent_frame, latest_stamped.child_frame
125        );
126        println!("  Time: {}", latest_stamped.stamp.as_nanos());
127        let mat = latest_stamped.transform.to_matrix();
128        println!(
129            "  Translation: [{}, {}, {}]",
130            mat[0][3], mat[1][3], mat[2][3]
131        );
132    }
133
134    println!("\nThis buffer is stack-allocated with capacity 5 - no heap allocation!");
135
136    // Demonstrate the StampedFrameTransform pattern with TransformTree
137    println!("\n\nStampedFrameTransform Pattern Demo");
138    println!("================================");
139
140    let mut tree = TransformTree::<f32>::new();
141
142    // Create a CuMsg with TransformMsg
143    let frame_transform = FrameTransform::new(
144        transform,
145        FrameIdString::from("world").expect("Frame name too long"),
146        FrameIdString::from("robot").expect("Frame name too long"),
147    );
148
149    let mut sft = StampedFrameTransform::new(Some(frame_transform));
150    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
151
152    // Add using the new API
153    tree.add_transform(&sft).expect("Failed to add transform");
154    println!("Added transform using CuMsg<TransformMsg> pattern");
155
156    // Query the transform
157    let robot_clock = cu29::clock::RobotClock::default();
158    let result = tree.lookup_transform("world", "robot", CuDuration(1_000_000_000), &robot_clock);
159
160    match result {
161        Ok(transform) => {
162            let mat = transform.to_matrix();
163            println!(
164                "Retrieved transform: translation=({:.2}, {:.2}, {:.2})",
165                mat[3][0], mat[3][1], mat[3][2]
166            );
167        }
168        Err(e) => println!("Error: {e}"),
169    }
170}

Source

pub fn find_path( &self, from_frame: &str, to_frame: &str, ) -> TransformResult<Vec<(FrameIdString, FrameIdString, bool)>>

Source

pub fn lookup_transform( &self, from_frame: &str, to_frame: &str, time: CuTime, robot_clock: &RobotClock, ) -> TransformResult<Transform3D<T>>

Examples found in repository ?

examples/payload_usage.rs (line 101)

6fn main() {
7    println!("Cu Transform - CuMsg Pattern Demo");
8    println!("=================================");
9    println!();
10
11    // Create a transform tree
12    let mut tree = TransformTree::<f32>::new();
13    let clock = RobotClock::default();
14
15    // Create transforms using the new CuMsg pattern
16    println!("Creating transforms with CuMsg...");
17
18    // World to base transform
19    let transform1 = Transform3D::from_matrix([
20        [1.0f32, 0.0, 0.0, 0.0], // Column-major: each inner array is a column
21        [0.0, 1.0, 0.0, 0.0],
22        [0.0, 0.0, 1.0, 0.0],
23        [1.0, 0.0, 0.0, 1.0], // Translation (1, 0, 0)
24    ]);
25
26    let frame_transform = FrameTransform::new(
27        transform1,
28        FrameIdString::from("world").expect("Frame name too long"),
29        FrameIdString::from("base").expect("Frame name too long"),
30    );
31    let mut sft = StampedFrameTransform::new(Some(frame_transform));
32    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
33
34    // Add to tree using the new API
35    tree.add_transform(&sft).expect("Failed to add transform");
36    println!("  Added world->base transform at t=1s");
37
38    // Base to arm transform
39    let transform2 = Transform3D::from_matrix([
40        [0.0, 1.0, 0.0, 0.0], // 90-degree rotation around Z
41        [-1.0, 0.0, 0.0, 0.0],
42        [0.0, 0.0, 1.0, 0.0],
43        [0.5, 0.0, 0.0, 1.0], // Translation (0.5, 0, 0)
44    ]);
45
46    let msg2 = FrameTransform::new(
47        transform2,
48        FrameIdString::from("base").expect("Frame name too long"),
49        FrameIdString::from("arm").expect("Frame name too long"),
50    );
51    let mut sft = StampedFrameTransform::new(Some(msg2));
52    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
53
54    tree.add_transform(&sft).expect("Failed to add transform");
55    println!("  Added base->arm transform at t=1s");
56
57    // Demonstrate using time ranges for a broadcast
58    println!("\nBroadcasting multiple transforms with time range...");
59
60    // Create multiple transforms at different times
61    let times = [
62        CuDuration(2_000_000_000), // 2 seconds
63        CuDuration(2_100_000_000), // 2.1 seconds
64        CuDuration(2_200_000_000),
65    ];
66
67    for (i, &time) in times.iter().enumerate() {
68        let x_translation = 1.0 + (i as f32) * 0.1;
69        let transform = Transform3D::from_matrix([
70            [1.0, 0.0, 0.0, 0.0],
71            [0.0, 1.0, 0.0, 0.0],
72            [0.0, 0.0, 1.0, 0.0],
73            [x_translation, 0.0, 0.0, 1.0],
74        ]);
75
76        let msg = FrameTransform::new(
77            transform,
78            FrameIdString::from("world").expect("Frame name too long"),
79            FrameIdString::from("base").expect("Frame name too long"),
80        );
81        let mut sft = StampedFrameTransform::new(Some(msg));
82
83        // For a broadcast with multiple transforms, use Range
84        if i == 0 {
85            sft.tov = Tov::Range(cu29::clock::CuTimeRange {
86                start: times[0],
87                end: *times.last().unwrap(),
88            });
89        } else {
90            sft.tov = Tov::Time(time);
91        }
92
93        tree.add_transform(&sft).expect("Failed to add transform");
94    }
95
96    println!("  Added 3 transforms with time range 2.0s - 2.2s");
97
98    // Query the tree
99    println!("\nQuerying transforms...");
100
101    let result = tree.lookup_transform("world", "arm", CuDuration(1_000_000_000), &clock);
102    match result {
103        Ok(transform) => {
104            let mat = transform.to_matrix();
105            println!(
106                "  World->arm at t=1s: translation=({:.2}, {:.2}, {:.2})",
107                mat[3][0], mat[3][1], mat[3][2]
108            );
109        }
110        Err(e) => println!("  Error: {e}"),
111    }
112
113    // Query velocity (requires transforms at multiple times)
114    let velocity = tree.lookup_velocity("world", "base", CuDuration(2_100_000_000), &clock);
115    match velocity {
116        Ok(vel) => {
117            println!(
118                "  World->base velocity at t=2.1s: linear=({:.2}, {:.2}, {:.2}) m/s",
119                vel.linear[0], vel.linear[1], vel.linear[2]
120            );
121        }
122        Err(e) => println!("  Error computing velocity: {e}"),
123    }
124
125    println!("\nKey advantages of CuMsg pattern:");
126    println!("- Integrates with Copper message system");
127    println!("- Timestamps handled by CuMsg metadata (Tov)");
128    println!("- Supports time ranges for broadcasts");
129}

More examples

Hide additional examples

examples/basic_usage.rs (line 158)

9fn main() {
10    // Example using the typed transform approach
11    println!("Cu Transform - New Typed Approach Demo");
12    println!("=====================================");
13
14    // Create a buffer for world -> robot transforms
15    let mut world_to_robot_buffer: TypedTransformBuffer<f32, WorldFrame, RobotFrame, 10> =
16        TypedTransformBuffer::new();
17
18    // Create a transform message
19    let transform = Transform3D::from_matrix([
20        [1.0, 0.0, 0.0, 1.0], // X translation
21        [0.0, 1.0, 0.0, 2.0], // Y translation
22        [0.0, 0.0, 1.0, 0.0],
23        [0.0, 0.0, 0.0, 1.0],
24    ]);
25
26    let world_to_robot_msg = TypedTransform::new(transform, CuDuration(1000));
27
28    println!(
29        "Created transform from {} to {}",
30        world_to_robot_msg.parent_name(),
31        world_to_robot_msg.child_name()
32    );
33    if let Some(t) = world_to_robot_msg.transform() {
34        let mat = t.to_matrix();
35        println!(
36            "  Translation: [{}, {}, {}]",
37            mat[0][3], mat[1][3], mat[2][3]
38        );
39    }
40
41    // Add to buffer
42    world_to_robot_buffer.add_transform(world_to_robot_msg);
43
44    // Create second transform
45    let transform2 = Transform3D::from_matrix([
46        [1.0, 0.0, 0.0, 2.0], // X translation
47        [0.0, 1.0, 0.0, 4.0], // Y translation
48        [0.0, 0.0, 1.0, 0.0],
49        [0.0, 0.0, 0.0, 1.0],
50    ]);
51
52    let world_to_robot_msg2 = TypedTransform::new(transform2, CuDuration(2000));
53    world_to_robot_buffer.add_transform(world_to_robot_msg2);
54
55    // Query the buffer
56    if let Some(latest) = world_to_robot_buffer.get_latest_transform() {
57        println!(
58            "\nLatest transform at time {}:",
59            latest.timestamp().unwrap().as_nanos()
60        );
61        if let Some(t) = latest.transform() {
62            let mat = t.to_matrix();
63            println!(
64                "  Translation: [{}, {}, {}]",
65                mat[0][3], mat[1][3], mat[2][3]
66            );
67        }
68    }
69
70    // Query closest to a specific time
71    if let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1500)) {
72        println!("\nClosest transform to time 1500:");
73        println!("  Actual time: {}", closest.timestamp().unwrap().as_nanos());
74        if let Some(t) = closest.transform() {
75            let mat = t.to_matrix();
76            println!(
77                "  Translation: [{}, {}, {}]",
78                mat[0][3], mat[1][3], mat[2][3]
79            );
80        }
81    }
82
83    // Demonstrate time range
84    if let Some(range) = world_to_robot_buffer.get_time_range() {
85        println!(
86            "\nTime range: {} to {}",
87            range.start.as_nanos(),
88            range.end.as_nanos()
89        );
90    }
91
92    // Demonstrate velocity computation
93    if let Some(latest) = world_to_robot_buffer.get_latest_transform()
94        && let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1000))
95        && let Some(velocity) = latest.compute_velocity(closest)
96    {
97        println!("\nVelocity computation:");
98        println!(
99            "  Linear velocity: [{}, {}, {}]",
100            velocity.linear[0], velocity.linear[1], velocity.linear[2]
101        );
102    }
103
104    // Demonstrate the stringly typed version of the API.
105    println!("\n\nConstant-Size Buffer Demo");
106    println!("===========================================");
107
108    let mut const_buffer: ConstTransformBuffer<f32, 5> = ConstTransformBuffer::new();
109
110    // Add some stamped transforms
111    let stamped_transform = StampedTransform {
112        transform,
113        stamp: CuDuration(1000),
114        parent_frame: "world".try_into().unwrap(),
115        child_frame: "robot".try_into().unwrap(),
116    };
117
118    const_buffer.add_transform(stamped_transform);
119
120    if let Some(latest_stamped) = const_buffer.get_latest_transform() {
121        println!("Latest transform in constant buffer:");
122        println!(
123            "  From: {} to: {}",
124            latest_stamped.parent_frame, latest_stamped.child_frame
125        );
126        println!("  Time: {}", latest_stamped.stamp.as_nanos());
127        let mat = latest_stamped.transform.to_matrix();
128        println!(
129            "  Translation: [{}, {}, {}]",
130            mat[0][3], mat[1][3], mat[2][3]
131        );
132    }
133
134    println!("\nThis buffer is stack-allocated with capacity 5 - no heap allocation!");
135
136    // Demonstrate the StampedFrameTransform pattern with TransformTree
137    println!("\n\nStampedFrameTransform Pattern Demo");
138    println!("================================");
139
140    let mut tree = TransformTree::<f32>::new();
141
142    // Create a CuMsg with TransformMsg
143    let frame_transform = FrameTransform::new(
144        transform,
145        FrameIdString::from("world").expect("Frame name too long"),
146        FrameIdString::from("robot").expect("Frame name too long"),
147    );
148
149    let mut sft = StampedFrameTransform::new(Some(frame_transform));
150    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
151
152    // Add using the new API
153    tree.add_transform(&sft).expect("Failed to add transform");
154    println!("Added transform using CuMsg<TransformMsg> pattern");
155
156    // Query the transform
157    let robot_clock = cu29::clock::RobotClock::default();
158    let result = tree.lookup_transform("world", "robot", CuDuration(1_000_000_000), &robot_clock);
159
160    match result {
161        Ok(transform) => {
162            let mat = transform.to_matrix();
163            println!(
164                "Retrieved transform: translation=({:.2}, {:.2}, {:.2})",
165                mat[3][0], mat[3][1], mat[3][2]
166            );
167        }
168        Err(e) => println!("Error: {e}"),
169    }
170}

Source

pub fn lookup_velocity( &self, from_frame: &str, to_frame: &str, time: CuTime, robot_clock: &RobotClock, ) -> TransformResult<VelocityTransform<T>>

Look up the velocity of a frame at a specific time

This computes the velocity by differentiating transforms over time. Returns the velocity expressed in the target frame.

Results are automatically cached for improved performance. The cache is invalidated when new transforms are added or when cache entries expire based on their age. The cache significantly improves performance for repeated lookups of the same frames and times.

§Arguments

from_frame - The source frame
to_frame - The target frame
time - The time at which to compute the velocity

§Returns

A VelocityTransform containing linear and angular velocity components
Error if the transform is not available or cannot be computed

§Performance

The first lookup of a specific frame pair and time will compute the velocity and cache the result. Subsequent lookups will use the cached result, which is much faster. For real-time or performance-critical applications, this caching is crucial.

§Cache Management

The cache is automatically cleared when new transforms are added. You can also manually clear the cache with clear_cache() or trigger cleanup with cleanup_cache().

Examples found in repository ?

examples/payload_usage.rs (line 114)

6fn main() {
7    println!("Cu Transform - CuMsg Pattern Demo");
8    println!("=================================");
9    println!();
10
11    // Create a transform tree
12    let mut tree = TransformTree::<f32>::new();
13    let clock = RobotClock::default();
14
15    // Create transforms using the new CuMsg pattern
16    println!("Creating transforms with CuMsg...");
17
18    // World to base transform
19    let transform1 = Transform3D::from_matrix([
20        [1.0f32, 0.0, 0.0, 0.0], // Column-major: each inner array is a column
21        [0.0, 1.0, 0.0, 0.0],
22        [0.0, 0.0, 1.0, 0.0],
23        [1.0, 0.0, 0.0, 1.0], // Translation (1, 0, 0)
24    ]);
25
26    let frame_transform = FrameTransform::new(
27        transform1,
28        FrameIdString::from("world").expect("Frame name too long"),
29        FrameIdString::from("base").expect("Frame name too long"),
30    );
31    let mut sft = StampedFrameTransform::new(Some(frame_transform));
32    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
33
34    // Add to tree using the new API
35    tree.add_transform(&sft).expect("Failed to add transform");
36    println!("  Added world->base transform at t=1s");
37
38    // Base to arm transform
39    let transform2 = Transform3D::from_matrix([
40        [0.0, 1.0, 0.0, 0.0], // 90-degree rotation around Z
41        [-1.0, 0.0, 0.0, 0.0],
42        [0.0, 0.0, 1.0, 0.0],
43        [0.5, 0.0, 0.0, 1.0], // Translation (0.5, 0, 0)
44    ]);
45
46    let msg2 = FrameTransform::new(
47        transform2,
48        FrameIdString::from("base").expect("Frame name too long"),
49        FrameIdString::from("arm").expect("Frame name too long"),
50    );
51    let mut sft = StampedFrameTransform::new(Some(msg2));
52    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
53
54    tree.add_transform(&sft).expect("Failed to add transform");
55    println!("  Added base->arm transform at t=1s");
56
57    // Demonstrate using time ranges for a broadcast
58    println!("\nBroadcasting multiple transforms with time range...");
59
60    // Create multiple transforms at different times
61    let times = [
62        CuDuration(2_000_000_000), // 2 seconds
63        CuDuration(2_100_000_000), // 2.1 seconds
64        CuDuration(2_200_000_000),
65    ];
66
67    for (i, &time) in times.iter().enumerate() {
68        let x_translation = 1.0 + (i as f32) * 0.1;
69        let transform = Transform3D::from_matrix([
70            [1.0, 0.0, 0.0, 0.0],
71            [0.0, 1.0, 0.0, 0.0],
72            [0.0, 0.0, 1.0, 0.0],
73            [x_translation, 0.0, 0.0, 1.0],
74        ]);
75
76        let msg = FrameTransform::new(
77            transform,
78            FrameIdString::from("world").expect("Frame name too long"),
79            FrameIdString::from("base").expect("Frame name too long"),
80        );
81        let mut sft = StampedFrameTransform::new(Some(msg));
82
83        // For a broadcast with multiple transforms, use Range
84        if i == 0 {
85            sft.tov = Tov::Range(cu29::clock::CuTimeRange {
86                start: times[0],
87                end: *times.last().unwrap(),
88            });
89        } else {
90            sft.tov = Tov::Time(time);
91        }
92
93        tree.add_transform(&sft).expect("Failed to add transform");
94    }
95
96    println!("  Added 3 transforms with time range 2.0s - 2.2s");
97
98    // Query the tree
99    println!("\nQuerying transforms...");
100
101    let result = tree.lookup_transform("world", "arm", CuDuration(1_000_000_000), &clock);
102    match result {
103        Ok(transform) => {
104            let mat = transform.to_matrix();
105            println!(
106                "  World->arm at t=1s: translation=({:.2}, {:.2}, {:.2})",
107                mat[3][0], mat[3][1], mat[3][2]
108            );
109        }
110        Err(e) => println!("  Error: {e}"),
111    }
112
113    // Query velocity (requires transforms at multiple times)
114    let velocity = tree.lookup_velocity("world", "base", CuDuration(2_100_000_000), &clock);
115    match velocity {
116        Ok(vel) => {
117            println!(
118                "  World->base velocity at t=2.1s: linear=({:.2}, {:.2}, {:.2}) m/s",
119                vel.linear[0], vel.linear[1], vel.linear[2]
120            );
121        }
122        Err(e) => println!("  Error computing velocity: {e}"),
123    }
124
125    println!("\nKey advantages of CuMsg pattern:");
126    println!("- Integrates with Copper message system");
127    println!("- Timestamps handled by CuMsg metadata (Tov)");
128    println!("- Supports time ranges for broadcasts");
129}

Trait Implementations§

Source §

impl<T> Default for TransformTree<T>
where Transform3D<T>: Clone + HasInverse<T> + Mul<Output = Transform3D<T>>, T: Add<Output = T> + Sub<Output = T> + Mul<Output = T> + Div<Output = T> + AddAssign + SubAssign + NumCast + Copy + Debug + Default + One + Serialize + DeserializeOwned + 'static + Neg<Output = T>,

Source §

fn default() -> Self

Returns the “default value” for a type. Read more

Auto Trait Implementations§

§

impl<T> UnwindSafe for TransformTree<T>
where T: UnwindSafe,

Blanket Implementations§

Source §

impl<T> Any for T
where T: 'static + ?Sized,

Source §

fn type_id(&self) -> TypeId

Gets the TypeId of self. Read more

Source §

impl<T> Borrow<T> for T
where T: ?Sized,

Source §

fn borrow(&self) -> &T

Immutably borrows from an owned value. Read more

Source §

impl<T> BorrowMut<T> for T
where T: ?Sized,

Source §

fn borrow_mut(&mut self) -> &mut T

Mutably borrows from an owned value. Read more

Source §

impl<T> From<T> for T

Source §

fn from(t: T) -> T

Returns the argument unchanged.

Source §

impl<T, U> Into for T
where U: From<T>,

Source §

fn into(self) -> U

Calls U::from(self).

That is, this conversion is whatever the implementation of From<T> for U chooses to do.

Source §

impl<T> IntoEither for T

Source §

fn into_either(self, into_left: bool) -> Either<Self, Self>

Converts self into a Left variant of Either<Self, Self> if into_left is true. Converts self into a Right variant of Either<Self, Self> otherwise. Read more

Source §

fn into_either_with<F>(self, into_left: F) -> Either<Self, Self>
where F: FnOnce(&Self) -> bool,

Converts self into a Left variant of Either<Self, Self> if into_left(&self) returns true. Converts self into a Right variant of Either<Self, Self> otherwise. Read more

Source §