tf_rosrust/
tf_individual_transform_chain.rs

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

tf_rosrust/
tf_individual_transform_chain.rs
