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 + '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 141)

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        if let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1000)) {
95            if let Some(velocity) = latest.compute_velocity(closest) {
96                println!("\nVelocity computation:");
97                println!(
98                    "  Linear velocity: [{}, {}, {}]",
99                    velocity.linear[0], velocity.linear[1], velocity.linear[2]
100                );
101            }
102        }
103    }
104
105    // Demonstrate the stringly typed version of the API.
106    println!("\n\nConstant-Size Buffer Demo");
107    println!("===========================================");
108
109    let mut const_buffer: ConstTransformBuffer<f32, 5> = ConstTransformBuffer::new();
110
111    // Add some stamped transforms
112    let stamped_transform = StampedTransform {
113        transform,
114        stamp: CuDuration(1000),
115        parent_frame: "world".try_into().unwrap(),
116        child_frame: "robot".try_into().unwrap(),
117    };
118
119    const_buffer.add_transform(stamped_transform);
120
121    if let Some(latest_stamped) = const_buffer.get_latest_transform() {
122        println!("Latest transform in constant buffer:");
123        println!(
124            "  From: {} to: {}",
125            latest_stamped.parent_frame, latest_stamped.child_frame
126        );
127        println!("  Time: {}", latest_stamped.stamp.as_nanos());
128        let mat = latest_stamped.transform.to_matrix();
129        println!(
130            "  Translation: [{}, {}, {}]",
131            mat[0][3], mat[1][3], mat[2][3]
132        );
133    }
134
135    println!("\nThis buffer is stack-allocated with capacity 5 - no heap allocation!");
136
137    // Demonstrate the StampedFrameTransform pattern with TransformTree
138    println!("\n\nStampedFrameTransform Pattern Demo");
139    println!("================================");
140
141    let mut tree = TransformTree::<f32>::new();
142
143    // Create a CuMsg with TransformMsg
144    let frame_transform = FrameTransform::new(
145        transform,
146        FrameIdString::from("world").expect("Frame name too long"),
147        FrameIdString::from("robot").expect("Frame name too long"),
148    );
149
150    let mut sft = StampedFrameTransform::new(Some(frame_transform));
151    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
152
153    // Add using the new API
154    tree.add_transform(&sft).expect("Failed to add transform");
155    println!("Added transform using CuMsg<TransformMsg> pattern");
156
157    // Query the transform
158    let robot_clock = cu29::clock::RobotClock::default();
159    let result = tree.lookup_transform("world", "robot", CuDuration(1_000_000_000), &robot_clock);
160
161    match result {
162        Ok(transform) => {
163            let mat = transform.to_matrix();
164            println!(
165                "Retrieved transform: translation=({:.2}, {:.2}, {:.2})",
166                mat[3][0], mat[3][1], mat[3][2]
167            );
168        }
169        Err(e) => println!("Error: {e}"),
170    }
171}

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 154)

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        if let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1000)) {
95            if let Some(velocity) = latest.compute_velocity(closest) {
96                println!("\nVelocity computation:");
97                println!(
98                    "  Linear velocity: [{}, {}, {}]",
99                    velocity.linear[0], velocity.linear[1], velocity.linear[2]
100                );
101            }
102        }
103    }
104
105    // Demonstrate the stringly typed version of the API.
106    println!("\n\nConstant-Size Buffer Demo");
107    println!("===========================================");
108
109    let mut const_buffer: ConstTransformBuffer<f32, 5> = ConstTransformBuffer::new();
110
111    // Add some stamped transforms
112    let stamped_transform = StampedTransform {
113        transform,
114        stamp: CuDuration(1000),
115        parent_frame: "world".try_into().unwrap(),
116        child_frame: "robot".try_into().unwrap(),
117    };
118
119    const_buffer.add_transform(stamped_transform);
120
121    if let Some(latest_stamped) = const_buffer.get_latest_transform() {
122        println!("Latest transform in constant buffer:");
123        println!(
124            "  From: {} to: {}",
125            latest_stamped.parent_frame, latest_stamped.child_frame
126        );
127        println!("  Time: {}", latest_stamped.stamp.as_nanos());
128        let mat = latest_stamped.transform.to_matrix();
129        println!(
130            "  Translation: [{}, {}, {}]",
131            mat[0][3], mat[1][3], mat[2][3]
132        );
133    }
134
135    println!("\nThis buffer is stack-allocated with capacity 5 - no heap allocation!");
136
137    // Demonstrate the StampedFrameTransform pattern with TransformTree
138    println!("\n\nStampedFrameTransform Pattern Demo");
139    println!("================================");
140
141    let mut tree = TransformTree::<f32>::new();
142
143    // Create a CuMsg with TransformMsg
144    let frame_transform = FrameTransform::new(
145        transform,
146        FrameIdString::from("world").expect("Frame name too long"),
147        FrameIdString::from("robot").expect("Frame name too long"),
148    );
149
150    let mut sft = StampedFrameTransform::new(Some(frame_transform));
151    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
152
153    // Add using the new API
154    tree.add_transform(&sft).expect("Failed to add transform");
155    println!("Added transform using CuMsg<TransformMsg> pattern");
156
157    // Query the transform
158    let robot_clock = cu29::clock::RobotClock::default();
159    let result = tree.lookup_transform("world", "robot", CuDuration(1_000_000_000), &robot_clock);
160
161    match result {
162        Ok(transform) => {
163            let mat = transform.to_matrix();
164            println!(
165                "Retrieved transform: translation=({:.2}, {:.2}, {:.2})",
166                mat[3][0], mat[3][1], mat[3][2]
167            );
168        }
169        Err(e) => println!("Error: {e}"),
170    }
171}

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 159)

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        if let Some(closest) = world_to_robot_buffer.get_closest_transform(CuDuration(1000)) {
95            if let Some(velocity) = latest.compute_velocity(closest) {
96                println!("\nVelocity computation:");
97                println!(
98                    "  Linear velocity: [{}, {}, {}]",
99                    velocity.linear[0], velocity.linear[1], velocity.linear[2]
100                );
101            }
102        }
103    }
104
105    // Demonstrate the stringly typed version of the API.
106    println!("\n\nConstant-Size Buffer Demo");
107    println!("===========================================");
108
109    let mut const_buffer: ConstTransformBuffer<f32, 5> = ConstTransformBuffer::new();
110
111    // Add some stamped transforms
112    let stamped_transform = StampedTransform {
113        transform,
114        stamp: CuDuration(1000),
115        parent_frame: "world".try_into().unwrap(),
116        child_frame: "robot".try_into().unwrap(),
117    };
118
119    const_buffer.add_transform(stamped_transform);
120
121    if let Some(latest_stamped) = const_buffer.get_latest_transform() {
122        println!("Latest transform in constant buffer:");
123        println!(
124            "  From: {} to: {}",
125            latest_stamped.parent_frame, latest_stamped.child_frame
126        );
127        println!("  Time: {}", latest_stamped.stamp.as_nanos());
128        let mat = latest_stamped.transform.to_matrix();
129        println!(
130            "  Translation: [{}, {}, {}]",
131            mat[0][3], mat[1][3], mat[2][3]
132        );
133    }
134
135    println!("\nThis buffer is stack-allocated with capacity 5 - no heap allocation!");
136
137    // Demonstrate the StampedFrameTransform pattern with TransformTree
138    println!("\n\nStampedFrameTransform Pattern Demo");
139    println!("================================");
140
141    let mut tree = TransformTree::<f32>::new();
142
143    // Create a CuMsg with TransformMsg
144    let frame_transform = FrameTransform::new(
145        transform,
146        FrameIdString::from("world").expect("Frame name too long"),
147        FrameIdString::from("robot").expect("Frame name too long"),
148    );
149
150    let mut sft = StampedFrameTransform::new(Some(frame_transform));
151    sft.tov = Tov::Time(CuDuration(1_000_000_000)); // 1 second
152
153    // Add using the new API
154    tree.add_transform(&sft).expect("Failed to add transform");
155    println!("Added transform using CuMsg<TransformMsg> pattern");
156
157    // Query the transform
158    let robot_clock = cu29::clock::RobotClock::default();
159    let result = tree.lookup_transform("world", "robot", CuDuration(1_000_000_000), &robot_clock);
160
161    match result {
162        Ok(transform) => {
163            let mat = transform.to_matrix();
164            println!(
165                "Retrieved transform: translation=({:.2}, {:.2}, {:.2})",
166                mat[3][0], mat[3][1], mat[3][2]
167            );
168        }
169        Err(e) => println!("Error: {e}"),
170    }
171}

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 + '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 §