ecoord_core/
transform_edge.rs

1use crate::Error::NoTransforms;
2use crate::{
3    Error, ExtrapolationMethod, FrameId, InterpolationMethod, TimedTransform, Transform,
4    TransformId,
5};
6use chrono::{DateTime, Utc};
7
8#[derive(Debug, Clone, PartialEq)]
9pub enum TransformEdge {
10    Static(StaticTransform),
11    Dynamic(DynamicTransform),
12}
13
14impl TransformEdge {
15    pub fn at_time(&self, timestamp: DateTime<Utc>) -> Transform {
16        match self {
17            TransformEdge::Static(s) => s.transform,
18            TransformEdge::Dynamic(d) => d.interpolate(timestamp),
19        }
20    }
21
22    pub fn parent_frame_id(&self) -> &FrameId {
23        match self {
24            TransformEdge::Static(s) => &s.parent_frame_id,
25            TransformEdge::Dynamic(d) => &d.parent_frame_id,
26        }
27    }
28
29    pub fn child_frame_id(&self) -> &FrameId {
30        match self {
31            TransformEdge::Static(s) => &s.child_frame_id,
32            TransformEdge::Dynamic(d) => &d.child_frame_id,
33        }
34    }
35
36    pub fn transform_id(&self) -> TransformId {
37        match self {
38            TransformEdge::Static(s) => s.transform_id(),
39            TransformEdge::Dynamic(d) => d.transform_id(),
40        }
41    }
42}
43
44#[derive(Debug, Clone, PartialEq)]
45pub struct StaticTransform {
46    parent_frame_id: FrameId,
47    child_frame_id: FrameId,
48    pub transform: Transform,
49}
50
51impl StaticTransform {
52    pub fn new(parent_frame_id: FrameId, child_frame_id: FrameId, transform: Transform) -> Self {
53        Self {
54            parent_frame_id,
55            child_frame_id,
56            transform,
57        }
58    }
59
60    pub fn parent_frame_id(&self) -> &FrameId {
61        &self.parent_frame_id
62    }
63
64    pub fn child_frame_id(&self) -> &FrameId {
65        &self.child_frame_id
66    }
67
68    pub fn transform_id(&self) -> TransformId {
69        TransformId::new(self.parent_frame_id.clone(), self.child_frame_id.clone())
70    }
71}
72
73#[derive(Debug, Clone, PartialEq)]
74pub struct DynamicTransform {
75    parent_frame_id: FrameId,
76    child_frame_id: FrameId,
77    pub interpolation: Option<InterpolationMethod>,
78    pub extrapolation: Option<ExtrapolationMethod>,
79    pub samples: Vec<TimedTransform>,
80}
81
82impl DynamicTransform {
83    pub fn new(
84        parent_frame_id: FrameId,
85        child_frame_id: FrameId,
86        interpolation: Option<InterpolationMethod>,
87        extrapolation: Option<ExtrapolationMethod>,
88        mut samples: Vec<TimedTransform>,
89    ) -> Result<Self, Error> {
90        if samples.is_empty() {
91            return Err(NoTransforms());
92        }
93        samples.sort_by_key(|s| s.timestamp);
94
95        for window in samples.windows(2) {
96            if window[0].timestamp == window[1].timestamp {
97                return Err(Error::DuplicateTimestamp(window[1].timestamp));
98            }
99        }
100
101        Ok(Self {
102            parent_frame_id,
103            child_frame_id,
104            interpolation,
105            extrapolation,
106            samples,
107        })
108    }
109
110    pub fn parent_frame_id(&self) -> &FrameId {
111        &self.parent_frame_id
112    }
113
114    pub fn child_frame_id(&self) -> &FrameId {
115        &self.child_frame_id
116    }
117
118    pub fn transform_id(&self) -> TransformId {
119        TransformId::new(self.parent_frame_id.clone(), self.child_frame_id.clone())
120    }
121
122    pub fn sample_timestamps(&self) -> Vec<DateTime<Utc>> {
123        self.samples.iter().map(|x| x.timestamp).collect()
124    }
125
126    pub fn first_sample_time(&self) -> DateTime<Utc> {
127        self.samples
128            .first()
129            .expect("must at least have one sample")
130            .timestamp
131    }
132
133    pub fn last_sample_time(&self) -> DateTime<Utc> {
134        self.samples
135            .last()
136            .expect("must at least have one sample")
137            .timestamp
138    }
139}
140
141impl DynamicTransform {
142    pub fn interpolate(&self, timestamp: DateTime<Utc>) -> Transform {
143        debug_assert!(
144            self.samples.is_sorted_by_key(|t| t.timestamp),
145            "transforms must be sorted by timestamp"
146        );
147        debug_assert!(
148            self.samples
149                .windows(2)
150                .all(|t| t[0].timestamp != t[1].timestamp),
151            "transforms must not contain two samples with same timestamps"
152        );
153
154        if timestamp < self.first_sample_time() || self.last_sample_time() <= timestamp {
155            return match self.extrapolation.unwrap_or_default() {
156                ExtrapolationMethod::Constant => {
157                    crate::utils::transforms_interpolation::extrapolate_constant(
158                        &self.samples,
159                        &timestamp,
160                    )
161                }
162
163                ExtrapolationMethod::Linear => {
164                    crate::utils::transforms_interpolation::extrapolate_linear(
165                        &self.samples,
166                        &timestamp,
167                    )
168                }
169            };
170        }
171
172        match self.interpolation.unwrap_or_default() {
173            InterpolationMethod::Step => {
174                crate::utils::transforms_interpolation::interpolate_step_function(
175                    &self.samples,
176                    &timestamp,
177                )
178            }
179            InterpolationMethod::Linear => {
180                crate::utils::transforms_interpolation::interpolate_linearly(
181                    &self.samples,
182                    &timestamp,
183                )
184            }
185        }
186    }
187
188    pub fn filter_samples_by_time(
189        &mut self,
190        start_time: Option<DateTime<Utc>>,
191        end_time: Option<DateTime<Utc>>,
192    ) -> Result<(), Error> {
193        let filtered_samples: Vec<TimedTransform> = self
194            .samples
195            .iter()
196            .filter(|t| start_time.is_none_or(|x| x <= t.timestamp))
197            .filter(|t| end_time.is_none_or(|x| t.timestamp < x))
198            .copied()
199            .collect();
200        if filtered_samples.is_empty() {
201            return Err(NoTransforms());
202        }
203
204        self.samples = filtered_samples;
205        Ok(())
206    }
207}
ecoord_core/transform_edge.rs

ecoord_core/
transform_edge.rs
