basic_usage/
basic_usage.rs

1use cu_spatial_payloads::Transform3D;
2use cu_transform::transform_payload::StampedFrameTransform;
3use cu_transform::{
4    ConstTransformBuffer, FrameIdString, FrameTransform, RobotFrame, StampedTransform,
5    TransformTree, TypedTransform, TypedTransformBuffer, WorldFrame,
6};
7use cu29::clock::{CuDuration, Tov};
8
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}
basic_usage/basic_usage.rs

basic_usage/
basic_usage.rs
