tf_r2r/
tf_buffer.rs

Help
1use std::collections::{hash_map::Entry, HashMap, HashSet, VecDeque};
2
3use r2r::{
4    builtin_interfaces::msg::{Duration, Time},
5    geometry_msgs::msg::{Transform, TransformStamped},
6    std_msgs::msg::Header,
7    tf2_msgs::msg::TFMessage,
8};
9
10use crate::{
11    tf_error::TfError,
12    tf_graph_node::TfGraphNode,
13    tf_individual_transform_chain::TfIndividualTransformChain,
14    transforms::{chain_transforms, get_inverse, to_transform_stamped},
15};
16
17#[derive(Clone, Debug)]
18pub struct TfBuffer {
19    child_transform_index: HashMap<String, HashSet<String>>,
20    transform_data: HashMap<TfGraphNode, TfIndividualTransformChain>,
21    cache_duration: Duration,
22}
23
24const DEFAULT_CACHE_DURATION_SECONDS: i32 = 10;
25
26impl TfBuffer {
27    pub(crate) fn new() -> Self {
28        Self::new_with_duration(Duration {
29            sec: DEFAULT_CACHE_DURATION_SECONDS,
30            nanosec: 0,
31        })
32    }
33
34    pub fn new_with_duration(cache_duration: Duration) -> Self {
35        TfBuffer {
36            child_transform_index: HashMap::new(),
37            transform_data: HashMap::new(),
38            cache_duration,
39        }
40    }
41
42    pub(crate) fn handle_incoming_transforms(&mut self, transforms: TFMessage, static_tf: bool) {
43        for transform in transforms.transforms {
44            self.add_transform(&transform, static_tf);
45            self.add_transform(&get_inverse(&transform), static_tf);
46        }
47    }
48
49    fn add_transform(&mut self, transform: &TransformStamped, static_tf: bool) {
50        //TODO: Detect is new transform will create a loop
51        self.child_transform_index
52            .entry(transform.header.frame_id.clone())
53            .or_default()
54            .insert(transform.child_frame_id.clone());
55
56        let key = TfGraphNode {
57            child: transform.child_frame_id.clone(),
58            parent: transform.header.frame_id.clone(),
59        };
60
61        match self.transform_data.entry(key) {
62            Entry::Occupied(e) => e.into_mut(),
63            Entry::Vacant(e) => e.insert(TfIndividualTransformChain::new(
64                static_tf,
65                self.cache_duration.clone(),
66            )),
67        }
68        .add_to_buffer(transform.clone());
69    }
70
71    /// Retrieves the transform path
72    fn retrieve_transform_path(
73        &self,
74        from: String,
75        to: String,
76        time: &Time,
77    ) -> Result<Vec<String>, TfError> {
78        let mut res = vec![];
79        let mut frontier: VecDeque<String> = VecDeque::new();
80        let mut visited: HashSet<String> = HashSet::new();
81        let mut parents: HashMap<String, String> = HashMap::new();
82        visited.insert(from.clone());
83        frontier.push_front(from.clone());
84
85        while !frontier.is_empty() {
86            let current_node = frontier.pop_front().unwrap();
87            if current_node == to {
88                break;
89            }
90            if let Some(children) = self.child_transform_index.get(&current_node) {
91                for v in children {
92                    if visited.contains(v) {
93                        continue;
94                    }
95
96                    if self
97                        .transform_data
98                        .get(&TfGraphNode {
99                            child: v.clone(),
100                            parent: current_node.clone(),
101                        })
102                        .map_or(false, |chain| chain.has_valid_transform(time))
103                    {
104                        parents.insert(v.to_string(), current_node.clone());
105                        frontier.push_front(v.to_string());
106                        visited.insert(v.to_string());
107                    }
108                }
109            }
110        }
111        let mut r = to.clone();
112        while r != from {
113            res.push(r.clone());
114            let parent = parents.get(&r);
115
116            match parent {
117                Some(x) => r = x.to_string(),
118                None => {
119                    return Err(TfError::CouldNotFindTransform(
120                        from,
121                        to,
122                        self.child_transform_index.clone(),
123                    ))
124                }
125            }
126        }
127        res.reverse();
128        Ok(res)
129    }
130
131    /// Looks up a transform within the tree at a given time.
132    pub fn lookup_transform(
133        &self,
134        from: &str,
135        to: &str,
136        time: &Time,
137    ) -> Result<TransformStamped, TfError> {
138        let from = from.to_string();
139        let to = to.to_string();
140        let path = self.retrieve_transform_path(from.clone(), to.clone(), time);
141
142        match path {
143            Ok(path) => {
144                let mut tf_list: Vec<Transform> = Vec::new();
145                let mut first = from.clone();
146                for intermediate in path {
147                    let node = TfGraphNode {
148                        child: intermediate.clone(),
149                        parent: first.clone(),
150                    };
151                    let time_cache = self.transform_data.get(&node).unwrap();
152                    let transform = time_cache.get_closest_transform(time);
153                    match transform {
154                        Err(e) => return Err(e),
155                        Ok(x) => {
156                            tf_list.push(x.transform);
157                        }
158                    }
159                    first.clone_from(&intermediate);
160                }
161                let final_tf = chain_transforms(&tf_list);
162                let msg = TransformStamped {
163                    child_frame_id: to,
164                    header: Header {
165                        frame_id: from,
166                        stamp: time.clone(),
167                    },
168                    transform: final_tf,
169                };
170                Ok(msg)
171            }
172            Err(x) => Err(x),
173        }
174    }
175
176    pub(crate) fn lookup_transform_with_time_travel(
177        &self,
178        to: &str,
179        time2: Time,
180        from: &str,
181        time1: Time,
182        fixed_frame: &str,
183    ) -> Result<TransformStamped, TfError> {
184        let tf1 = self.lookup_transform(from, fixed_frame, &time1)?;
185        let tf2 = self.lookup_transform(to, fixed_frame, &time2)?;
186        let transforms = get_inverse(&tf1);
187        let result = chain_transforms(&[tf2.transform, transforms.transform]);
188        Ok(to_transform_stamped(
189            result,
190            from.to_string(),
191            to.to_string(),
192            &time1,
193        ))
194    }
195}
196
197#[cfg(test)]
198mod test {
199    use r2r::{
200        builtin_interfaces::msg::Time,
201        geometry_msgs::msg::{Quaternion, Vector3},
202    };
203
204    use super::*;
205    use crate::utils::time_from_nanosec;
206
207    const PARENT: &str = "parent";
208    const CHILD0: &str = "child0";
209    const CHILD1: &str = "child1";
210
211    /// This function builds a tree consisting of the following items:
212    /// * a world coordinate frame
213    /// * an item in the world frame at (1,0,0)
214    /// * base_link of a robot starting at (0,0,0) and progressing at (0,t,0) where t is time in seconds
215    /// * a camera which is (0.5, 0, 0) from the base_link
216    fn build_test_tree(buffer: &mut TfBuffer, time: f64) {
217        let nsecs = ((time - ((time.floor() as i64) as f64)) * 1E9) as u32;
218
219        let world_to_item = TransformStamped {
220            child_frame_id: "item".to_string(),
221            header: Header {
222                frame_id: "world".to_string(),
223                stamp: Time {
224                    sec: time.floor() as i32,
225                    nanosec: nsecs,
226                },
227            },
228            transform: Transform {
229                rotation: Quaternion {
230                    x: 0f64,
231                    y: 0f64,
232                    z: 0f64,
233                    w: 1f64,
234                },
235                translation: Vector3 {
236                    x: 1f64,
237                    y: 0f64,
238                    z: 0f64,
239                },
240            },
241        };
242        buffer.add_transform(&world_to_item, true);
243        buffer.add_transform(&get_inverse(&world_to_item), true);
244
245        let world_to_base_link = TransformStamped {
246            child_frame_id: "base_link".to_string(),
247            header: Header {
248                frame_id: "world".to_string(),
249                stamp: Time {
250                    sec: time.floor() as i32,
251                    nanosec: nsecs,
252                },
253            },
254            transform: Transform {
255                rotation: Quaternion {
256                    x: 0f64,
257                    y: 0f64,
258                    z: 0f64,
259                    w: 1f64,
260                },
261                translation: Vector3 {
262                    x: 0f64,
263                    y: time,
264                    z: 0f64,
265                },
266            },
267        };
268        buffer.add_transform(&world_to_base_link, false);
269        buffer.add_transform(&get_inverse(&world_to_base_link), false);
270
271        let base_link_to_camera = TransformStamped {
272            child_frame_id: "camera".to_string(),
273            header: Header {
274                frame_id: "base_link".to_string(),
275                stamp: Time {
276                    sec: time.floor() as i32,
277                    nanosec: nsecs,
278                },
279            },
280            transform: Transform {
281                rotation: Quaternion {
282                    x: 0f64,
283                    y: 0f64,
284                    z: 0f64,
285                    w: 1f64,
286                },
287                translation: Vector3 {
288                    x: 0.5f64,
289                    y: 0f64,
290                    z: 0f64,
291                },
292            },
293        };
294        buffer.add_transform(&base_link_to_camera, true);
295        buffer.add_transform(&get_inverse(&base_link_to_camera), true);
296    }
297
298    /// Tests a basic lookup
299    #[test]
300    fn test_basic_tf_lookup() {
301        let mut tf_buffer = TfBuffer::new();
302        build_test_tree(&mut tf_buffer, 0f64);
303        let res = tf_buffer.lookup_transform("camera", "item", &time_from_nanosec(0));
304        let expected = TransformStamped {
305            child_frame_id: "item".to_string(),
306            header: Header {
307                frame_id: "camera".to_string(),
308                stamp: time_from_nanosec(0),
309            },
310            transform: Transform {
311                rotation: Quaternion {
312                    x: 0f64,
313                    y: 0f64,
314                    z: 0f64,
315                    w: 1f64,
316                },
317                translation: Vector3 {
318                    x: 0.5f64,
319                    y: 0f64,
320                    z: 0f64,
321                },
322            },
323        };
324        assert_eq!(res.unwrap(), expected);
325    }
326
327    /// Tests an interpolated lookup.
328    #[test]
329    fn test_basic_tf_interpolation() {
330        let mut tf_buffer = TfBuffer::new();
331        build_test_tree(&mut tf_buffer, 0f64);
332        build_test_tree(&mut tf_buffer, 1f64);
333        let res = tf_buffer.lookup_transform("camera", "item", &time_from_nanosec(700_000_000));
334        let expected = TransformStamped {
335            child_frame_id: "item".to_string(),
336            header: Header {
337                frame_id: "camera".to_string(),
338                stamp: time_from_nanosec(700_000_000),
339            },
340            transform: Transform {
341                rotation: Quaternion {
342                    x: 0f64,
343                    y: 0f64,
344                    z: 0f64,
345                    w: 1f64,
346                },
347                translation: Vector3 {
348                    x: 0.5f64,
349                    y: -0.7f64,
350                    z: 0f64,
351                },
352            },
353        };
354        assert_eq!(res.unwrap(), expected);
355    }
356
357    /// Tests an interpolated lookup.
358    #[test]
359    fn test_basic_tf_time_travel() {
360        let mut tf_buffer = TfBuffer::new();
361        build_test_tree(&mut tf_buffer, 0f64);
362        build_test_tree(&mut tf_buffer, 1f64);
363        let res = tf_buffer.lookup_transform_with_time_travel(
364            "camera",
365            time_from_nanosec(400_000_000),
366            "camera",
367            time_from_nanosec(700_000_000),
368            "item",
369        );
370        let expected = TransformStamped {
371            child_frame_id: "camera".to_string(),
372            header: Header {
373                frame_id: "camera".to_string(),
374                stamp: time_from_nanosec(700_000_000),
375            },
376            transform: Transform {
377                rotation: Quaternion {
378                    x: 0f64,
379                    y: 0f64,
380                    z: 0f64,
381                    w: 1f64,
382                },
383                translation: Vector3 {
384                    x: 0f64,
385                    y: 0.3f64,
386                    z: 0f64,
387                },
388            },
389        };
390        assert_approx_eq(res.unwrap(), expected);
391    }
392
393    #[test]
394    fn test_add_transform() {
395        let mut tf_buffer = TfBuffer::new();
396        let transform00 = TransformStamped {
397            header: Header {
398                frame_id: PARENT.to_string(),
399                stamp: time_from_nanosec(0),
400            },
401            child_frame_id: CHILD0.to_string(),
402            ..Default::default()
403        };
404        let transform01 = TransformStamped {
405            header: Header {
406                frame_id: PARENT.to_string(),
407                stamp: Time { sec: 1, nanosec: 0 },
408            },
409            child_frame_id: CHILD0.to_string(),
410            ..Default::default()
411        };
412        let transform1 = TransformStamped {
413            header: Header {
414                frame_id: PARENT.to_string(),
415                ..Default::default()
416            },
417            child_frame_id: CHILD1.to_string(),
418            ..Default::default()
419        };
420        let transform0_key = TfGraphNode {
421            child: CHILD0.to_owned(),
422            parent: PARENT.to_owned(),
423        };
424        let transform1_key = TfGraphNode {
425            child: CHILD1.to_owned(),
426            parent: PARENT.to_owned(),
427        };
428        let static_tf = true;
429        tf_buffer.add_transform(&transform00, static_tf);
430        assert_eq!(tf_buffer.child_transform_index.len(), 1);
431        assert!(tf_buffer.child_transform_index.contains_key(PARENT));
432        let children = tf_buffer.child_transform_index.get(PARENT).unwrap();
433        assert_eq!(children.len(), 1);
434        assert!(children.contains(CHILD0));
435        assert_eq!(tf_buffer.transform_data.len(), 1);
436        assert!(tf_buffer.transform_data.contains_key(&transform0_key));
437        let data = tf_buffer.transform_data.get(&transform0_key);
438        assert!(data.is_some());
439        assert_eq!(data.unwrap().transform_chain.len(), 1);
440
441        tf_buffer.add_transform(&transform01, static_tf);
442        assert_eq!(tf_buffer.child_transform_index.len(), 1);
443        assert!(tf_buffer.child_transform_index.contains_key(PARENT));
444        let children = tf_buffer.child_transform_index.get(PARENT).unwrap();
445        assert_eq!(children.len(), 1);
446        assert!(children.contains(CHILD0));
447        assert_eq!(tf_buffer.transform_data.len(), 1);
448        assert!(tf_buffer.transform_data.contains_key(&transform0_key));
449        let data = tf_buffer.transform_data.get(&transform0_key);
450        assert!(data.is_some());
451        assert_eq!(data.unwrap().transform_chain.len(), 2);
452
453        tf_buffer.add_transform(&transform1, static_tf);
454        assert_eq!(tf_buffer.child_transform_index.len(), 1);
455        assert!(tf_buffer.child_transform_index.contains_key(PARENT));
456        let children = tf_buffer.child_transform_index.get(PARENT).unwrap();
457        assert_eq!(children.len(), 2);
458        assert!(children.contains(CHILD0));
459        assert!(children.contains(CHILD1));
460        assert_eq!(tf_buffer.transform_data.len(), 2);
461        assert!(tf_buffer.transform_data.contains_key(&transform0_key));
462        assert!(tf_buffer.transform_data.contains_key(&transform1_key));
463        let data = tf_buffer.transform_data.get(&transform0_key);
464        assert!(data.is_some());
465        assert_eq!(data.unwrap().transform_chain.len(), 2);
466        let data = tf_buffer.transform_data.get(&transform1_key);
467        assert!(data.is_some());
468        assert_eq!(data.unwrap().transform_chain.len(), 1);
469    }
470
471    #[test]
472    fn test_cache_duration() {
473        let mut tf_buffer = TfBuffer::new_with_duration(Duration { sec: 1, nanosec: 0 });
474        let transform00 = TransformStamped {
475            header: Header {
476                frame_id: PARENT.to_string(),
477                stamp: time_from_nanosec(0),
478            },
479            child_frame_id: CHILD0.to_string(),
480            ..Default::default()
481        };
482        let transform01 = TransformStamped {
483            header: Header {
484                frame_id: PARENT.to_string(),
485                stamp: Time { sec: 1, nanosec: 0 },
486            },
487            child_frame_id: CHILD0.to_string(),
488            ..Default::default()
489        };
490        let transform02 = TransformStamped {
491            header: Header {
492                frame_id: PARENT.to_string(),
493                stamp: Time { sec: 2, nanosec: 0 },
494            },
495            child_frame_id: CHILD0.to_string(),
496            ..Default::default()
497        };
498        let transform0_key = TfGraphNode {
499            child: CHILD0.to_owned(),
500            parent: PARENT.to_owned(),
501        };
502
503        let static_tf = true;
504        tf_buffer.add_transform(&transform00, static_tf);
505        assert_eq!(tf_buffer.child_transform_index.len(), 1);
506        assert_eq!(tf_buffer.transform_data.len(), 1);
507        assert!(tf_buffer.transform_data.contains_key(&transform0_key));
508        let data = tf_buffer.transform_data.get(&transform0_key);
509        assert!(data.is_some());
510        assert_eq!(data.unwrap().transform_chain.len(), 1);
511        assert_eq!(
512            data.unwrap().transform_chain[0].header.stamp,
513            time_from_nanosec(0)
514        );
515
516        tf_buffer.add_transform(&transform01, static_tf);
517        assert_eq!(tf_buffer.child_transform_index.len(), 1);
518        assert_eq!(tf_buffer.transform_data.len(), 1);
519        assert!(tf_buffer.transform_data.contains_key(&transform0_key));
520        let data = tf_buffer.transform_data.get(&transform0_key);
521        assert!(data.is_some());
522        assert_eq!(data.unwrap().transform_chain.len(), 2);
523        assert_eq!(
524            data.unwrap().transform_chain[0].header.stamp,
525            time_from_nanosec(0)
526        );
527        assert_eq!(
528            data.unwrap().transform_chain[1].header.stamp,
529            time_from_nanosec(1_000_000_000)
530        );
531
532        tf_buffer.add_transform(&transform02, static_tf);
533        assert_eq!(tf_buffer.child_transform_index.len(), 1);
534        assert_eq!(tf_buffer.transform_data.len(), 1);
535        assert!(tf_buffer.transform_data.contains_key(&transform0_key));
536        let data = tf_buffer.transform_data.get(&transform0_key);
537        assert!(data.is_some());
538        assert_eq!(data.unwrap().transform_chain.len(), 2);
539        assert_eq!(
540            data.unwrap().transform_chain[0].header.stamp,
541            time_from_nanosec(1_000_000_000)
542        );
543        assert_eq!(
544            data.unwrap().transform_chain[1].header.stamp,
545            time_from_nanosec(2_000_000_000)
546        );
547    }
548
549    fn assert_approx_eq(msg1: TransformStamped, msg2: TransformStamped) {
550        assert_eq!(msg1.header, msg2.header);
551        assert_eq!(msg1.child_frame_id, msg2.child_frame_id);
552
553        assert!((msg1.transform.rotation.x - msg2.transform.rotation.x).abs() < 1e-9);
554        assert!((msg1.transform.rotation.y - msg2.transform.rotation.y).abs() < 1e-9);
555        assert!((msg1.transform.rotation.z - msg2.transform.rotation.z).abs() < 1e-9);
556        assert!((msg1.transform.rotation.w - msg2.transform.rotation.w).abs() < 1e-9);
557
558        assert!((msg1.transform.translation.x - msg2.transform.translation.x).abs() < 1e-9);
559        assert!((msg1.transform.translation.y - msg2.transform.translation.y).abs() < 1e-9);
560        assert!((msg1.transform.translation.z - msg2.transform.translation.z).abs() < 1e-9);
561    }
562
563    /// Tests a case in which the tree structure changes dynamically
564    /// time 1-2(sec): [base] -> [camera1] -> [marker] -> [target]
565    /// time 3-4(sec): [base] -> [camera2] -> [marker] -> [target]
566    /// time 5-6(sec): [base] -> [camera1] -> [marker] -> [target]
567    #[test]
568    fn test_dynamic_tree() {
569        let mut tf_buffer = TfBuffer::new();
570
571        let base_to_camera1 = TransformStamped {
572            child_frame_id: "camera1".to_string(),
573            header: Header {
574                frame_id: "base".to_string(),
575                stamp: Time { sec: 1, nanosec: 0 },
576            },
577            transform: Transform {
578                rotation: Quaternion {
579                    x: 0.0,
580                    y: 0.0,
581                    z: 0.0,
582                    w: 1.0,
583                },
584                translation: Vector3 {
585                    x: 1.0,
586                    y: 0.0,
587                    z: 0.0,
588                },
589            },
590        };
591        tf_buffer.add_transform(&base_to_camera1, true);
592        tf_buffer.add_transform(&get_inverse(&base_to_camera1), true);
593
594        let base_to_camera2 = TransformStamped {
595            child_frame_id: "camera2".to_string(),
596            header: Header {
597                frame_id: "base".to_string(),
598                stamp: Time { sec: 1, nanosec: 0 },
599            },
600            transform: Transform {
601                rotation: Quaternion {
602                    x: 0.0,
603                    y: 0.0,
604                    z: 1.0,
605                    w: 0.0,
606                },
607                translation: Vector3 {
608                    x: -1.0,
609                    y: 0.0,
610                    z: 0.0,
611                },
612            },
613        };
614        tf_buffer.add_transform(&base_to_camera2, true);
615        tf_buffer.add_transform(&get_inverse(&base_to_camera2), true);
616
617        let marker_to_target = TransformStamped {
618            child_frame_id: "target".to_string(),
619            header: Header {
620                frame_id: "marker".to_string(),
621                stamp: Time { sec: 1, nanosec: 0 },
622            },
623            transform: Transform {
624                rotation: Quaternion {
625                    x: 0.0,
626                    y: 0.0,
627                    z: 0.0,
628                    w: 1.0,
629                },
630                translation: Vector3 {
631                    x: -0.5,
632                    y: 0.0,
633                    z: 0.0,
634                },
635            },
636        };
637        tf_buffer.add_transform(&marker_to_target, true);
638        tf_buffer.add_transform(&get_inverse(&marker_to_target), true);
639
640        let mut camera1_to_marker = TransformStamped {
641            child_frame_id: "marker".to_string(),
642            header: Header {
643                frame_id: "camera1".to_string(),
644                stamp: Time { sec: 1, nanosec: 0 },
645            },
646            transform: Transform {
647                rotation: Quaternion {
648                    x: 0.0,
649                    y: 0.0,
650                    z: 0.0,
651                    w: 1.0,
652                },
653                translation: Vector3 {
654                    x: 1.0,
655                    y: 1.0,
656                    z: 0.0,
657                },
658            },
659        };
660        tf_buffer.add_transform(&camera1_to_marker, false);
661        tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false);
662
663        camera1_to_marker.header.stamp.sec = 2;
664        camera1_to_marker.transform.translation.y = -1.0;
665        tf_buffer.add_transform(&camera1_to_marker, false);
666        tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false);
667
668        let mut camera2_to_marker = TransformStamped {
669            child_frame_id: "marker".to_string(),
670            header: Header {
671                frame_id: "camera2".to_string(),
672                stamp: Time { sec: 3, nanosec: 0 },
673            },
674            transform: Transform {
675                rotation: Quaternion {
676                    x: 0.0,
677                    y: 0.0,
678                    z: 0.0,
679                    w: 1.0,
680                },
681                translation: Vector3 {
682                    x: 1.0,
683                    y: 1.0,
684                    z: 0.0,
685                },
686            },
687        };
688        tf_buffer.add_transform(&camera2_to_marker, false);
689        tf_buffer.add_transform(&get_inverse(&camera2_to_marker), false);
690
691        camera2_to_marker.header.stamp.sec = 4;
692        camera2_to_marker.transform.translation.y = -1.0;
693        tf_buffer.add_transform(&camera2_to_marker, false);
694        tf_buffer.add_transform(&get_inverse(&camera2_to_marker), false);
695
696        let result = tf_buffer.lookup_transform("base", "target", &Time { sec: 1, nanosec: 0 });
697        assert_eq!(
698            result.unwrap().transform.translation,
699            Vector3 {
700                x: 1.5,
701                y: 1.0,
702                z: 0.0
703            }
704        );
705
706        let result = tf_buffer.lookup_transform(
707            "base",
708            "target",
709            &Time {
710                sec: 1,
711                nanosec: 500_000_000,
712            },
713        );
714        assert_eq!(
715            result.unwrap().transform.translation,
716            Vector3 {
717                x: 1.5,
718                y: 0.0,
719                z: 0.0
720            }
721        );
722
723        let result = tf_buffer.lookup_transform("base", "target", &Time { sec: 2, nanosec: 0 });
724        assert_eq!(
725            result.unwrap().transform.translation,
726            Vector3 {
727                x: 1.5,
728                y: -1.0,
729                z: 0.0
730            }
731        );
732
733        let result = tf_buffer.lookup_transform(
734            "base",
735            "target",
736            &Time {
737                sec: 2,
738                nanosec: 500_000_000,
739            },
740        );
741        assert!(result.is_err());
742
743        let result = tf_buffer.lookup_transform("base", "target", &Time { sec: 3, nanosec: 0 });
744        assert_eq!(
745            result.unwrap().transform.translation,
746            Vector3 {
747                x: -1.5,
748                y: -1.0,
749                z: 0.0
750            }
751        );
752
753        let result = tf_buffer.lookup_transform(
754            "base",
755            "target",
756            &Time {
757                sec: 3,
758                nanosec: 500_000_000,
759            },
760        );
761        assert_eq!(
762            result.unwrap().transform.translation,
763            Vector3 {
764                x: -1.5,
765                y: -0.0,
766                z: 0.0
767            }
768        );
769
770        let result = tf_buffer.lookup_transform("base", "target", &Time { sec: 4, nanosec: 0 });
771        assert_eq!(
772            result.unwrap().transform.translation,
773            Vector3 {
774                x: -1.5,
775                y: 1.0,
776                z: 0.0
777            }
778        );
779
780        let result = tf_buffer.lookup_transform(
781            "base",
782            "target",
783            &Time {
784                sec: 4,
785                nanosec: 500_000_000,
786            },
787        );
788        assert!(result.is_err());
789
790        camera1_to_marker.header.stamp.sec = 5;
791        camera1_to_marker.transform.translation.x = 0.5;
792        camera1_to_marker.transform.translation.y = 1.0;
793        tf_buffer.add_transform(&camera1_to_marker, false);
794        tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false);
795
796        camera1_to_marker.header.stamp.sec = 6;
797        camera1_to_marker.transform.translation.y = -1.0;
798        tf_buffer.add_transform(&camera1_to_marker, false);
799        tf_buffer.add_transform(&get_inverse(&camera1_to_marker), false);
800
801        let result = tf_buffer.lookup_transform("base", "target", &Time { sec: 5, nanosec: 0 });
802        assert_eq!(
803            result.unwrap().transform.translation,
804            Vector3 {
805                x: 1.0,
806                y: 1.0,
807                z: 0.0
808            }
809        );
810
811        let result = tf_buffer.lookup_transform(
812            "base",
813            "target",
814            &Time {
815                sec: 5,
816                nanosec: 500_000_000,
817            },
818        );
819        assert_eq!(
820            result.unwrap().transform.translation,
821            Vector3 {
822                x: 1.0,
823                y: 0.0,
824                z: 0.0
825            }
826        );
827
828        let result = tf_buffer.lookup_transform("base", "target", &Time { sec: 6, nanosec: 0 });
829        assert_eq!(
830            result.unwrap().transform.translation,
831            Vector3 {
832                x: 1.0,
833                y: -1.0,
834                z: 0.0
835            }
836        );
837    }
838}
tf_r2r/tf_buffer.rs

tf_r2r/
tf_buffer.rs
