tf_rosrust/
transforms.rs

1pub use nalgebra;
2use nalgebra::geometry::{Isometry3, Translation3, UnitQuaternion};
3
4rosrust::rosmsg_include!(
5    geometry_msgs / Transform,
6    geometry_msgs / Pose,
7    geometry_msgs / Vector3,
8    geometry_msgs / Quaternion,
9    geometry_msgs / TransformStamped,
10    std_msgs / Header,
11    tf2_msgs / TFMessage
12);
13
14use geometry_msgs::{Pose, Quaternion, Transform, TransformStamped, Vector3};
15use std_msgs::Header;
16
17pub fn isometry_from_pose(pose: &Pose) -> Isometry3<f64> {
18    let trans = Translation3::new(pose.position.x, pose.position.y, pose.position.z);
19    let rot = UnitQuaternion::new_normalize(nalgebra::geometry::Quaternion::new(
20        pose.orientation.w,
21        pose.orientation.x,
22        pose.orientation.y,
23        pose.orientation.z,
24    ));
25
26    Isometry3::from_parts(trans, rot)
27}
28
29pub fn isometry_from_transform(tf: &Transform) -> Isometry3<f64> {
30    let trans = Translation3::new(tf.translation.x, tf.translation.y, tf.translation.z);
31    let rot = UnitQuaternion::new_normalize(nalgebra::geometry::Quaternion::new(
32        tf.rotation.w,
33        tf.rotation.x,
34        tf.rotation.y,
35        tf.rotation.z,
36    ));
37
38    Isometry3::from_parts(trans, rot)
39}
40
41pub fn isometry_to_transform(iso: Isometry3<f64>) -> Transform {
42    Transform {
43        translation: Vector3 {
44            x: iso.translation.x,
45            y: iso.translation.y,
46            z: iso.translation.z,
47        },
48        rotation: Quaternion {
49            x: iso.rotation.i,
50            y: iso.rotation.j,
51            z: iso.rotation.k,
52            w: iso.rotation.w,
53        },
54    }
55}
56
57pub fn get_inverse(trans: &TransformStamped) -> TransformStamped {
58    TransformStamped {
59        header: Header {
60            seq: 1u32,
61            stamp: trans.header.stamp,
62            frame_id: trans.child_frame_id.clone(),
63        },
64        child_frame_id: trans.header.frame_id.clone(),
65        transform: isometry_to_transform(
66            isometry_from_transform(&trans.transform).clone().inverse(),
67        ),
68    }
69}
70
71///Chain multiple transforms together. Takes in a vector of transforms. The vector should be in order of desired transformations
72pub fn chain_transforms(transforms: &[Transform]) -> Transform {
73    let mut final_transform = Isometry3::identity();
74    for t in transforms {
75        let tf = isometry_from_transform(t);
76        final_transform *= tf;
77    }
78    isometry_to_transform(final_transform)
79}
80
81pub fn interpolate(t1: Transform, t2: Transform, weight: f64) -> Transform {
82    let r1 = nalgebra::geometry::Quaternion::new(
83        t1.rotation.w,
84        t1.rotation.x,
85        t1.rotation.y,
86        t1.rotation.z,
87    );
88    let r2 = nalgebra::geometry::Quaternion::new(
89        t2.rotation.w,
90        t2.rotation.x,
91        t2.rotation.y,
92        t2.rotation.z,
93    );
94    let r1 = UnitQuaternion::from_quaternion(r1);
95    let r2 = UnitQuaternion::from_quaternion(r2);
96    let res = r1.try_slerp(&r2, weight, 1e-9);
97    match res {
98        Some(qt) => Transform {
99            translation: Vector3 {
100                x: t1.translation.x * weight + t2.translation.x * (1.0 - weight),
101                y: t1.translation.y * weight + t2.translation.y * (1.0 - weight),
102                z: t1.translation.z * weight + t2.translation.z * (1.0 - weight),
103            },
104            rotation: Quaternion {
105                x: qt.coords[0],
106                y: qt.coords[1],
107                z: qt.coords[2],
108                w: qt.coords[3],
109            },
110        },
111        None => {
112            if weight > 0.5 {
113                Transform {
114                    translation: Vector3 {
115                        x: t1.translation.x * weight + t2.translation.x * (1.0 - weight),
116                        y: t1.translation.y * weight + t2.translation.y * (1.0 - weight),
117                        z: t1.translation.z * weight + t2.translation.z * (1.0 - weight),
118                    },
119                    rotation: t1.rotation,
120                }
121            } else {
122                Transform {
123                    translation: Vector3 {
124                        x: t1.translation.x * weight + t2.translation.x * (1.0 - weight),
125                        y: t1.translation.y * weight + t2.translation.y * (1.0 - weight),
126                        z: t1.translation.z * weight + t2.translation.z * (1.0 - weight),
127                    },
128                    rotation: t2.rotation,
129                }
130            }
131        }
132    }
133}
134
135#[cfg(test)]
136mod test {
137    use super::*;
138
139    #[test]
140    fn test_basic_translation_chaining() {
141        let tf1 = Transform {
142            translation: Vector3 {
143                x: 1f64,
144                y: 1f64,
145                z: 0f64,
146            },
147            rotation: Quaternion {
148                x: 0f64,
149                y: 0f64,
150                z: 0f64,
151                w: 1f64,
152            },
153        };
154        let expected_tf = Transform {
155            translation: Vector3 {
156                x: 2f64,
157                y: 2f64,
158                z: 0f64,
159            },
160            rotation: Quaternion {
161                x: 0f64,
162                y: 0f64,
163                z: 0f64,
164                w: 1f64,
165            },
166        };
167        let transform_chain = vec![tf1.clone(), tf1];
168        let res = chain_transforms(&transform_chain);
169        assert_eq!(res, expected_tf);
170    }
171
172    #[test]
173    fn test_basic_interpolation() {
174        let tf1 = Transform {
175            translation: Vector3 {
176                x: 1f64,
177                y: 1f64,
178                z: 0f64,
179            },
180            rotation: Quaternion {
181                x: 0f64,
182                y: 0f64,
183                z: 0f64,
184                w: 1f64,
185            },
186        };
187        let tf2 = Transform {
188            translation: Vector3 {
189                x: 2f64,
190                y: 2f64,
191                z: 0f64,
192            },
193            rotation: Quaternion {
194                x: 0f64,
195                y: 0f64,
196                z: 0f64,
197                w: 1f64,
198            },
199        };
200        let expected = Transform {
201            translation: Vector3 {
202                x: 1.5f64,
203                y: 1.5f64,
204                z: 0f64,
205            },
206            rotation: Quaternion {
207                x: 0f64,
208                y: 0f64,
209                z: 0f64,
210                w: 1f64,
211            },
212        };
213        assert_eq!(interpolate(tf1, tf2, 0.5), expected);
214    }
215}
216
217pub(crate) fn to_transform_stamped(
218    tf: Transform,
219    from: std::string::String,
220    to: std::string::String,
221    time: rosrust::Time,
222) -> TransformStamped {
223    TransformStamped {
224        header: Header {
225            frame_id: from,
226            stamp: time,
227            seq: 1u32,
228        },
229        child_frame_id: to,
230        transform: tf,
231    }
232}