velocity_iterator_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 
36 
37 
38 #include <gtest/gtest.h>
39 
41 
42 
43 namespace base_local_planner {
44 
45 TEST(VelocityIteratorTest, testsingle) {
46  double result[5];
47  int i = 0;
48  for(base_local_planner::VelocityIterator x_it(0.0, 0.0, 1); !x_it.isFinished(); x_it++) {
49  result[i] = x_it.getVelocity();
50  i++;
51  }
52  EXPECT_EQ(1, i);
53  EXPECT_EQ(0, result[0]);
54 }
55 
56 TEST(VelocityIteratorTest, testsingle_pos) {
57  double result[5];
58  int i = 0;
59  for(base_local_planner::VelocityIterator x_it(2.2, 2.2, 1); !x_it.isFinished(); x_it++) {
60  result[i] = x_it.getVelocity();
61  i++;
62  }
63  EXPECT_EQ(1, i);
64  EXPECT_EQ(2.2, result[0]);
65 }
66 
67 TEST(VelocityIteratorTest, testsingle_neg) {
68  double result[5];
69  int i = 0;
70  for(base_local_planner::VelocityIterator x_it(-3.3, -3.3, 1); !x_it.isFinished(); x_it++) {
71  result[i] = x_it.getVelocity();
72  i++;
73  }
74  EXPECT_EQ(1, i);
75  EXPECT_EQ(-3.3, result[0]);
76 }
77 
78 TEST(VelocityIteratorTest, test1) {
79  double result[5];
80  int i = 0;
81  for(base_local_planner::VelocityIterator x_it(-30, 30, 1); !x_it.isFinished(); x_it++) {
82  result[i] = x_it.getVelocity();
83  i++;
84  }
85  EXPECT_EQ(3, i);
86  double expected [3]= {-30.0, 0.0, 30.0};
87  for (int j = 0; j < 3; ++j) {
88  EXPECT_EQ(expected[j], result[j]);
89  }
90 }
91 
92 TEST(VelocityIteratorTest, test1_pos) {
93  double result[5];
94  int i = 0;
95  for(base_local_planner::VelocityIterator x_it(10, 30, 1); !x_it.isFinished(); x_it++) {
96  result[i] = x_it.getVelocity();
97  i++;
98  }
99  EXPECT_EQ(2, i);
100  double expected [2]= {10.0, 30.0};
101  for (int j = 0; j < 2; ++j) {
102  EXPECT_EQ(expected[j], result[j]);
103  }
104 }
105 
106 TEST(VelocityIteratorTest, test1_neg) {
107  double result[5];
108  int i = 0;
109  for(base_local_planner::VelocityIterator x_it(-30, -10, 1); !x_it.isFinished(); x_it++) {
110  result[i] = x_it.getVelocity();
111  i++;
112  }
113  EXPECT_EQ(2, i);
114  double expected [2]= {-30.0, -10.0};
115  for (int j = 0; j < 2; ++j) {
116  EXPECT_EQ(expected[j], result[j]);
117  }
118 }
119 
120 TEST(VelocityIteratorTest, test3) {
121  double result[5];
122  int i = 0;
123  for(base_local_planner::VelocityIterator x_it(-30, 30, 3); !x_it.isFinished(); x_it++) {
124  result[i] = x_it.getVelocity();
125  i++;
126  }
127  EXPECT_EQ(3, i);
128  double expected [3]= {-30.0, 0.0, 30};
129  for (int j = 0; j < 3; ++j) {
130  EXPECT_EQ(expected[j], result[j]);
131  }
132 }
133 
134 TEST(VelocityIteratorTest, test4) {
135  double result[5];
136  int i = 0;
137  for(base_local_planner::VelocityIterator x_it(-30, 30, 4); !x_it.isFinished(); x_it++) {
138  result[i] = x_it.getVelocity();
139  i++;
140  }
141  EXPECT_EQ(5, i);
142  double expected [5]= {-30.0, -10.0, 0.0, 10.0, 30};
143  for (int j = 0; j < 5; ++j) {
144  EXPECT_EQ(expected[j], result[j]);
145  }
146 }
147 
148 TEST(VelocityIteratorTest, test_shifted) {
149  // test where zero is not in the middle
150  double result[5];
151  int i = 0;
152  for(base_local_planner::VelocityIterator x_it(-10, 50, 4); !x_it.isFinished(); x_it++) {
153  result[i] = x_it.getVelocity();
154  i++;
155  }
156  EXPECT_EQ(5, i);
157  double expected [5]= {-10.0, 0.0, 10.0, 30, 50};
158  for (int j = 0; j < 5; ++j) {
159  EXPECT_EQ(expected[j], result[j]);
160  }
161 }
162 
163 TEST(VelocityIteratorTest, test_cranky) {
164  // test where one value is almost zero (nothing to do about that)
165  double result[5];
166  int i = 0;
167  for(base_local_planner::VelocityIterator x_it(-10.00001, 10, 3); !x_it.isFinished(); x_it++) {
168  result[i] = x_it.getVelocity();
169  i++;
170  }
171  EXPECT_EQ(4, i);
172  for (int j = 0; j < 5; ++j) {
173  double expected [5]= {-10.00001, -0.000005, 0.0, 10.0};
174  EXPECT_FLOAT_EQ(expected[j], result[j]);
175  }
176 }
177 
178 } // namespace
TEST(FootprintHelperTest, correctFootprint)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49