00001 #include <imu_filter_madgwick/stateless_orientation.h> 00002 #include "test_helpers.h" 00003 00004 00005 template<WorldFrame::WorldFrame FRAME> 00006 bool computeOrientation( 00007 float Ax, float Ay, float Az, 00008 float Mx, float My, float Mz, 00009 double& q0, double& q1, double& q2, double& q3) { 00010 00011 geometry_msgs::Vector3 A, M; 00012 geometry_msgs::Quaternion orientation; 00013 A.x = Ax; A.y = Ay; A.z = Az; 00014 M.x = Mx; M.y = My; M.z = Mz; 00015 00016 bool res = StatelessOrientation::computeOrientation(FRAME, A, M, orientation); 00017 00018 q0 = orientation.w; 00019 q1 = orientation.x; 00020 q2 = orientation.y; 00021 q3 = orientation.z; 00022 00023 return res; 00024 } 00025 00026 template<WorldFrame::WorldFrame FRAME> 00027 bool computeOrientation( 00028 float Ax, float Ay, float Az, 00029 double& q0, double& q1, double& q2, double& q3) { 00030 00031 geometry_msgs::Vector3 A; 00032 geometry_msgs::Quaternion orientation; 00033 A.x = Ax; A.y = Ay; A.z = Az; 00034 00035 bool res = StatelessOrientation::computeOrientation(FRAME, A, orientation); 00036 00037 q0 = orientation.w; 00038 q1 = orientation.x; 00039 q2 = orientation.y; 00040 q3 = orientation.z; 00041 00042 return res; 00043 } 00044 00045 00046 #define TEST_STATELESS_ENU(in_am, exp_result) \ 00047 TEST(StatelessOrientationTest, Stationary_ENU_ ## in_am){ \ 00048 double q0, q1, q2, q3; \ 00049 ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(in_am, q0, q1, q2, q3)); \ 00050 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 00051 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 00052 TEST(StatelessOrientationTest, Stationary_ENU_NM_ ## in_am){ \ 00053 double q0, q1, q2, q3; \ 00054 ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 00055 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 00056 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 00057 00058 #define TEST_STATELESS_NED(in_am, exp_result) \ 00059 TEST(StatelessOrientationTest, Stationary_NED_ ## in_am){ \ 00060 double q0, q1, q2, q3; \ 00061 ASSERT_TRUE(computeOrientation<WorldFrame::NED>(in_am, q0, q1, q2, q3)); \ 00062 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 00063 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 00064 TEST(StatelessOrientationTest, Stationary_NED_NM_ ## in_am){ \ 00065 double q0, q1, q2, q3; \ 00066 ASSERT_TRUE(computeOrientation<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 00067 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 00068 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 00069 00070 #define TEST_STATELESS_NWU(in_am, exp_result) \ 00071 TEST(StatelessOrientationTest, Stationary_NWU_ ## in_am){ \ 00072 double q0, q1, q2, q3; \ 00073 ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(in_am, q0, q1, q2, q3)); \ 00074 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 00075 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 00076 TEST(StatelessOrientationTest, Stationary_NWU_NM_ ## in_am){ \ 00077 double q0, q1, q2, q3; \ 00078 ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 00079 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 00080 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 00081 00082 TEST_STATELESS_ENU(AM_EAST_NORTH_UP, QUAT_IDENTITY) 00083 TEST_STATELESS_ENU(AM_SOUTH_UP_WEST, QUAT_XMYMZ_120) 00084 TEST_STATELESS_ENU(AM_SOUTH_EAST_UP, QUAT_MZ_90) 00085 TEST_STATELESS_ENU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_ENU) 00086 TEST_STATELESS_ENU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_ENU) 00087 00088 00089 TEST_STATELESS_NED(AM_NORTH_EAST_DOWN, QUAT_IDENTITY) 00090 TEST_STATELESS_NED(AM_WEST_NORTH_DOWN, QUAT_MZ_90) 00091 TEST_STATELESS_NED(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NED) 00092 TEST_STATELESS_NED(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NED) 00093 00094 00095 TEST_STATELESS_NWU(AM_NORTH_EAST_DOWN, QUAT_X_180) 00096 TEST_STATELESS_NWU(AM_NORTH_WEST_UP, QUAT_IDENTITY) 00097 TEST_STATELESS_NWU(AM_WEST_NORTH_DOWN_RSD, QUAT_WEST_NORTH_DOWN_RSD_NWU) 00098 TEST_STATELESS_NWU(AM_NE_NW_UP_RSD, QUAT_NE_NW_UP_RSD_NWU) 00099 00100 00101 TEST(StatelessOrientationTest, Check_NoAccel){ 00102 double q0, q1, q2, q3; 00103 ASSERT_FALSE(computeOrientation<WorldFrame::ENU>( 00104 0.0, 0.0, 0.0, 00105 0.0, 0.0005, -0.0005, 00106 q0, q1, q2, q3)); 00107 } 00108 00109 TEST(StatelessOrientationTest, Check_NoMag){ 00110 double q0, q1, q2, q3; 00111 ASSERT_FALSE(computeOrientation<WorldFrame::ENU>( 00112 0.0, 0.0, 9.81, 00113 0.0, 0.0, 0.0, 00114 q0, q1, q2, q3)); 00115 } 00116 00117