tf_r2r/
tf_listener.rs

Help
1use std::{
2    sync::{Arc, RwLock},
3    time::Duration,
4};
5
6use futures::StreamExt;
7use r2r::{
8    builtin_interfaces::msg::Time, geometry_msgs::msg::TransformStamped, tf2_msgs::msg::TFMessage,
9    QosProfile,
10};
11
12use crate::{tf_buffer::TfBuffer, tf_error::TfError};
13
14pub struct TfListener {
15    buffer: Arc<RwLock<TfBuffer>>,
16}
17
18impl TfListener {
19    /// Create a new TfListener
20    #[track_caller]
21    pub fn new(node: &mut r2r::Node) -> Self {
22        Self::new_with_buffer(node, TfBuffer::new())
23    }
24
25    #[track_caller]
26    pub fn new_with_buffer(node: &mut r2r::Node, tf_buffer: TfBuffer) -> Self {
27        let buff = Arc::new(RwLock::new(tf_buffer));
28
29        let mut dynamic_subscriber = node
30            .subscribe::<TFMessage>("/tf", QosProfile::default())
31            .unwrap();
32
33        let buff_for_dynamic_sub = buff.clone();
34        tokio::spawn(async move {
35            while Arc::strong_count(&buff_for_dynamic_sub) > 1 {
36                if let Some(tf) = dynamic_subscriber.next().await {
37                    buff_for_dynamic_sub
38                        .write()
39                        .unwrap()
40                        .handle_incoming_transforms(tf, false);
41                }
42                tokio::time::sleep(Duration::from_millis(100)).await;
43            }
44        });
45
46        let mut static_subscriber = node
47            .subscribe::<TFMessage>("/tf_static", QosProfile::default())
48            .unwrap();
49
50        let buff_for_static_sub = buff.clone();
51        tokio::spawn(async move {
52            while Arc::strong_count(&buff_for_static_sub) > 1 {
53                if let Some(tf) = static_subscriber.next().await {
54                    buff_for_static_sub
55                        .write()
56                        .unwrap()
57                        .handle_incoming_transforms(tf, true);
58                }
59                tokio::time::sleep(Duration::from_millis(100)).await;
60            }
61        });
62
63        TfListener { buffer: buff }
64    }
65
66    /// Looks up a transform within the tree at a given time.
67    pub fn lookup_transform(
68        &self,
69        from: &str,
70        to: &str,
71        time: Time,
72    ) -> Result<TransformStamped, TfError> {
73        self.buffer
74            .read()
75            .unwrap()
76            .lookup_transform(from, to, &time)
77    }
78
79    /// Looks up a transform within the tree at a given time.
80    pub fn lookup_transform_with_time_travel(
81        &self,
82        from: &str,
83        time1: Time,
84        to: &str,
85        time2: Time,
86        fixed_frame: &str,
87    ) -> Result<TransformStamped, TfError> {
88        self.buffer
89            .read()
90            .unwrap()
91            .lookup_transform_with_time_travel(from, time1, to, time2, fixed_frame)
92    }
93}
tf_r2r/tf_listener.rs

tf_r2r/
tf_listener.rs
