00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031 #include <gtest/gtest.h>
00032 #include <sys/time.h>
00033 #include <vector>
00034 #include "filters/transfer_function.h"
00035
00036 using namespace filters ;
00037
00038
00039 TEST(MultiChannelTransferFunctionDoubleFilter, LowPass)
00040 {
00041 double epsilon = 1e-4;
00042
00043 MultiChannelFilterBase<double> * filter = new MultiChannelTransferFunctionFilter<double> ();
00044 EXPECT_TRUE(filter->configure(1, "LowPass" ));
00045
00046
00047 std::vector<double> in1,in2,in3,in4,in5,in6,in7;
00048 std::vector<double> out1;
00049
00050 in1.push_back(10.0);
00051 in2.push_back(70.0);
00052 in3.push_back(10.0);
00053 in4.push_back(44.0);
00054 in5.push_back(10.0);
00055 in6.push_back(5.0);
00056 in7.push_back(6.0);
00057 out1.push_back(11.8008);
00058 EXPECT_TRUE(filter->update(in1, in1));
00059 EXPECT_TRUE(filter->update(in2, in2));
00060 EXPECT_TRUE(filter->update(in3, in3));
00061 EXPECT_TRUE(filter->update(in4, in4));
00062 EXPECT_TRUE(filter->update(in5, in5));
00063 EXPECT_TRUE(filter->update(in6, in6));
00064 EXPECT_TRUE(filter->update(in7, in7));
00065
00066 EXPECT_NEAR(out1[0], in7[0], epsilon);
00067 }
00068
00069 TEST(SingleChannelTransferFunctionDoubleFilter, SingleLowPass)
00070 {
00071 double epsilon = 1e-4;
00072
00073 FilterBase<double> * filter = new SingleChannelTransferFunctionFilter<double> ();
00074 EXPECT_TRUE(filter->configure("LowPassSingle" ));
00075
00076
00077 double in1,in2,in3,in4,in5,in6,in7;
00078 double out1;
00079
00080 in1=10.0;
00081 in2=70.0;
00082 in3=10.0;
00083 in4=44.0;
00084 in5=10.0;
00085 in6=5.0;
00086 in7=6.0;
00087 out1=11.8008;
00088 EXPECT_TRUE(filter->update(in1, in1));
00089 EXPECT_TRUE(filter->update(in2, in2));
00090 EXPECT_TRUE(filter->update(in3, in3));
00091 EXPECT_TRUE(filter->update(in4, in4));
00092 EXPECT_TRUE(filter->update(in5, in5));
00093 EXPECT_TRUE(filter->update(in6, in6));
00094 EXPECT_TRUE(filter->update(in7, in7));
00095
00096 EXPECT_NEAR(out1, in7, epsilon);
00097 }
00098
00099
00100 TEST(MultiChannelTransferFunctionDoubleFilter, LowPassNonUnity)
00101 {
00102 double epsilon = 1e-4;
00103
00104 MultiChannelFilterBase<double> * filter = new MultiChannelTransferFunctionFilter<double> ();
00105 EXPECT_TRUE(filter->configure(1, "LowPassNonUnity" ));
00106
00107 std::vector<double> in1,in2,in3,in4,in5,in6,in7;
00108 std::vector<double> out1;
00109
00110 in1.push_back(10.0);
00111 in2.push_back(70.0);
00112 in3.push_back(10.0);
00113 in4.push_back(44.0);
00114 in5.push_back(10.0);
00115 in6.push_back(5.0);
00116 in7.push_back(6.0);
00117 out1.push_back(2.4088);
00118 EXPECT_TRUE(filter->update(in1, in1));
00119 EXPECT_TRUE(filter->update(in2, in2));
00120 EXPECT_TRUE(filter->update(in3, in3));
00121 EXPECT_TRUE(filter->update(in4, in4));
00122 EXPECT_TRUE(filter->update(in5, in5));
00123 EXPECT_TRUE(filter->update(in6, in6));
00124 EXPECT_TRUE(filter->update(in7, in7));
00125
00126 EXPECT_NEAR(out1[0], in7[0], epsilon);
00127 }
00128
00129 TEST(MultiChannelTransferFunctionDoubleFilter, LowPassMulti)
00130 {
00131 double epsilon = 1e-4;
00132
00133 MultiChannelFilterBase<double> * filter = new MultiChannelTransferFunctionFilter<double> ();
00134 EXPECT_TRUE(filter->configure(3, "LowPassMulti" ));
00135
00136 std::vector<double> in1,in2,in3,in4,in5,in6,in7;
00137 std::vector<double> out1;
00138
00139 in1.push_back(10.0);
00140 in1.push_back(10.0);
00141 in1.push_back(10.0);
00142
00143 in2.push_back(70.0);
00144 in2.push_back(30.0);
00145 in2.push_back(8.0);
00146
00147 in3.push_back(-1.0);
00148 in3.push_back(5.0);
00149 in3.push_back(22.0);
00150
00151 in4.push_back(44.0);
00152 in4.push_back(23.0);
00153 in4.push_back(8.0);
00154
00155 in5.push_back(10.0);
00156 in5.push_back(10.0);
00157 in5.push_back(10.0);
00158
00159 in6.push_back(5.0);
00160 in6.push_back(-1.0);
00161 in6.push_back(5.0);
00162
00163 in7.push_back(6.0);
00164 in7.push_back(-30.0);
00165 in7.push_back(2.0);
00166
00167 out1.push_back(60.6216);
00168 out1.push_back(33.9829);
00169 out1.push_back(28.1027);
00170 EXPECT_TRUE(filter->update(in1, in1));
00171 EXPECT_TRUE(filter->update(in2, in2));
00172 EXPECT_TRUE(filter->update(in3, in3));
00173 EXPECT_TRUE(filter->update(in4, in4));
00174 EXPECT_TRUE(filter->update(in5, in5));
00175 EXPECT_TRUE(filter->update(in6, in6));
00176 EXPECT_TRUE(filter->update(in7, in7));
00177
00178 for(unsigned int i=0; i<out1.size(); i++)
00179 {
00180 EXPECT_NEAR(out1[i], in7[i], epsilon);
00181 }
00182 }
00183
00184 TEST(MultiChannelTransferFunctionDoubleFilter, LowPassIrrational)
00185 {
00186 double epsilon = 1e-4;
00187
00188 MultiChannelFilterBase<double> * filter = new MultiChannelTransferFunctionFilter<double> ();
00189 EXPECT_TRUE(filter->configure(3, "LowPassIrrational" ));
00190
00191 std::vector<double> in1,in2,in3,in4,in5,in6,in7;
00192 std::vector<double> out1;
00193
00194 in1.push_back(10.0);
00195 in1.push_back(10.0);
00196 in1.push_back(10.0);
00197
00198 in2.push_back(70.0);
00199 in2.push_back(30.0);
00200 in2.push_back(8.0);
00201
00202 in3.push_back(-1.0);
00203 in3.push_back(5.0);
00204 in3.push_back(22.0);
00205
00206 in4.push_back(44.0);
00207 in4.push_back(23.0);
00208 in4.push_back(8.0);
00209
00210 in5.push_back(10.0);
00211 in5.push_back(10.0);
00212 in5.push_back(10.0);
00213
00214 in6.push_back(5.0);
00215 in6.push_back(-1.0);
00216 in6.push_back(5.0);
00217
00218 in7.push_back(6.0);
00219 in7.push_back(-30.0);
00220 in7.push_back(2.0);
00221
00222 out1.push_back(17.1112);
00223 out1.push_back(9.0285);
00224 out1.push_back(8.3102);
00225 EXPECT_TRUE(filter->update(in1, in1));
00226 EXPECT_TRUE(filter->update(in2, in2));
00227 EXPECT_TRUE(filter->update(in3, in3));
00228 EXPECT_TRUE(filter->update(in4, in4));
00229 EXPECT_TRUE(filter->update(in5, in5));
00230 EXPECT_TRUE(filter->update(in6, in6));
00231 EXPECT_TRUE(filter->update(in7, in7));
00232
00233 for(unsigned int i=0; i<out1.size(); i++)
00234 {
00235 EXPECT_NEAR(out1[i], in7[i], epsilon);
00236 }
00237 }
00238
00239 int main(int argc, char **argv){
00240 testing::InitGoogleTest(&argc, argv);
00241 ros::init(argc, argv, "test_transfer_function");
00242 return RUN_ALL_TESTS();
00243 }