tf_rosrust/
tf_buffer.rs

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

tf_rosrust/
tf_buffer.rs
