tf_r2r/
tf_individual_transform_chain.rs

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
use r2r::{
    builtin_interfaces::msg::{Duration, Time},
    geometry_msgs::msg::TransformStamped,
};

use crate::{
    tf_error::TfError,
    transforms::{interpolate, to_transform_stamped},
    utils::*,
};

fn get_nanos(dur: Duration) -> i64 {
    i64::from(dur.sec) * 1_000_000_000 + i64::from(dur.nanosec)
}

fn binary_search_time(chain: &[TransformStamped], time: &Time) -> Result<usize, usize> {
    chain.binary_search_by(|element| {
        time_as_ns_i64(&element.header.stamp).cmp(&time_as_ns_i64(time))
    })
}

#[derive(Clone, Debug)]
pub(crate) struct TfIndividualTransformChain {
    cache_duration: Duration,
    static_tf: bool,
    // TODO: Implement a circular buffer. Current method is slow.
    pub(crate) transform_chain: Vec<TransformStamped>,
}

impl TfIndividualTransformChain {
    pub(crate) fn new(static_tf: bool, cache_duration: Duration) -> Self {
        Self {
            cache_duration,
            transform_chain: Vec::new(),
            static_tf,
        }
    }

    fn newest_stamp(&self) -> Option<Time> {
        self.transform_chain.last().map(|x| x.header.stamp.clone())
    }

    pub(crate) fn add_to_buffer(&mut self, msg: TransformStamped) {
        let index = binary_search_time(&self.transform_chain, &msg.header.stamp)
            .unwrap_or_else(|index| index);
        self.transform_chain.insert(index, msg.clone());

        if let Some(newest_stamp) = self.newest_stamp() {
            if is_time_later(
                &newest_stamp,
                &add_time_and_duration(&time_from_nanosec(0), &self.cache_duration),
            ) {
                let time_to_keep = sub_duration_from_time(&newest_stamp, &self.cache_duration);
                let index =
                    binary_search_time(&self.transform_chain, &time_to_keep).unwrap_or_else(|x| x);
                self.transform_chain.drain(..index);
            }
        }
    }

    /// If timestamp is zero, return the latest transform.
    pub(crate) fn get_closest_transform(&self, time: &Time) -> Result<TransformStamped, TfError> {
        if time_as_ns_i64(time) == 0 {
            return Ok(self.transform_chain.last().unwrap().clone());
        }

        if self.static_tf {
            return Ok(self.transform_chain.last().unwrap().clone());
        }

        match binary_search_time(&self.transform_chain, time) {
            Ok(x) => return Ok(self.transform_chain.get(x).unwrap().clone()),
            Err(x) => {
                if x == 0 {
                    return Err(TfError::AttemptedLookupInPast(
                        time.clone(),
                        Box::new(self.transform_chain.first().unwrap().clone()),
                    ));
                }
                if x >= self.transform_chain.len() {
                    return Err(TfError::AttemptedLookUpInFuture(
                        Box::new(self.transform_chain.last().unwrap().clone()),
                        time.clone(),
                    ));
                }
                let tf1 = self.transform_chain.get(x - 1).unwrap().clone().transform;
                let tf2 = self.transform_chain.get(x).unwrap().clone().transform;
                let time1 = self
                    .transform_chain
                    .get(x - 1)
                    .unwrap()
                    .header
                    .stamp
                    .clone();
                let time2 = self.transform_chain.get(x).unwrap().header.stamp.clone();
                let header = self.transform_chain.get(x).unwrap().header.clone();
                let child_frame = self.transform_chain.get(x).unwrap().child_frame_id.clone();
                let total_duration = get_nanos(sub_time_and_time(&time2, &time1)) as f64;
                let desired_duration = get_nanos(sub_time_and_time(time, &time1)) as f64;
                let weight = 1.0 - desired_duration / total_duration;
                let final_tf = interpolate(tf1, tf2, weight);
                let ros_msg = to_transform_stamped(final_tf, header.frame_id, child_frame, time);
                Ok(ros_msg)
            }
        }
    }

    pub(crate) fn has_valid_transform(&self, time: &Time) -> bool {
        if self.transform_chain.is_empty() {
            return false;
        }

        if self.static_tf {
            return true;
        }

        let first = self.transform_chain.first().unwrap();
        let last = self.transform_chain.last().unwrap();

        time_as_ns_i64(time) == 0
            || is_time_in_range_eq(time, &first.header.stamp, &last.header.stamp)
    }
}