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