arci_ros/
ros_transform_resolver.rs

Help
1use std::time::Duration;
2
3use arci::{Isometry3, TransformResolver};
4use nalgebra::{Quaternion, Translation3, UnitQuaternion};
5use rosrust::rate;
6use tf_rosrust::{TfBuffer, TfListener};
7use tracing::{debug, warn};
8
9use crate::convert_system_time_to_ros_time;
10
11pub struct RosTransformResolver {
12    retry_rate: f64,
13    max_retry: usize,
14    tf_listener: TfListener,
15}
16
17impl RosTransformResolver {
18    pub fn new(cache_duration: Duration, retry_rate: f64, max_retry: usize) -> Self {
19        Self {
20            retry_rate,
21            max_retry,
22            tf_listener: TfListener::new_with_buffer(TfBuffer::new_with_duration(
23                cache_duration.into(),
24            )),
25        }
26    }
27}
28
29impl TransformResolver for RosTransformResolver {
30    fn resolve_transformation(
31        &self,
32        from: &str,
33        to: &str,
34        time: std::time::SystemTime,
35    ) -> Result<nalgebra::Isometry3<f64>, arci::Error> {
36        let ros_time = convert_system_time_to_ros_time(&time);
37        let rate = rate(self.retry_rate);
38        let mut last_error = None;
39        for i in 0..=self.max_retry {
40            if i != 0 {
41                debug!("Retrying {from} -> {to} ({i} / {}) ...", self.max_retry);
42            }
43            let result = self.tf_listener.lookup_transform(from, to, ros_time);
44            match result {
45                Ok(result) => {
46                    let translation = result.transform.translation;
47                    let rotation = result.transform.rotation;
48
49                    return Ok(Isometry3::from_parts(
50                        Translation3::new(translation.x, translation.y, translation.z),
51                        UnitQuaternion::from_quaternion(Quaternion::new(
52                            rotation.w, rotation.x, rotation.y, rotation.z,
53                        )),
54                    ));
55                }
56                Err(e) => {
57                    debug!("Failed to lookup_transform ({e:?})");
58                    last_error = Some(e)
59                }
60            }
61            rate.sleep();
62        }
63        match last_error {
64            Some(e) => {
65                warn!("{e:?}");
66                Err(anyhow::anyhow!("{e:?}").into())
67            }
68            None => Err(anyhow::anyhow!("Broken Logic").into()),
69        }
70    }
71}
arci_ros/ros_transform_resolver.rs

arci_ros/
ros_transform_resolver.rs
