$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 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 }