payload_usage/
payload_usage.rs

1use cu29::clock::{CuDuration, RobotClock, Tov};
2use cu_spatial_payloads::Transform3D;
3use cu_transform::transform_payload::StampedFrameTransform;
4use cu_transform::{FrameIdString, FrameTransform, TransformTree};
5
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}
payload_usage/payload_usage.rs

payload_usage/
payload_usage.rs
