tf_r2r/
tf_individual_transform_chain.rs
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 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 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}