arci_ros/
ros_transform_resolver.rs
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}