tf_r2r/
tf_individual_transform_chain.rs

Help
1use r2r::{
2    builtin_interfaces::msg::{Duration, Time},
3    geometry_msgs::msg::TransformStamped,
4};
5
6use crate::{
7    tf_error::TfError,
8    transforms::{interpolate, to_transform_stamped},
9    utils::*,
10};
11
12fn get_nanos(dur: Duration) -> i64 {
13    i64::from(dur.sec) * 1_000_000_000 + i64::from(dur.nanosec)
14}
15
16fn binary_search_time(chain: &[TransformStamped], time: &Time) -> Result<usize, usize> {
17    chain.binary_search_by(|element| {
18        time_as_ns_i64(&element.header.stamp).cmp(&time_as_ns_i64(time))
19    })
20}
21
22#[derive(Clone, Debug)]
23pub(crate) struct TfIndividualTransformChain {
24    cache_duration: Duration,
25    static_tf: bool,
26    // TODO: Implement a circular buffer. Current method is slow.
27    pub(crate) transform_chain: Vec<TransformStamped>,
28}
29
30impl TfIndividualTransformChain {
31    pub(crate) fn new(static_tf: bool, cache_duration: Duration) -> Self {
32        Self {
33            cache_duration,
34            transform_chain: Vec::new(),
35            static_tf,
36        }
37    }
38
39    fn newest_stamp(&self) -> Option<Time> {
40        self.transform_chain.last().map(|x| x.header.stamp.clone())
41    }
42
43    pub(crate) fn add_to_buffer(&mut self, msg: TransformStamped) {
44        let index = binary_search_time(&self.transform_chain, &msg.header.stamp)
45            .unwrap_or_else(|index| index);
46        self.transform_chain.insert(index, msg.clone());
47
48        if let Some(newest_stamp) = self.newest_stamp() {
49            if is_time_later(
50                &newest_stamp,
51                &add_time_and_duration(&time_from_nanosec(0), &self.cache_duration),
52            ) {
53                let time_to_keep = sub_duration_from_time(&newest_stamp, &self.cache_duration);
54                let index =
55                    binary_search_time(&self.transform_chain, &time_to_keep).unwrap_or_else(|x| x);
56                self.transform_chain.drain(..index);
57            }
58        }
59    }
60
61    /// If timestamp is zero, return the latest transform.
62    pub(crate) fn get_closest_transform(&self, time: &Time) -> Result<TransformStamped, TfError> {
63        if time_as_ns_i64(time) == 0 {
64            return Ok(self.transform_chain.last().unwrap().clone());
65        }
66
67        if self.static_tf {
68            return Ok(self.transform_chain.last().unwrap().clone());
69        }
70
71        match binary_search_time(&self.transform_chain, time) {
72            Ok(x) => return Ok(self.transform_chain.get(x).unwrap().clone()),
73            Err(x) => {
74                if x == 0 {
75                    return Err(TfError::AttemptedLookupInPast(
76                        time.clone(),
77                        Box::new(self.transform_chain.first().unwrap().clone()),
78                    ));
79                }
80                if x >= self.transform_chain.len() {
81                    return Err(TfError::AttemptedLookUpInFuture(
82                        Box::new(self.transform_chain.last().unwrap().clone()),
83                        time.clone(),
84                    ));
85                }
86                let tf1 = self.transform_chain.get(x - 1).unwrap().clone().transform;
87                let tf2 = self.transform_chain.get(x).unwrap().clone().transform;
88                let time1 = self
89                    .transform_chain
90                    .get(x - 1)
91                    .unwrap()
92                    .header
93                    .stamp
94                    .clone();
95                let time2 = self.transform_chain.get(x).unwrap().header.stamp.clone();
96                let header = self.transform_chain.get(x).unwrap().header.clone();
97                let child_frame = self.transform_chain.get(x).unwrap().child_frame_id.clone();
98                let total_duration = get_nanos(sub_time_and_time(&time2, &time1)) as f64;
99                let desired_duration = get_nanos(sub_time_and_time(time, &time1)) as f64;
100                let weight = 1.0 - desired_duration / total_duration;
101                let final_tf = interpolate(tf1, tf2, weight);
102                let ros_msg = to_transform_stamped(final_tf, header.frame_id, child_frame, time);
103                Ok(ros_msg)
104            }
105        }
106    }
107
108    pub(crate) fn has_valid_transform(&self, time: &Time) -> bool {
109        if self.transform_chain.is_empty() {
110            return false;
111        }
112
113        if self.static_tf {
114            return true;
115        }
116
117        let first = self.transform_chain.first().unwrap();
118        let last = self.transform_chain.last().unwrap();
119
120        time_as_ns_i64(time) == 0
121            || is_time_in_range_eq(time, &first.header.stamp, &last.header.stamp)
122    }
123}
tf_r2r/tf_individual_transform_chain.rs

tf_r2r/
tf_individual_transform_chain.rs
