tf2_rs 0.1.0

ROS 2 TF2 bindings for Rust using cxx and rclrs
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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

use crate::error::Tf2Error;
use crate::ffi::ffi::{self, BufferCoreWrapper};
use crate::ffi_utils::{call_bool, call_out};
use crate::time::{LookupTime, TimeSpec};
use crate::transform::Transformable;
use crate::transform_stamped::TransformStamped;
use ros_env::tf2_msgs;

unsafe impl Send for BufferCoreWrapper {}
unsafe impl Sync for BufferCoreWrapper {}

#[derive(Clone)]
pub struct BufferCore {
    inner: cxx::SharedPtr<BufferCoreWrapper>,
}

impl BufferCore {
    pub fn new(cache_time_ns: std::time::Duration) -> Self {
        Self {
            inner: ffi::new_buffer_core(cache_time_ns.as_nanos() as u64),
        }
    }

    fn wrapper(&self) -> &BufferCoreWrapper {
        self.inner.as_ref().expect("BufferCoreWrapper is null")
    }

    pub fn clear(&mut self) {
        self.wrapper().clear();
    }

    pub fn set_transform(
        &mut self,
        tf: &TransformStamped,
        authority: &str,
        is_static: bool,
    ) -> Result<(), Tf2Error> {
        let ok = call_bool(|out_ok| {
            self.wrapper()
                .set_transform(&tf.to_ffi(), authority, is_static, out_ok)
        })?;

        if ok {
            Ok(())
        } else {
            Err(Tf2Error::InvalidArgument(
                "buffer_set_transform rejected transform".to_string(),
            ))
        }
    }

    pub fn check_transform(
        &self,
        target_frame: &str,
        source_frame: &str,
        when: LookupTime,
    ) -> Result<TransformAvailability, Tf2Error> {
        let t = ffi::Tf2Time::from(when);
        let out = call_out(|out| {
            self.wrapper()
                .can_transform(target_frame, source_frame, &t, out)
        })?;

        Ok(if out.available {
            TransformAvailability::Available
        } else {
            TransformAvailability::Unavailable {
                diagnostic: (!out.diagnostic.is_empty()).then_some(out.diagnostic),
            }
        })
    }

    pub fn can_transform(
        &self,
        target_frame: &str,
        source_frame: &str,
        when: LookupTime,
    ) -> Result<bool, Tf2Error> {
        Ok(self
            .check_transform(target_frame, source_frame, when)?
            .is_available())
    }

    pub fn lookup_transform(
        &self,
        target_frame: &str,
        source_frame: &str,
        when: LookupTime,
    ) -> Result<TransformStamped, Tf2Error> {
        let t = ffi::Tf2Time::from(when);
        let ffi_tf = call_out(|out| {
            self.wrapper()
                .lookup_transform(target_frame, source_frame, &t, out)
        })?;

        Ok(TransformStamped::from_ffi(ffi_tf))
    }

    pub fn transform<T: Transformable>(
        &self,
        msg: &T,
        target_frame: &str,
        time: TimeSpec,
    ) -> Result<T, Tf2Error> {
        let when = time.resolve(msg);
        let tf = self.lookup_transform(target_frame, msg.frame_id(), when)?;
        msg.apply_transform(&tf)
    }

    pub fn ingest_tf_message(
        &mut self,
        msg: tf2_msgs::msg::TFMessage,
        authority: &str,
        is_static: bool,
        mut on_err: impl FnMut(Tf2Error),
    ) {
        for t in msg.transforms {
            let tf = (&t).into();
            if let Err(e) = self.set_transform(&tf, authority, is_static) {
                on_err(e);
            }
        }
    }
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub enum TransformAvailability {
    Available,
    Unavailable { diagnostic: Option<String> },
}

impl TransformAvailability {
    pub fn is_available(&self) -> bool {
        matches!(self, Self::Available)
    }

    pub fn diagnostic(&self) -> Option<&str> {
        match self {
            Self::Available => None,
            Self::Unavailable { diagnostic } => diagnostic.as_deref(),
        }
    }
}