arci_ros2/
ros2_transform_resolver.rs

1use std::time::Duration;
2
3use arci::{
4    nalgebra::{Quaternion, Translation3},
5    Isometry3, TransformResolver, UnitQuaternion,
6};
7use r2r::builtin_interfaces::msg as builtin_msg;
8use tf_r2r::{TfBuffer, TfListener};
9use tracing::{debug, warn};
10
11use crate::{utils::convert_system_time_to_ros2_time, Node};
12
13/// `arci::TransformResolver` implementation for ROS2.
14pub struct Ros2TransformResolver {
15    retry_rate: f64,
16    max_retry: usize,
17    tf_listener: TfListener,
18    // keep not to be dropped
19    _node: Node,
20}
21
22impl Ros2TransformResolver {
23    /// Creates a new `Ros2TransformResolver`.
24    #[track_caller]
25    pub fn new(node: Node, cache_duration: Duration, retry_rate: f64, max_retry: usize) -> Self {
26        let duration = builtin_msg::Duration {
27            sec: cache_duration.as_secs() as i32,
28            nanosec: cache_duration.subsec_nanos(),
29        };
30
31        let tf_listener =
32            TfListener::new_with_buffer(&mut node.r2r(), TfBuffer::new_with_duration(duration));
33        Self {
34            retry_rate,
35            max_retry,
36            tf_listener,
37            _node: node,
38        }
39    }
40}
41
42impl TransformResolver for Ros2TransformResolver {
43    fn resolve_transformation(
44        &self,
45        from: &str,
46        to: &str,
47        time: std::time::SystemTime,
48    ) -> Result<Isometry3<f64>, arci::Error> {
49        const BILLION: u128 = 1_000_000_000;
50
51        let ros_time = convert_system_time_to_ros2_time(&time);
52        let wait_nanos = BILLION as u64 / self.retry_rate as u64;
53
54        let mut last_error = None;
55
56        for i in 0..=self.max_retry {
57            if i != 0 {
58                debug!("Retrying {from} -> {to} ({i} / {}) ...", self.max_retry);
59            }
60            let result = self
61                .tf_listener
62                .lookup_transform(from, to, ros_time.clone());
63            match result {
64                Ok(result) => {
65                    let translation = result.transform.translation;
66                    let rotation = result.transform.rotation;
67
68                    return Ok(Isometry3::from_parts(
69                        Translation3::new(translation.x, translation.y, translation.z),
70                        UnitQuaternion::from_quaternion(Quaternion::new(
71                            rotation.w, rotation.x, rotation.y, rotation.z,
72                        )),
73                    ));
74                }
75                Err(e) => {
76                    debug!("Failed to lookup_transform ({e:?})");
77                    last_error = Some(e)
78                }
79            }
80            std::thread::sleep(Duration::from_nanos(wait_nanos));
81        }
82        match last_error {
83            Some(e) => {
84                warn!("{e:?}");
85                Err(anyhow::anyhow!("{e:?}").into())
86            }
87            None => Err(anyhow::anyhow!("Broken Logic").into()),
88        }
89    }
90}
arci_ros2/ros2_transform_resolver.rs

arci_ros2/
ros2_transform_resolver.rs
