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
00031
00032
00033
00034 #include <gtest/gtest.h>
00035 #include <dwb_plugins/one_d_velocity_iterator.h>
00036
00037 using dwb_plugins::OneDVelocityIterator;
00038
00039 const double EPSILON = 1e-3;
00040
00041 TEST(VelocityIterator, basics)
00042 {
00043 OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 2);
00044 EXPECT_FALSE(it.isFinished());
00045 EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
00046 EXPECT_FALSE(it.isFinished());
00047 ++it;
00048 EXPECT_FALSE(it.isFinished());
00049 EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
00050 EXPECT_FALSE(it.isFinished());
00051 ++it;
00052 EXPECT_TRUE(it.isFinished());
00053 it.reset();
00054 EXPECT_FALSE(it.isFinished());
00055 EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
00056 }
00057
00058 TEST(VelocityIterator, limits)
00059 {
00060 OneDVelocityIterator it(2.0, 1.5, 2.5, 1.0, -1.0, 1.0, 2);
00061 EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
00062 ++it;
00063 EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
00064 }
00065
00066 TEST(VelocityIterator, acceleration)
00067 {
00068 OneDVelocityIterator it(2.0, 0.0, 5.0, 0.5, -0.5, 1.0, 2);
00069 EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
00070 ++it;
00071 EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
00072 }
00073
00074
00075 TEST(VelocityIterator, time)
00076 {
00077 OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 0.5, 2);
00078 EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
00079 ++it;
00080 EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
00081 }
00082
00083 TEST(VelocityIterator, samples)
00084 {
00085 OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 3);
00086 EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
00087 ++it;
00088 EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
00089 ++it;
00090 EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
00091 ++it;
00092 EXPECT_TRUE(it.isFinished());
00093 }
00094
00095
00096 TEST(VelocityIterator, samples2)
00097 {
00098 OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 5);
00099 EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
00100 ++it;
00101 EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
00102 ++it;
00103 EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
00104 ++it;
00105 EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
00106 ++it;
00107 EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
00108 ++it;
00109 EXPECT_TRUE(it.isFinished());
00110 }
00111
00112 TEST(VelocityIterator, around_zero)
00113 {
00114 OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 2);
00115 EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
00116 ++it;
00117 EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
00118 ++it;
00119 EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
00120 ++it;
00121 }
00122
00123 TEST(VelocityIterator, around_zero2)
00124 {
00125 OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 4);
00126 EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
00127 ++it;
00128 EXPECT_NEAR(it.getVelocity(), -0.3333, EPSILON);
00129 ++it;
00130 EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
00131 ++it;
00132 EXPECT_NEAR(it.getVelocity(), 0.3333, EPSILON);
00133 ++it;
00134 EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
00135 ++it;
00136 }
00137
00138 int main(int argc, char **argv)
00139 {
00140 testing::InitGoogleTest(&argc, argv);
00141 return RUN_ALL_TESTS();
00142 }