tf_r2r/
tf_listener.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
use std::{
    sync::{Arc, RwLock},
    time::Duration,
};

use futures::StreamExt;
use r2r::{
    builtin_interfaces::msg::Time, geometry_msgs::msg::TransformStamped, tf2_msgs::msg::TFMessage,
    QosProfile,
};

use crate::{tf_buffer::TfBuffer, tf_error::TfError};

pub struct TfListener {
    buffer: Arc<RwLock<TfBuffer>>,
}

impl TfListener {
    /// Create a new TfListener
    #[track_caller]
    pub fn new(node: &mut r2r::Node) -> Self {
        Self::new_with_buffer(node, TfBuffer::new())
    }

    #[track_caller]
    pub fn new_with_buffer(node: &mut r2r::Node, tf_buffer: TfBuffer) -> Self {
        let buff = Arc::new(RwLock::new(tf_buffer));

        let mut dynamic_subscriber = node
            .subscribe::<TFMessage>("/tf", QosProfile::default())
            .unwrap();

        let buff_for_dynamic_sub = buff.clone();
        tokio::spawn(async move {
            while Arc::strong_count(&buff_for_dynamic_sub) > 1 {
                if let Some(tf) = dynamic_subscriber.next().await {
                    buff_for_dynamic_sub
                        .write()
                        .unwrap()
                        .handle_incoming_transforms(tf, false);
                }
                tokio::time::sleep(Duration::from_millis(100)).await;
            }
        });

        let mut static_subscriber = node
            .subscribe::<TFMessage>("/tf_static", QosProfile::default())
            .unwrap();

        let buff_for_static_sub = buff.clone();
        tokio::spawn(async move {
            while Arc::strong_count(&buff_for_static_sub) > 1 {
                if let Some(tf) = static_subscriber.next().await {
                    buff_for_static_sub
                        .write()
                        .unwrap()
                        .handle_incoming_transforms(tf, true);
                }
                tokio::time::sleep(Duration::from_millis(100)).await;
            }
        });

        TfListener { buffer: buff }
    }

    /// Looks up a transform within the tree at a given time.
    pub fn lookup_transform(
        &self,
        from: &str,
        to: &str,
        time: Time,
    ) -> Result<TransformStamped, TfError> {
        self.buffer
            .read()
            .unwrap()
            .lookup_transform(from, to, &time)
    }

    /// Looks up a transform within the tree at a given time.
    pub fn lookup_transform_with_time_travel(
        &self,
        from: &str,
        time1: Time,
        to: &str,
        time2: Time,
        fixed_frame: &str,
    ) -> Result<TransformStamped, TfError> {
        self.buffer
            .read()
            .unwrap()
            .lookup_transform_with_time_travel(from, time1, to, time2, fixed_frame)
    }
}