basic_usage/
basic_usage.rs

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

basic_usage/
basic_usage.rs
