cu_aligner/
lib.rs

1#![doc = include_str!("../README.md")]
2
3pub mod buffers;
4
5/// Define a task that aligns incoming messages based on their timestamps
6/// See module doc for use.
7#[macro_export]
8macro_rules! define_task {
9    ($name:ident, $($index:tt => { $mis:expr, $mos:expr, $p:ty }),+) => {
10
11       paste::paste! {
12            #[allow(unused_imports)]
13            use cu29::prelude::*;
14
15            $crate::buffers::alignment_buffers!(
16                AlignmentBuffers,
17                $(
18                    [<buffer $index>]: TimeboundCircularBuffer<$mis, CuStampedData<$p, CuMsgMetadata>>
19                ),*
20            );
21        }
22
23        #[derive(Reflect)]
24        #[reflect(from_reflect = false)]
25        pub struct $name {
26            #[reflect(ignore)]
27            aligner: AlignmentBuffers,
28        }
29
30        impl Freezable for $name {
31            fn freeze<E: cu29::bincode::enc::Encoder>(&self, encoder: &mut E) -> Result<(), cu29::bincode::error::EncodeError> {
32                self.aligner.freeze(encoder)
33            }
34
35            fn thaw<D: cu29::bincode::de::Decoder>(&mut self, decoder: &mut D) -> Result<(), cu29::bincode::error::DecodeError> {
36                self.aligner.thaw(decoder)
37            }
38        }
39
40        impl CuTask for $name {
41    type Resources<'r> = ();
42            type Input<'m> = input_msg!('m, $($p),*);
43            type Output<'m> = output_msg!(($(
44                CuArray<$p, { $mos }>
45            ),*));
46
47            fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
48            where
49                Self: Sized,
50            {
51                let config = config.ok_or_else(|| CuError::from("Config Missing"))?;
52                let target_alignment_window_ms: u64 = config
53                    .get::<u32>("target_alignment_window_ms")?
54                    .ok_or_else(|| CuError::from("Missing target_alignment_window"))?
55                    .into();
56                let stale_data_horizon_ms: u64 = config
57                    .get::<u32>("stale_data_horizon_ms")?
58                    .ok_or_else(|| CuError::from("Missing stale_data_horizon"))?
59                    .into();
60
61                let target_alignment_window =
62                    cu29_clock::CuDuration(target_alignment_window_ms * 1_000_000);
63                let stale_data_horizon =
64                    cu29_clock::CuDuration(stale_data_horizon_ms * 1_000_000);
65
66                Ok(Self {
67                    aligner: AlignmentBuffers::new(target_alignment_window, stale_data_horizon),
68                })
69            }
70
71            fn preprocess(&mut self, ctx: &CuContext) -> CuResult<()> {
72                self.aligner.purge(ctx.now());
73                Ok(())
74            }
75
76            fn process(
77                &mut self,
78                _ctx: &CuContext,
79                input: &Self::Input<'_>,
80                output: &mut Self::Output<'_>,
81            ) -> CuResult<()> {
82                // add the incoming data into the buffers
83                // input is a tuple of &CuMsg<T> for each T in the input
84                paste::paste! {
85                    $(
86                        self.aligner.[<buffer $index>].push(input.$index.clone());
87                    )*
88                }
89
90
91                // this is a tuple of iterators of CuStampedDataSet
92                let Some(tuple_of_iters) = self.aligner.get_latest_aligned_data() else {
93                    return Ok(());
94                };
95
96                // Populate the CuArray fields in the output message
97                let output_payload = output.payload_mut().get_or_insert_with(Default::default);
98                $(
99                    output_payload.$index.fill_from_iter(tuple_of_iters.$index.map(|msg| msg.payload().unwrap().clone()));
100                )*
101                Ok(())
102            }
103        }
104    };
105}
106
107#[cfg(test)]
108mod tests {
109    use cu29::prelude::*;
110
111    define_task!(AlignerTask, 0 => { 10, 5, f32 }, 1 => { 5, 10, i32 });
112    #[test]
113    fn test_aligner_smoketest() {
114        let mut config = ComponentConfig::default();
115        config.set("target_alignment_window_ms", 1000);
116        config.set("stale_data_horizon_ms", 2000);
117        let mut aligner = AlignerTask::new(Some(&config), ()).unwrap();
118        let m1 = CuStampedData::<f32, CuMsgMetadata>::default();
119        let m2 = CuStampedData::<i32, CuMsgMetadata>::default();
120        let input: <AlignerTask as CuTask>::Input<'_> = (&m1, &m2);
121        let m3 = CuStampedData::<(CuArray<f32, 5>, CuArray<i32, 10>), CuMsgMetadata>::default();
122        let mut output: <AlignerTask as CuTask>::Output<'_> = m3;
123
124        let ctx = CuContext::new_with_clock();
125        let result = aligner.process(&ctx, &input, &mut output);
126        assert!(result.is_ok());
127    }
128    mod string_payload {
129        use super::*;
130
131        define_task!(StringAlignerTask, 0 => { 4, 4, String }, 1 => { 4, 4, String });
132
133        #[test]
134        fn test_aligner_string_payload() {
135            let mut config = ComponentConfig::default();
136            config.set("target_alignment_window_ms", 10);
137            config.set("stale_data_horizon_ms", 1000);
138            let mut aligner = StringAlignerTask::new(Some(&config), ()).unwrap();
139
140            let mut left = CuStampedData::<String, CuMsgMetadata>::new(Some("left".to_string()));
141            let mut right = CuStampedData::<String, CuMsgMetadata>::new(Some("right".to_string()));
142            let tov_time = CuTime::from_millis(100);
143            left.tov = Tov::Time(tov_time);
144            right.tov = Tov::Time(tov_time);
145
146            let input: <StringAlignerTask as CuTask>::Input<'_> = (&left, &right);
147            let mut output =
148                CuStampedData::<(CuArray<String, 4>, CuArray<String, 4>), CuMsgMetadata>::default();
149
150            let ctx = CuContext::new_with_clock();
151            aligner.process(&ctx, &input, &mut output).unwrap();
152
153            let payload = output.payload().unwrap();
154            assert_eq!(payload.0.len(), 1);
155            assert_eq!(payload.1.len(), 1);
156            assert_eq!(payload.0.as_slice()[0], "left");
157            assert_eq!(payload.1.as_slice()[0], "right");
158        }
159    }
160}
cu_aligner/lib.rs

cu_aligner/
lib.rs
