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
71pub 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}