tf2_rs 0.1.0

ROS 2 TF2 bindings for Rust using cxx and rclrs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90

use rclrs::{IntoPrimitiveOptions, Publisher};
use ros_env::{geometry_msgs, tf2_msgs::msg::TFMessage};

#[derive(Clone)]
struct TfBroadcasterInner {
    pub_: Publisher<TFMessage>,
}

impl TfBroadcasterInner {
    fn new(pub_: Publisher<TFMessage>) -> Self {
        Self { pub_ }
    }

    fn send_transform<T>(&self, tf: T) -> Result<(), rclrs::RclrsError>
    where
        T: Into<geometry_msgs::msg::TransformStamped>,
    {
        let tf: geometry_msgs::msg::TransformStamped = tf.into();
        self.send_transforms(std::iter::once(tf))
    }

    fn send_transforms<I, T>(&self, tfs: I) -> Result<(), rclrs::RclrsError>
    where
        I: IntoIterator<Item = T>,
        T: Into<geometry_msgs::msg::TransformStamped>,
    {
        let msg = TFMessage {
            transforms: tfs.into_iter().map(Into::into).collect(),
        };
        self.pub_.publish(msg)
    }
}

#[derive(Clone)]
pub struct TransformBroadcaster {
    inner: TfBroadcasterInner,
}

impl TransformBroadcaster {
    pub fn new(node: &rclrs::Node) -> Result<Self, rclrs::RclrsError> {
        let pub_ = node.create_publisher("/tf".keep_last(100).reliable())?;
        Ok(Self {
            inner: TfBroadcasterInner::new(pub_),
        })
    }

    pub fn send_transform<T>(&self, tf: T) -> Result<(), rclrs::RclrsError>
    where
        T: Into<geometry_msgs::msg::TransformStamped>,
    {
        self.inner.send_transform(tf)
    }

    pub fn send_transforms<I, T>(&self, tfs: I) -> Result<(), rclrs::RclrsError>
    where
        I: IntoIterator<Item = T>,
        T: Into<geometry_msgs::msg::TransformStamped>,
    {
        self.inner.send_transforms(tfs)
    }
}

#[derive(Clone)]
pub struct StaticTransformBroadcaster {
    inner: TfBroadcasterInner,
}

impl StaticTransformBroadcaster {
    pub fn new(node: &rclrs::Node) -> Result<Self, rclrs::RclrsError> {
        let pub_ = node.create_publisher("/tf_static".keep_last(1).reliable().transient_local())?;
        Ok(Self {
            inner: TfBroadcasterInner::new(pub_),
        })
    }

    pub fn send_transform<T>(&self, tf: T) -> Result<(), rclrs::RclrsError>
    where
        T: Into<geometry_msgs::msg::TransformStamped>,
    {
        self.inner.send_transform(tf)
    }

    pub fn send_transforms<I, T>(&self, tfs: I) -> Result<(), rclrs::RclrsError>
    where
        I: IntoIterator<Item = T>,
        T: Into<geometry_msgs::msg::TransformStamped>,
    {
        self.inner.send_transforms(tfs)
    }
}