stateless_orientation_test.cpp
Go to the documentation of this file.
2 #include "test_helpers.h"
3 
4 
5 template<WorldFrame::WorldFrame FRAME>
7  float Ax, float Ay, float Az,
8  float Mx, float My, float Mz,
9  double& q0, double& q1, double& q2, double& q3) {
10 
11  geometry_msgs::Vector3 A, M;
12  geometry_msgs::Quaternion orientation;
13  A.x = Ax; A.y = Ay; A.z = Az;
14  M.x = Mx; M.y = My; M.z = Mz;
15 
16  bool res = StatelessOrientation::computeOrientation(FRAME, A, M, orientation);
17 
18  q0 = orientation.w;
19  q1 = orientation.x;
20  q2 = orientation.y;
21  q3 = orientation.z;
22 
23  return res;
24 }
25 
26 template<WorldFrame::WorldFrame FRAME>
28  float Ax, float Ay, float Az,
29  double& q0, double& q1, double& q2, double& q3) {
30 
31  geometry_msgs::Vector3 A;
32  geometry_msgs::Quaternion orientation;
33  A.x = Ax; A.y = Ay; A.z = Az;
34 
35  bool res = StatelessOrientation::computeOrientation(FRAME, A, orientation);
36 
37  q0 = orientation.w;
38  q1 = orientation.x;
39  q2 = orientation.y;
40  q3 = orientation.z;
41 
42  return res;
43 }
44 
45 
46 #define TEST_STATELESS_ENU(in_am, exp_result) \
47  TEST(StatelessOrientationTest, Stationary_ENU_ ## in_am){ \
48  double q0, q1, q2, q3; \
49  ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(in_am, q0, q1, q2, q3)); \
50  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
51  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
52  TEST(StatelessOrientationTest, Stationary_ENU_NM_ ## in_am){ \
53  double q0, q1, q2, q3; \
54  ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
55  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
56  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
57 
58 #define TEST_STATELESS_NED(in_am, exp_result) \
59  TEST(StatelessOrientationTest, Stationary_NED_ ## in_am){ \
60  double q0, q1, q2, q3; \
61  ASSERT_TRUE(computeOrientation<WorldFrame::NED>(in_am, q0, q1, q2, q3)); \
62  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
63  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
64  TEST(StatelessOrientationTest, Stationary_NED_NM_ ## in_am){ \
65  double q0, q1, q2, q3; \
66  ASSERT_TRUE(computeOrientation<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
67  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
68  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
69 
70 #define TEST_STATELESS_NWU(in_am, exp_result) \
71  TEST(StatelessOrientationTest, Stationary_NWU_ ## in_am){ \
72  double q0, q1, q2, q3; \
73  ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(in_am, q0, q1, q2, q3)); \
74  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
75  ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \
76  TEST(StatelessOrientationTest, Stationary_NWU_NM_ ## in_am){ \
77  double q0, q1, q2, q3; \
78  ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \
79  ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \
80  ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); }
81 
87 
88 
91 TEST_STATELESS_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED)
93 
94 
95 TEST_STATELESS_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180)
97 TEST_STATELESS_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU)
99 
100 
101 TEST(StatelessOrientationTest, Check_NoAccel){
102  double q0, q1, q2, q3;
103  ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
104  0.0, 0.0, 0.0,
105  0.0, 0.0005, -0.0005,
106  q0, q1, q2, q3));
107 }
108 
109 TEST(StatelessOrientationTest, Check_NoMag){
110  double q0, q1, q2, q3;
111  ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
112  0.0, 0.0, 9.81,
113  0.0, 0.0, 0.0,
114  q0, q1, q2, q3));
115 }
116 
117 
#define QUAT_XMYMZ_120
Definition: test_helpers.h:93
#define QUAT_WEST_NORTH_DOWN_RSD_ENU
Definition: test_helpers.h:95
#define AM_NORTH_WEST_UP
Definition: test_helpers.h:76
#define QUAT_NE_NW_UP_RSD_ENU
Definition: test_helpers.h:98
#define QUAT_X_180
Definition: test_helpers.h:92
#define QUAT_WEST_NORTH_DOWN_RSD_NED
Definition: test_helpers.h:96
#define AM_SOUTH_EAST_UP
Definition: test_helpers.h:78
#define TEST_STATELESS_NWU(in_am, exp_result)
TEST(StatelessOrientationTest, Check_NoAccel)
#define AM_NE_NW_UP_RSD
Definition: test_helpers.h:83
#define AM_NORTH_EAST_DOWN
Definition: test_helpers.h:75
#define AM_WEST_NORTH_DOWN
Definition: test_helpers.h:79
#define TEST_STATELESS_ENU(in_am, exp_result)
#define AM_EAST_NORTH_UP
Definition: test_helpers.h:74
#define QUAT_MZ_90
Definition: test_helpers.h:91
#define AM_SOUTH_UP_WEST
Definition: test_helpers.h:77
#define AM_WEST_NORTH_DOWN_RSD
Definition: test_helpers.h:82
#define QUAT_NE_NW_UP_RSD_NWU
Definition: test_helpers.h:97
#define TEST_STATELESS_NED(in_am, exp_result)
static bool computeOrientation(WorldFrame::WorldFrame frame, geometry_msgs::Vector3 acceleration, geometry_msgs::Vector3 magneticField, geometry_msgs::Quaternion &orientation)
#define QUAT_NE_NW_UP_RSD_NED
Definition: test_helpers.h:99
#define QUAT_IDENTITY
Definition: test_helpers.h:90
#define QUAT_WEST_NORTH_DOWN_RSD_NWU
Definition: test_helpers.h:94
bool computeOrientation(float Ax, float Ay, float Az, float Mx, float My, float Mz, double &q0, double &q1, double &q2, double &q3)


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00