tf_rosrust/
tf_listener.rs

1use std::sync::{Arc, RwLock};
2
3use crate::{
4    tf_buffer::TfBuffer,
5    tf_error::TfError,
6    transforms::{geometry_msgs::TransformStamped, tf2_msgs::TFMessage},
7};
8
9///This struct tries to be the same as the C++ version of `TransformListener`. Use this struct to lookup transforms.
10///
11/// Example usage:
12///
13/// ```no_run
14/// use tf_rosrust::TfListener;
15///
16/// rosrust::init("listener");
17/// let listener = TfListener::new();
18///
19/// let rate = rosrust::rate(1.0);
20/// while rosrust::is_ok() {
21///     let tf = listener.lookup_transform("camera", "base_link", rosrust::Time::new());
22///     println!("{tf:?}");
23///     rate.sleep();
24/// }
25/// ```
26/// Do note that unlike the C++ variant of the TfListener, only one TfListener can be created at a time. Like its C++ counterpart,
27/// it must be scoped to exist through the lifetime of the program. One way to do this is using an `Arc` or `RwLock`.
28pub struct TfListener {
29    buffer: Arc<RwLock<TfBuffer>>,
30    _static_subscriber: rosrust::Subscriber,
31    _dynamic_subscriber: rosrust::Subscriber,
32}
33
34impl TfListener {
35    /// Create a new TfListener
36    pub fn new() -> Self {
37        Self::new_with_buffer(TfBuffer::new())
38    }
39
40    pub fn new_with_buffer(tf_buffer: TfBuffer) -> Self {
41        let buff = RwLock::new(tf_buffer);
42        let arc = Arc::new(buff);
43        let r1 = arc.clone();
44        let _dynamic_subscriber = rosrust::subscribe("tf", 100, move |v: TFMessage| {
45            r1.write().unwrap().handle_incoming_transforms(v, false);
46        })
47        .unwrap();
48
49        let r2 = arc.clone();
50        let _static_subscriber = rosrust::subscribe("tf_static", 100, move |v: TFMessage| {
51            r2.write().unwrap().handle_incoming_transforms(v, true);
52        })
53        .unwrap();
54
55        TfListener {
56            buffer: arc,
57            _static_subscriber,
58            _dynamic_subscriber,
59        }
60    }
61
62    /// Looks up a transform within the tree at a given time.
63    pub fn lookup_transform(
64        &self,
65        from: &str,
66        to: &str,
67        time: rosrust::Time,
68    ) -> Result<TransformStamped, TfError> {
69        self.buffer.read().unwrap().lookup_transform(from, to, time)
70    }
71
72    /// Looks up a transform within the tree at a given time.
73    pub fn lookup_transform_with_time_travel(
74        &self,
75        from: &str,
76        time1: rosrust::Time,
77        to: &str,
78        time2: rosrust::Time,
79        fixed_frame: &str,
80    ) -> Result<TransformStamped, TfError> {
81        self.buffer
82            .read()
83            .unwrap()
84            .lookup_transform_with_time_travel(from, time1, to, time2, fixed_frame)
85    }
86}
87
88impl Default for TfListener {
89    fn default() -> Self {
90        TfListener::new()
91    }
92}
tf_rosrust/tf_listener.rs

tf_rosrust/
tf_listener.rs
