test_quaternion_operation.cpp
Go to the documentation of this file.
1 
12 //headers in Google Test
13 #include <gtest/gtest.h>
14 
15 //headers in ROS
16 #include <ros/ros.h>
17 
18 //headers in this package
19 #include <quaternion_operation/quaternion_operation.h>
20 
25 TEST(TestSuite, testCase1)
26 {
27  geometry_msgs::Quaternion ans,q1,q2;
28  q1.x = 0;
29  q1.y = 1;
30  q1.z = 0;
31  q1.w = 1;
32  q2.x = 0;
33  q2.y = 1;
34  q2.z = 0;
35  q2.w = 1;
36  ans = q1 + q2;
37  EXPECT_EQ(ans.x,0);
38  EXPECT_EQ(ans.y,2);
39  EXPECT_EQ(ans.z,0);
40  EXPECT_EQ(ans.w,2);
41 }
42 
47 TEST(TestSuite, testCase2)
48 {
49  geometry_msgs::Quaternion ans,q1,q2;
50  q1.x = 0;
51  q1.y = 1;
52  q1.z = 0;
53  q1.w = 0;
54  q2.x = 0;
55  q2.y = 0;
56  q2.z = 0;
57  q2.w = 1;
58  ans = q1 * q2;
59  EXPECT_EQ(quaternion_operation::equals(q1,ans),true);
60 }
61 
66 TEST(TestSuite, testCase3)
67 {
68  geometry_msgs::Quaternion q1;
69  q1.x = 0;
70  q1.y = 0;
71  q1.z = 0;
72  q1.w = 1;
73  Eigen::MatrixXd mat = quaternion_operation::getRotationMatrix(q1);
74  EXPECT_FLOAT_EQ(mat(0,0), 1.0);
75  EXPECT_FLOAT_EQ(mat(0,1), 0.0);
76  EXPECT_FLOAT_EQ(mat(0,2), 0.0);
77  EXPECT_FLOAT_EQ(mat(1,0), 0.0);
78  EXPECT_FLOAT_EQ(mat(1,1), 1.0);
79  EXPECT_FLOAT_EQ(mat(1,2), 0.0);
80  EXPECT_FLOAT_EQ(mat(2,0), 0.0);
81  EXPECT_FLOAT_EQ(mat(2,1), 0.0);
82  EXPECT_FLOAT_EQ(mat(2,2), 1.0);
83 }
84 
89 TEST(TestSuite, testCase4)
90 {
91  geometry_msgs::Quaternion q1;
92  q1.x = std::sqrt((double)0.5);
93  q1.y = 0;
94  q1.z = 0;
95  q1.w = std::sqrt((double)0.5);
96  Eigen::MatrixXd mat = quaternion_operation::getRotationMatrix(q1);
97  EXPECT_FLOAT_EQ(mat(0,0), 1.0);
98  EXPECT_FLOAT_EQ(mat(0,1), 0.0);
99  EXPECT_FLOAT_EQ(mat(0,2), 0.0);
100  EXPECT_FLOAT_EQ(mat(1,0), 0.0);
101  EXPECT_FLOAT_EQ(mat(1,1), 0.0);
102  EXPECT_FLOAT_EQ(mat(1,2), -1.0);
103  EXPECT_FLOAT_EQ(mat(2,0), 0.0);
104  EXPECT_FLOAT_EQ(mat(2,1), 1.0);
105  EXPECT_FLOAT_EQ(mat(2,2), 0.0);
106 }
107 
112 TEST(TestSuite, testCase5)
113 {
114  geometry_msgs::Quaternion q1,q2,ans;
115  q1.x = 0;
116  q1.y = 0;
117  q1.z = 0;
118  q1.w = 1;
119  q2.x = 0;
120  q2.y = 0;
121  q2.z = 0;
122  q2.w = 1;
123  ans = quaternion_operation::slerp(q1,q2,0);
124  EXPECT_EQ(quaternion_operation::equals(q1,ans),true);
125 }
126 
131 TEST(TestSuite, testCase6)
132 {
133  geometry_msgs::Quaternion q1,q2,ans;
134  q1.x = 0;
135  q1.y = 0;
136  q1.z = 0;
137  q1.w = 1;
138  q2.x = 1;
139  q2.y = 0;
140  q2.z = 0;
141  q2.w = 0;
142  ans = quaternion_operation::slerp(q1,q2,0);
143  EXPECT_EQ(quaternion_operation::equals(q1,ans),true);
144  ans = quaternion_operation::slerp(q1,q2,1);
145  EXPECT_EQ(quaternion_operation::equals(q2,ans),true);
146 }
147 
152 TEST(TestSuite, testCase7)
153 {
154  geometry_msgs::Quaternion q1,q2,ans;
155  q1.x = 0;
156  q1.y = 0;
157  q1.z = 0;
158  q1.w = 1;
159  q2.x = 1;
160  q2.y = 0;
161  q2.z = 0;
162  q2.w = 0;
163  ans = quaternion_operation::getRotation(q1,quaternion_operation::rotation(q1,q2));
164  EXPECT_EQ(quaternion_operation::equals(q2,ans),true);
165  q1.x = 0;
166  q1.y = 0;
167  q1.z = 0;
168  q1.w = 1;
169  q2.x = 0;
170  q2.y = 0;
171  q2.z = 1;
172  q2.w = 0;
173  ans = quaternion_operation::getRotation(q1,quaternion_operation::rotation(q1,q2));
174  EXPECT_EQ(quaternion_operation::equals(q2,ans),true);
175  q1.x = 0;
176  q1.y = 0;
177  q1.z = 0;
178  q1.w = 1;
179  q2.x = std::sqrt((double)0.5);
180  q2.y = std::sqrt((double)0.5);
181  q2.z = 0;
182  q2.w = 0;
183  ans = quaternion_operation::getRotation(q1,quaternion_operation::rotation(q1,q2));
184  EXPECT_EQ(quaternion_operation::equals(q2,ans),true);
185  q1.x = 0;
186  q1.y = 0;
187  q1.z = 0;
188  q1.w = 1;
189  q2.x = std::sqrt((double)0.4);
190  q2.y = std::sqrt((double)0.3);
191  q2.z = 0;
192  q2.w = std::sqrt((double)0.3);
193  ans = quaternion_operation::getRotation(q1,quaternion_operation::rotation(q1,q2));
194  EXPECT_EQ(quaternion_operation::equals(q2,ans),true);
195  q1.x = std::sqrt((double)0.8);
196  q1.y = 0;
197  q1.z = 0;
198  q1.w = std::sqrt((double)0.2);
199  q2.x = std::sqrt((double)0.4);
200  q2.y = std::sqrt((double)0.3);
201  q2.z = 0;
202  q2.w = std::sqrt((double)0.3);
203  ans = quaternion_operation::getRotation(q1,quaternion_operation::rotation(q1,q2));
204  EXPECT_EQ(quaternion_operation::equals(q2,ans),true);
205 }
206 
211 TEST(TestSuite, testCase8)
212 {
213  geometry_msgs::Quaternion q1,q2,ans;
214  q1.x = 1;
215  q1.y = 0;
216  q1.z = 0;
217  q1.w = 0;
218  q2.x = 0;
219  q2.y = 0;
220  q2.z = 0;
221  q2.w = 1;
222  ans = quaternion_operation::rotation(q1,q2);
223  EXPECT_EQ(quaternion_operation::equals(q1,ans),true);
224 }
225 
230 TEST(TestSuite, testCase9)
231 {
232  geometry_msgs::Quaternion q1,q2;
233  q1.x = std::sqrt((double)0.8);
234  q1.y = 0;
235  q1.z = 0;
236  q1.w = std::sqrt((double)0.2);
237  q2.x = std::sqrt((double)0.4);
238  q2.y = std::sqrt((double)0.3);
239  q2.z = 0;
240  q2.w = std::sqrt((double)0.3);
241  EXPECT_EQ(quaternion_operation::equals(q1*q2,q2*q1),false);
242 }
243 
248 TEST(TestSuite, testCase10)
249 {
250  geometry_msgs::Quaternion q;
251  geometry_msgs::Vector3 euler;
252  euler.x = 0;
253  euler.y = 0;
254  euler.z = 0;
255  q = quaternion_operation::convertEulerAngleToQuaternion(euler);
256  EXPECT_FLOAT_EQ(q.x,0);
257  EXPECT_FLOAT_EQ(q.y,0);
258  EXPECT_FLOAT_EQ(q.z,0);
259  EXPECT_FLOAT_EQ(q.w,1);
260 }
261 
266 TEST(TestSuite, testCase11)
267 {
268  geometry_msgs::Quaternion q;
269  geometry_msgs::Vector3 euler;
270  q.x = 0;
271  q.y = 0;
272  q.z = 0;
273  q.w = 1;
274  euler = quaternion_operation::convertQuaternionToEulerAngle(q);
275  EXPECT_FLOAT_EQ(euler.x,0);
276  EXPECT_FLOAT_EQ(euler.y,0);
277  EXPECT_FLOAT_EQ(euler.z,0);
278 }
279 
284 TEST(TestSuite, testCase12)
285 {
286  geometry_msgs::Quaternion q1;
287  q1.x = 0;
288  q1.y = std::sqrt((double)0.5);
289  q1.z = 0;
290  q1.w = std::sqrt((double)0.5);
291  Eigen::MatrixXd mat = quaternion_operation::getRotationMatrix(q1);
292  EXPECT_FLOAT_EQ(mat(0,0), 0.0);
293  EXPECT_FLOAT_EQ(mat(0,1), 0.0);
294  EXPECT_FLOAT_EQ(mat(0,2), 1.0);
295  EXPECT_FLOAT_EQ(mat(1,0), 0.0);
296  EXPECT_FLOAT_EQ(mat(1,1), 1.0);
297  EXPECT_FLOAT_EQ(mat(1,2), 0.0);
298  EXPECT_FLOAT_EQ(mat(2,0), -1.0);
299  EXPECT_FLOAT_EQ(mat(2,1), 0.0);
300  EXPECT_FLOAT_EQ(mat(2,2), 0.0);
301 }
302 
310 int main(int argc, char **argv){
311  testing::InitGoogleTest(&argc, argv);
312  return RUN_ALL_TESTS();
313 }
quaternion_operation::convertEulerAngleToQuaternion
geometry_msgs::Quaternion convertEulerAngleToQuaternion(geometry_msgs::Vector3 euler)
convert Euler angles to Quaternion
Definition: quaternion_operation.cpp:36
quaternion_operation::getRotation
geometry_msgs::Quaternion getRotation(geometry_msgs::Quaternion from, geometry_msgs::Quaternion to)
Get the Rotation from 2 Quaternions.
Definition: quaternion_operation.cpp:137
quaternion_operation.h
definitions of quaternion operation
main
int main(int argc, char **argv)
Run all the tests that were declared with TEST()
Definition: test_quaternion_operation.cpp:310
quaternion_operation::rotation
geometry_msgs::Quaternion rotation(geometry_msgs::Quaternion from, geometry_msgs::Quaternion rotation)
rotate Quaternion
Definition: quaternion_operation.cpp:132
quaternion_operation::convertQuaternionToEulerAngle
geometry_msgs::Vector3 convertQuaternionToEulerAngle(geometry_msgs::Quaternion quat)
convert Quaternion to the Euler angle
Definition: quaternion_operation.cpp:78
quaternion_operation::equals
bool equals(double a, double b)
checke 2 double values are equal or not
Definition: quaternion_operation.cpp:98
TEST
TEST(TestSuite, testCase1)
test for + operator
Definition: test_quaternion_operation.cpp:25
quaternion_operation::slerp
geometry_msgs::Quaternion slerp(geometry_msgs::Quaternion quat1, geometry_msgs::Quaternion quat2, double t)
Spherical linear interpolation function for geometry_msgs::Quaternion.
Definition: quaternion_operation.cpp:144
quaternion_operation::getRotationMatrix
Eigen::Matrix3d getRotationMatrix(geometry_msgs::Quaternion quat)
Get the Rotation Matrix from geometry_msgs::Quaternion.
Definition: quaternion_operation.cpp:64

quaternion_operation
Author(s):
autogenerated on Tue Jul 16 2019 03:20:48