test_imu_transforms.cpp
Go to the documentation of this file.
1 
9 #include "gtest/gtest.h"
10 
11 #include <imu_transformer/tf2_sensor_msgs.h>
12 #include <sensor_msgs/Imu.h>
13 #include <sensor_msgs/MagneticField.h>
14 #include <tf2/LinearMath/Quaternion.h>
15 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
16 
17 void compareCovariances(const boost::array<double, 9>& c1, const boost::array<double, 9>& c2)
18 {
19  for (size_t i = 0; i < 9; ++i)
20  EXPECT_NEAR(c1[i], c2[i], 1e-6) << "Wrong value at position " << i;
21 }
22 
23 TEST(Covariance, Transform)
24 {
25  boost::array<double, 9> in = {1, 0, 0, 0, 2, 0, 0, 0, 3};
26  boost::array<double, 9> expectedOut = {1, 0, 0, 0, 2, 0, 0, 0, 3};
27  boost::array<double, 9> out{};
28  Eigen::Quaterniond q(1, 0, 0, 0);
29  tf2::transformCovariance(in, out, q);
30  compareCovariances(expectedOut, out);
31 
32  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0, 0, 1)));
33  expectedOut = {2, 0, 0, 0, 1, 0, 0, 0, 3};
34  tf2::transformCovariance(in, out, q);
35  compareCovariances(expectedOut, out);
36 
37  q = q.inverse();
38  tf2::transformCovariance(in, out, q);
39  compareCovariances(expectedOut, out);
40 
41  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 0, 0)));
42  expectedOut = {1, 0, 0, 0, 3, 0, 0, 0, 2};
43  tf2::transformCovariance(in, out, q);
44  compareCovariances(expectedOut, out);
45 
46  q = q.inverse();
47  tf2::transformCovariance(in, out, q);
48  compareCovariances(expectedOut, out);
49 
50  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(0, 1, 0)));
51  expectedOut = {3, 0, 0, 0, 2, 0, 0, 0, 1};
52  tf2::transformCovariance(in, out, q);
53  compareCovariances(expectedOut, out);
54 
55  q = q.inverse();
56  tf2::transformCovariance(in, out, q);
57  compareCovariances(expectedOut, out);
58 
59  q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d(1, 1, 1)));
60  expectedOut = {2.5, -0.5, 3, 1, 0, -1, -1.5, 2, 0.5};
61  tf2::transformCovariance(in, out, q);
62  compareCovariances(expectedOut, out);
63 
64  q = q.inverse();
65  expectedOut = {1.5, -1, 1, 2, 2, -1.5, -0.5, 3, -0.5};
66  tf2::transformCovariance(in, out, q);
67  compareCovariances(expectedOut, out);
68 }
69 
70 TEST(Imu, GetTimestamp)
71 {
72  sensor_msgs::Imu msg;
73  msg.header.stamp.sec = 1;
74  msg.header.stamp.nsec = 2;
75 
76  EXPECT_EQ(msg.header.stamp, tf2::getTimestamp(msg));
77 }
78 
79 TEST(Imu, GetFrameId)
80 {
81  sensor_msgs::Imu msg;
82  msg.header.frame_id = "test";
83 
84  EXPECT_EQ(msg.header.frame_id, tf2::getFrameId(msg));
85 }
86 
87 void prepareImuMsg(sensor_msgs::Imu& msg)
88 {
89  msg.header.frame_id = "test2";
90  msg.header.stamp.sec = 1;
91  msg.angular_velocity.x = 1;
92  msg.angular_velocity.y = 2;
93  msg.angular_velocity.z = 3;
94  msg.angular_velocity_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
95  msg.linear_acceleration.x = 1;
96  msg.linear_acceleration.y = 2;
97  msg.linear_acceleration.z = 3;
98  msg.linear_acceleration_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
99  msg.orientation.w = 1;
100  msg.orientation_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
101 }
102 
103 void prepareTf(geometry_msgs::TransformStamped& tf)
104 {
105  tf.header.frame_id = "test";
106  tf.header.stamp.sec = 1;
107  tf.child_frame_id = "test2";
108  tf.transform.translation.x = 1e6;
109  tf.transform.translation.y = 2e6;
110  tf.transform.translation.z = -3e6;
111  tf.transform.rotation.w = 1;
112 }
113 
114 TEST(Imu, DoTransformYaw)
115 {
116  // Q = +90 degrees yaw
117 
118  sensor_msgs::Imu msg;
119  prepareImuMsg(msg);
120 
121  geometry_msgs::TransformStamped tf;
122  prepareTf(tf);
123 
124  tf2::Quaternion q;
125  q.setRPY(0, 0, M_PI_2);
126  tf2::convert(q, tf.transform.rotation);
127 
128  sensor_msgs::Imu out;
129  tf2::doTransform(msg, out, tf);
130 
131  tf2::Quaternion rot;
132 
133  EXPECT_EQ("test", out.header.frame_id);
134  EXPECT_EQ(msg.header.stamp, out.header.stamp);
135  EXPECT_NEAR(-msg.angular_velocity.y, out.angular_velocity.x, 1e-6);
136  EXPECT_NEAR(msg.angular_velocity.x, out.angular_velocity.y, 1e-6);
137  EXPECT_NEAR(msg.angular_velocity.z, out.angular_velocity.z, 1e-6);
138  EXPECT_NEAR(-msg.linear_acceleration.y, out.linear_acceleration.x, 1e-6);
139  EXPECT_NEAR(msg.linear_acceleration.x, out.linear_acceleration.y, 1e-6);
140  EXPECT_NEAR(msg.linear_acceleration.z, out.linear_acceleration.z, 1e-6);
141  // Transforming orientation means expressing the attitude of the new frame in the same world frame (i.e. you have
142  // data in imu frame and want to ask what is the world-referenced orientation of the base_link frame that is attached
143  // to this IMU). This is why the orientation change goes the other way than the transform.
144  tf2::convert(out.orientation, rot);
145  EXPECT_NEAR(0, rot.angleShortestPath(q.inverse()), 1e-6);
146 
147  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.angular_velocity_covariance);
148  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.linear_acceleration_covariance);
149  // Orientation covariance stays as it is measured regarding the fixed world frame
150  compareCovariances(msg.orientation_covariance, out.orientation_covariance);
151 }
152 
153 TEST(Imu, DoTransformEnuNed)
154 {
155  // Q = ENU->NED transform
156 
157  sensor_msgs::Imu msg;
158  prepareImuMsg(msg);
159 
160  geometry_msgs::TransformStamped tf;
161  prepareTf(tf);
162 
163  tf2::Quaternion q;
164  q.setRPY(M_PI, 0, M_PI_2);
165  tf2::convert(q, tf.transform.rotation);
166 
167  sensor_msgs::Imu out;
168  tf2::doTransform(msg, out, tf);
169 
170  tf2::Quaternion rot;
171 
172  EXPECT_EQ("test", out.header.frame_id);
173  EXPECT_EQ(msg.header.stamp, out.header.stamp);
174  EXPECT_NEAR(msg.angular_velocity.y, out.angular_velocity.x, 1e-6);
175  EXPECT_NEAR(msg.angular_velocity.x, out.angular_velocity.y, 1e-6);
176  EXPECT_NEAR(-msg.angular_velocity.z, out.angular_velocity.z, 1e-6);
177  EXPECT_NEAR(msg.linear_acceleration.y, out.linear_acceleration.x, 1e-6);
178  EXPECT_NEAR(msg.linear_acceleration.x, out.linear_acceleration.y, 1e-6);
179  EXPECT_NEAR(-msg.linear_acceleration.z, out.linear_acceleration.z, 1e-6);
180  // Transforming orientation means expressing the attitude of the new frame in the same world frame (i.e. you have
181  // data in imu frame and want to ask what is the world-referenced orientation of the base_link frame that is attached
182  // to this IMU). This is why the orientation change goes the other way than the transform.
183  tf2::convert(out.orientation, rot);
184  EXPECT_NEAR(0, rot.angleShortestPath(q.inverse()), 1e-6);
185 
186  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.angular_velocity_covariance);
187  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.linear_acceleration_covariance);
188  // Orientation covariance stays as it is measured regarding the fixed world frame
189  compareCovariances(msg.orientation_covariance, out.orientation_covariance);
190 }
191 
192 TEST(Mag, GetTimestamp)
193 {
194  sensor_msgs::MagneticField msg;
195  msg.header.stamp.sec = 1;
196  msg.header.stamp.nsec = 2;
197 
198  EXPECT_EQ(msg.header.stamp, tf2::getTimestamp(msg));
199 }
200 
201 TEST(Mag, GetFrameId)
202 {
203  sensor_msgs::MagneticField msg;
204  msg.header.frame_id = "test";
205 
206  EXPECT_EQ(msg.header.frame_id, tf2::getFrameId(msg));
207 }
208 
209 void prepareMagMsg(sensor_msgs::MagneticField& msg)
210 {
211  msg.header.frame_id = "test2";
212  msg.header.stamp.sec = 1;
213  msg.magnetic_field.x = 1;
214  msg.magnetic_field.y = 2;
215  msg.magnetic_field.z = 3;
216  msg.magnetic_field_covariance = {1, 0, 0, 0, 2, 0, 0, 0, 3};
217 }
218 
219 TEST(Mag, DoTransformYaw)
220 {
221  // Q = +90 degrees yaw
222 
223  sensor_msgs::MagneticField msg;
224  prepareMagMsg(msg);
225 
226  geometry_msgs::TransformStamped tf;
227  prepareTf(tf);
228 
229  tf2::Quaternion q;
230  q.setRPY(0, 0, M_PI_2);
231  tf2::convert(q, tf.transform.rotation);
232 
233  sensor_msgs::MagneticField out;
234  tf2::doTransform(msg, out, tf);
235 
236  EXPECT_EQ("test", out.header.frame_id);
237  EXPECT_EQ(msg.header.stamp, out.header.stamp);
238  EXPECT_NEAR(-msg.magnetic_field.y, out.magnetic_field.x, 1e-6);
239  EXPECT_NEAR(msg.magnetic_field.x, out.magnetic_field.y, 1e-6);
240  EXPECT_NEAR(msg.magnetic_field.z, out.magnetic_field.z, 1e-6);
241 
242  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.magnetic_field_covariance);
243 }
244 
245 TEST(Mag, DoTransformEnuNed)
246 {
247  // Q = ENU->NED transform
248 
249  sensor_msgs::MagneticField msg;
250  prepareMagMsg(msg);
251 
252  geometry_msgs::TransformStamped tf;
253  prepareTf(tf);
254 
255  tf2::Quaternion q;
256  q.setRPY(M_PI, 0, M_PI_2);
257  tf2::convert(q, tf.transform.rotation);
258 
259  sensor_msgs::MagneticField out;
260  tf2::doTransform(msg, out, tf);
261 
262  EXPECT_EQ("test", out.header.frame_id);
263  EXPECT_EQ(msg.header.stamp, out.header.stamp);
264  EXPECT_NEAR(msg.magnetic_field.y, out.magnetic_field.x, 1e-6);
265  EXPECT_NEAR(msg.magnetic_field.x, out.magnetic_field.y, 1e-6);
266  EXPECT_NEAR(-msg.magnetic_field.z, out.magnetic_field.z, 1e-6);
267 
268  compareCovariances({2, 0, 0, 0, 1, 0, 0, 0, 3}, out.magnetic_field_covariance);
269 }
270 
271 int main(int argc, char **argv)
272 {
273  testing::InitGoogleTest(&argc, argv);
274  return RUN_ALL_TESTS();
275 }
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
tf2::getFrameId
const std::string & getFrameId(const T &t)
tf2::transformCovariance
void transformCovariance(const boost::array< double, 9 > &in, boost::array< double, 9 > &out, Eigen::Quaternion< double > r)
Definition: tf2_sensor_msgs.h:69
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
main
int main(int argc, char **argv)
Definition: test_imu_transforms.cpp:271
tf2::Quaternion::inverse
Quaternion inverse() const
tf2_sensor_msgs.h
tf2::getTimestamp
const ros::Time & getTimestamp(const T &t)
tf2::convert
void convert(const A &a, B &b)
TEST
TEST(Covariance, Transform)
Definition: test_imu_transforms.cpp:23
prepareImuMsg
void prepareImuMsg(sensor_msgs::Imu &msg)
Definition: test_imu_transforms.cpp:87
tf2::Quaternion::angleShortestPath
tf2Scalar angleShortestPath(const Quaternion &q) const
compareCovariances
void compareCovariances(const boost::array< double, 9 > &c1, const boost::array< double, 9 > &c2)
Definition: test_imu_transforms.cpp:17
prepareMagMsg
void prepareMagMsg(sensor_msgs::MagneticField &msg)
Definition: test_imu_transforms.cpp:209
prepareTf
void prepareTf(geometry_msgs::TransformStamped &tf)
Definition: test_imu_transforms.cpp:103

imu_transformer
Author(s): Paul Bovbel
autogenerated on Fri Nov 25 2022 03:51:09