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

use crate::ffi::ffi;
use ros_env::geometry_msgs;

#[derive(Clone, Debug)]
pub struct TransformStamped {
    pub stamp_sec: i32,
    pub stamp_nanosec: u32,
    pub parent_frame: String,
    pub child_frame: String,
    pub translation: [f64; 3],
    pub rotation: [f64; 4], // x,y,z,w
}

impl TransformStamped {
    pub fn to_ffi(&self) -> ffi::Tf2TransformStamped {
        ffi::Tf2TransformStamped {
            stamp: ffi::Tf2Time {
                sec: self.stamp_sec,
                nanosec: self.stamp_nanosec,
            },
            parent_frame: self.parent_frame.clone(),
            child_frame: self.child_frame.clone(),
            translation_x: self.translation[0],
            translation_y: self.translation[1],
            translation_z: self.translation[2],
            rotation_x: self.rotation[0],
            rotation_y: self.rotation[1],
            rotation_z: self.rotation[2],
            rotation_w: self.rotation[3],
        }
    }

    pub fn from_ffi(v: ffi::Tf2TransformStamped) -> Self {
        Self {
            stamp_sec: v.stamp.sec,
            stamp_nanosec: v.stamp.nanosec,
            parent_frame: v.parent_frame,
            child_frame: v.child_frame,
            translation: [v.translation_x, v.translation_y, v.translation_z],
            rotation: [v.rotation_x, v.rotation_y, v.rotation_z, v.rotation_w],
        }
    }
}

impl From<TransformStamped> for geometry_msgs::msg::TransformStamped {
    fn from(t: TransformStamped) -> Self {
        let mut msg = geometry_msgs::msg::TransformStamped::default();

        msg.header.stamp.sec = t.stamp_sec;
        msg.header.stamp.nanosec = t.stamp_nanosec;
        msg.header.frame_id = t.parent_frame;

        msg.child_frame_id = t.child_frame;

        msg.transform.translation.x = t.translation[0];
        msg.transform.translation.y = t.translation[1];
        msg.transform.translation.z = t.translation[2];

        msg.transform.rotation.x = t.rotation[0];
        msg.transform.rotation.y = t.rotation[1];
        msg.transform.rotation.z = t.rotation[2];
        msg.transform.rotation.w = t.rotation[3];

        msg
    }
}

impl From<&TransformStamped> for geometry_msgs::msg::TransformStamped {
    fn from(t: &TransformStamped) -> Self {
        let mut msg = geometry_msgs::msg::TransformStamped::default();

        msg.header.stamp.sec = t.stamp_sec;
        msg.header.stamp.nanosec = t.stamp_nanosec;
        msg.header.frame_id = t.parent_frame.clone();

        msg.child_frame_id = t.child_frame.clone();

        msg.transform.translation.x = t.translation[0];
        msg.transform.translation.y = t.translation[1];
        msg.transform.translation.z = t.translation[2];

        msg.transform.rotation.x = t.rotation[0];
        msg.transform.rotation.y = t.rotation[1];
        msg.transform.rotation.z = t.rotation[2];
        msg.transform.rotation.w = t.rotation[3];

        msg
    }
}