Go to the documentation of this file.00001 #include <iostream>
00002 #include <gtest/gtest.h>
00003 #include "robodyn_utilities/SmoothDerivative.h"
00004 #include <ros/package.h>
00005 #include <stdexcept>
00006 #include <math.h>
00007 #include <fstream>
00008
00009 class SmoothDerivativeTest : public ::testing::Test
00010 {
00011 protected:
00012 virtual void SetUp()
00013 {
00014 ASSERT_NO_THROW(sd = new SmoothDerivative());
00015 }
00016
00017 virtual void TearDown()
00018 {
00019 }
00020
00021 SmoothDerivative* sd;
00022
00023 };
00024
00025
00026 TEST_F(SmoothDerivativeTest, initialize)
00027 {
00028 EXPECT_FALSE(sd->isInitialized);
00029
00030 std::vector<double> a_0;
00031 std::vector<double> a_smoothing;
00032
00033 EXPECT_TRUE(sd->Initialize(a_0, a_smoothing, 350));
00034
00035 EXPECT_TRUE(sd->isInitialized);
00036
00037 a_0.push_back(10);
00038
00039 EXPECT_FALSE(sd->Initialize(a_0, a_smoothing, 350));
00040 EXPECT_FALSE(sd->isInitialized);
00041
00042 a_smoothing.push_back(0.2);
00043
00044 EXPECT_TRUE(sd->Initialize(a_0, a_smoothing, 350));
00045
00046 }
00047
00048 TEST_F(SmoothDerivativeTest, OnExecute)
00049 {
00050 std::vector<double> a_0;
00051 std::vector<double> a;
00052 std::vector<double> adot;
00053 std::vector<double> a_smoothing;
00054
00055 a_0.push_back(0);
00056 a_smoothing.push_back(0.2);
00057
00058 EXPECT_THROW(sd->OnExecute(a, adot), std::runtime_error);
00059
00060 sd->Initialize(a_0, a_smoothing, 1);
00061
00062
00063 EXPECT_THROW(sd->OnExecute(a, adot), std::runtime_error);
00064
00065 a.push_back(0);
00066
00067 EXPECT_NO_THROW(sd->OnExecute(a, adot));
00068
00069 for (int i = 1; i < 100; ++i)
00070 {
00071 a[0] = i * 3;
00072
00073 EXPECT_NO_THROW(sd->OnExecute(a, adot));
00074
00075 EXPECT_EQ(1, a.size());
00076 EXPECT_EQ(1, adot.size());
00077
00078
00079 }
00080
00081 double oldvel = a[0];
00082 EXPECT_FLOAT_EQ(3, adot[0]);
00083
00084 for (int i = 1; i < 10000; ++i)
00085 {
00086 a[0] = oldvel;
00087
00088 EXPECT_NO_THROW(sd->OnExecute(a, adot));
00089
00090 EXPECT_EQ(1, a.size());
00091 EXPECT_EQ(1, adot.size());
00092
00093 }
00094
00095 EXPECT_NEAR(0, adot[0], 0.001);
00096
00097 }
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180 int main(int argc, char** argv)
00181 {
00182 testing::InitGoogleTest(&argc, argv);
00183 return RUN_ALL_TESTS();
00184 }