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
00035
00036
00037
00038 #include <gtest/gtest.h>
00039
00040 #include <base_local_planner/velocity_iterator.h>
00041
00042
00043 namespace base_local_planner {
00044
00045 TEST(VelocityIteratorTest, testsingle) {
00046 double result[5];
00047 int i = 0;
00048 for(base_local_planner::VelocityIterator x_it(0.0, 0.0, 1); !x_it.isFinished(); x_it++) {
00049 result[i] = x_it.getVelocity();
00050 i++;
00051 }
00052 EXPECT_EQ(1, i);
00053 EXPECT_EQ(0, result[0]);
00054 }
00055
00056 TEST(VelocityIteratorTest, testsingle_pos) {
00057 double result[5];
00058 int i = 0;
00059 for(base_local_planner::VelocityIterator x_it(2.2, 2.2, 1); !x_it.isFinished(); x_it++) {
00060 result[i] = x_it.getVelocity();
00061 i++;
00062 }
00063 EXPECT_EQ(1, i);
00064 EXPECT_EQ(2.2, result[0]);
00065 }
00066
00067 TEST(VelocityIteratorTest, testsingle_neg) {
00068 double result[5];
00069 int i = 0;
00070 for(base_local_planner::VelocityIterator x_it(-3.3, -3.3, 1); !x_it.isFinished(); x_it++) {
00071 result[i] = x_it.getVelocity();
00072 i++;
00073 }
00074 EXPECT_EQ(1, i);
00075 EXPECT_EQ(-3.3, result[0]);
00076 }
00077
00078 TEST(VelocityIteratorTest, test1) {
00079 double result[5];
00080 int i = 0;
00081 for(base_local_planner::VelocityIterator x_it(-30, 30, 1); !x_it.isFinished(); x_it++) {
00082 result[i] = x_it.getVelocity();
00083 i++;
00084 }
00085 EXPECT_EQ(3, i);
00086 double expected [3]= {-30.0, 0.0, 30.0};
00087 for (int j = 0; j < 3; ++j) {
00088 EXPECT_EQ(expected[j], result[j]);
00089 }
00090 }
00091
00092 TEST(VelocityIteratorTest, test1_pos) {
00093 double result[5];
00094 int i = 0;
00095 for(base_local_planner::VelocityIterator x_it(10, 30, 1); !x_it.isFinished(); x_it++) {
00096 result[i] = x_it.getVelocity();
00097 i++;
00098 }
00099 EXPECT_EQ(2, i);
00100 double expected [2]= {10.0, 30.0};
00101 for (int j = 0; j < 2; ++j) {
00102 EXPECT_EQ(expected[j], result[j]);
00103 }
00104 }
00105
00106 TEST(VelocityIteratorTest, test1_neg) {
00107 double result[5];
00108 int i = 0;
00109 for(base_local_planner::VelocityIterator x_it(-30, -10, 1); !x_it.isFinished(); x_it++) {
00110 result[i] = x_it.getVelocity();
00111 i++;
00112 }
00113 EXPECT_EQ(2, i);
00114 double expected [2]= {-30.0, -10.0};
00115 for (int j = 0; j < 2; ++j) {
00116 EXPECT_EQ(expected[j], result[j]);
00117 }
00118 }
00119
00120 TEST(VelocityIteratorTest, test3) {
00121 double result[5];
00122 int i = 0;
00123 for(base_local_planner::VelocityIterator x_it(-30, 30, 3); !x_it.isFinished(); x_it++) {
00124 result[i] = x_it.getVelocity();
00125 i++;
00126 }
00127 EXPECT_EQ(3, i);
00128 double expected [3]= {-30.0, 0.0, 30};
00129 for (int j = 0; j < 3; ++j) {
00130 EXPECT_EQ(expected[j], result[j]);
00131 }
00132 }
00133
00134 TEST(VelocityIteratorTest, test4) {
00135 double result[5];
00136 int i = 0;
00137 for(base_local_planner::VelocityIterator x_it(-30, 30, 4); !x_it.isFinished(); x_it++) {
00138 result[i] = x_it.getVelocity();
00139 i++;
00140 }
00141 EXPECT_EQ(5, i);
00142 double expected [5]= {-30.0, -10.0, 0.0, 10.0, 30};
00143 for (int j = 0; j < 5; ++j) {
00144 EXPECT_EQ(expected[j], result[j]);
00145 }
00146 }
00147
00148 TEST(VelocityIteratorTest, test_shifted) {
00149
00150 double result[5];
00151 int i = 0;
00152 for(base_local_planner::VelocityIterator x_it(-10, 50, 4); !x_it.isFinished(); x_it++) {
00153 result[i] = x_it.getVelocity();
00154 i++;
00155 }
00156 EXPECT_EQ(5, i);
00157 double expected [5]= {-10.0, 0.0, 10.0, 30, 50};
00158 for (int j = 0; j < 5; ++j) {
00159 EXPECT_EQ(expected[j], result[j]);
00160 }
00161 }
00162
00163 TEST(VelocityIteratorTest, test_cranky) {
00164
00165 double result[5];
00166 int i = 0;
00167 for(base_local_planner::VelocityIterator x_it(-10.00001, 10, 3); !x_it.isFinished(); x_it++) {
00168 result[i] = x_it.getVelocity();
00169 i++;
00170 }
00171 EXPECT_EQ(4, i);
00172 for (int j = 0; j < 5; ++j) {
00173 double expected [5]= {-10.00001, -0.000005, 0.0, 10.0};
00174 EXPECT_FLOAT_EQ(expected[j], result[j]);
00175 }
00176 }
00177
00178 }