tf_r2r/
tf_broadcaster.rs

1use r2r::{geometry_msgs::msg::TransformStamped, tf2_msgs::msg::TFMessage, QosProfile};
2
3use crate::tf_error::TfError;
4
5pub struct TfBroadcaster {
6    publisher: r2r::Publisher<TFMessage>,
7}
8
9impl TfBroadcaster {
10    /// Create a new TfBroadcaster
11    #[track_caller]
12    pub fn new(node: &mut r2r::Node) -> Self {
13        Self {
14            publisher: node.create_publisher("/tf", QosProfile::default()).unwrap(),
15        }
16    }
17
18    /// Broadcast transform
19    pub fn send_transform(&self, tf: TransformStamped) -> Result<(), TfError> {
20        let tf_message = TFMessage {
21            transforms: vec![tf],
22        };
23        // TODO: handle error correctly
24        self.publisher
25            .publish(&tf_message)
26            .map_err(|err| TfError::R2r(err.to_string()))
27    }
28}