00001 #include <gtest/gtest.h> 00002 #include "robodyn_utilities/AbsSlewFilter.h" 00003 00004 TEST(AbsSlewFilterTest, FilterSameSignFloat) 00005 { 00006 AbsSlewFilter s; 00007 s.setRates(1.0, 2.0); 00008 00009 EXPECT_FLOAT_EQ(0.5, s.filter(0.0, 0.5)); 00010 EXPECT_FLOAT_EQ(0.0, s.filter(1.5, 0.0)); 00011 00012 EXPECT_FLOAT_EQ(1.0, s.filter(0.0, 5.0)); 00013 EXPECT_FLOAT_EQ(2.0, s.filter(1.0, 5.0)); 00014 EXPECT_FLOAT_EQ(3.0, s.filter(2.0, 5.0)); 00015 EXPECT_FLOAT_EQ(4.0, s.filter(3.0, 5.0)); 00016 EXPECT_FLOAT_EQ(5.0, s.filter(4.0, 5.0)); 00017 EXPECT_FLOAT_EQ(5.0, s.filter(5.0, 5.0)); 00018 EXPECT_FLOAT_EQ(3.0, s.filter(5.0, 0.0)); 00019 EXPECT_FLOAT_EQ(1.0, s.filter(3.0, 0.0)); 00020 EXPECT_FLOAT_EQ(0.0, s.filter(1.0, 0.0)); 00021 EXPECT_FLOAT_EQ(0.0, s.filter(0.0, 0.0)); 00022 00023 EXPECT_FLOAT_EQ(-0.5, s.filter(0.0, -0.5)); 00024 EXPECT_FLOAT_EQ(0.0, s.filter(-1.5, 0.0)); 00025 00026 EXPECT_FLOAT_EQ(-1.0, s.filter(0.0, -5.0)); 00027 EXPECT_FLOAT_EQ(-2.0, s.filter(-1.0, -5.0)); 00028 EXPECT_FLOAT_EQ(-3.0, s.filter(-2.0, -5.0)); 00029 EXPECT_FLOAT_EQ(-4.0, s.filter(-3.0, -5.0)); 00030 EXPECT_FLOAT_EQ(-5.0, s.filter(-4.0, -5.0)); 00031 EXPECT_FLOAT_EQ(-5.0, s.filter(-5.0, -5.0)); 00032 EXPECT_FLOAT_EQ(-3.0, s.filter(-5.0, 0.0)); 00033 EXPECT_FLOAT_EQ(-1.0, s.filter(-3.0, 0.0)); 00034 EXPECT_FLOAT_EQ(0.0, s.filter(-1.0, 0.0)); 00035 EXPECT_FLOAT_EQ(0.0, s.filter(0.0, 0.0)); 00036 } 00037 00038 TEST(AbsSlewFilterTest, FilterSameSignInteger) 00039 { 00040 AbsSlewFilter s; 00041 s.setRates(1, 2); 00042 00043 EXPECT_EQ(1, s.filter(0, 1)); 00044 EXPECT_EQ(0, s.filter(2, 0)); 00045 00046 EXPECT_EQ(1, s.filter(0, 5)); 00047 EXPECT_EQ(2, s.filter(1, 5)); 00048 EXPECT_EQ(3, s.filter(2, 5)); 00049 EXPECT_EQ(4, s.filter(3, 5)); 00050 EXPECT_EQ(5, s.filter(4, 5)); 00051 EXPECT_EQ(5, s.filter(5, 5)); 00052 EXPECT_EQ(3, s.filter(5, 0)); 00053 EXPECT_EQ(1, s.filter(3, 0)); 00054 EXPECT_EQ(0, s.filter(1, 0)); 00055 EXPECT_EQ(0, s.filter(0, 0)); 00056 00057 EXPECT_EQ(-1, s.filter(0, -1)); 00058 EXPECT_EQ(0, s.filter(-1, 0)); 00059 00060 EXPECT_EQ(-1, s.filter(0, -5)); 00061 EXPECT_EQ(-2, s.filter(-1, -5)); 00062 EXPECT_EQ(-3, s.filter(-2, -5)); 00063 EXPECT_EQ(-4, s.filter(-3, -5)); 00064 EXPECT_EQ(-5, s.filter(-4, -5)); 00065 EXPECT_EQ(-5, s.filter(-5, -5)); 00066 EXPECT_EQ(-3, s.filter(-5, 0)); 00067 EXPECT_EQ(-1, s.filter(-3, 0)); 00068 EXPECT_EQ(0, s.filter(-1, 0)); 00069 EXPECT_EQ(0, s.filter(0, 0)); 00070 } 00071 00072 TEST(AbsSlewFilterTest, FilterSignChange) 00073 { 00074 AbsSlewFilter s; 00075 s.setRates(1.0, 2.0); 00076 00077 EXPECT_FLOAT_EQ(0.0, s.filter(-5.0, 5.0)); 00078 EXPECT_FLOAT_EQ(0.0, s.filter(-10.0, 5.0)); 00079 EXPECT_FLOAT_EQ(0.0, s.filter(-5.0, 10.0)); 00080 } 00081 00082 int main(int argc, char** argv) 00083 { 00084 ::testing::InitGoogleTest(&argc, argv); 00085 return RUN_ALL_TESTS(); 00086 } 00087