monocam_settler_unittest.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 #include <gtest/gtest.h>
36 
37 #include <monocam_settler/ConfigGoal.h>
39 
40 using namespace std;
41 using namespace monocam_settler;
42 
43 
44 
45 static const unsigned int N = 9;
46 static const unsigned int C = 2;
47 
48 static const double data[N][2*C] = { { 0, 0, 0, 0 },
49  { 1, 2, 1, 1 },
50  { 2, 4, 0, 0 },
51  { 3, 6, 1, 1 },
52  { 4, 8, 0, 0 },
53  { 0, 0, 1, 1 },
54  { 4, 1, 0, 0 },
55  { 8, 2, 1, 1 },
56  { 12, 3, 0, 0 }};
57 
58 // 2 possible sets of timestamps for the data
59 static const unsigned int times[N][2] = { { 0, 0 },
60  { 1, 1 },
61  { 2, 2 },
62  { 3, 3 },
63  { 4, 4 },
64  { 5, 15 },
65  { 6, 16 },
66  { 7, 17 },
67  { 8, 18 } };
68 
69 static const bool success[N] = { true,
70  true,
71  false,
72  true,
73  true,
74  true,
75  true,
76  true,
77  true };
78 
79 // add data to the settler, and see what intervals come out
80 vector<calibration_msgs::Interval> addToSettler(MonocamSettler& settler, unsigned int time_channel, bool always_success)
81 {
82  vector<calibration_msgs::Interval> intervals;
83 
84  for (unsigned int i=0; i<N; i++)
85  {
86  calibration_msgs::CalibrationPatternPtr msg(new calibration_msgs::CalibrationPattern);
87  msg->header.stamp = ros::Time(times[i][time_channel], 0);
88 
89  msg->image_points.resize(C);
90  for (unsigned int j=0; j<C; j++)
91  {
92  msg->image_points[j].x = data[i][2*j+0];
93  msg->image_points[j].y = data[i][2*j+1];
94  }
95  msg->success = always_success || success[i];
96 
97  calibration_msgs::Interval cur_interval;
98  bool result = settler.add(msg, cur_interval);
99  if (result)
100  intervals.push_back(cur_interval);
101  else
102  intervals.push_back(calibration_msgs::Interval());
103  }
104 
105  return intervals;
106 }
107 
108 void doEasyCheck(const vector<calibration_msgs::Interval>& intervals)
109 {
110  ASSERT_EQ(intervals.size(), N);
111  EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
112  EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
113 
114  EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
115  EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
116 
117  EXPECT_EQ(intervals[2].start.sec, (unsigned int) 1);
118  EXPECT_EQ(intervals[2].end.sec, (unsigned int) 2);
119 
120  EXPECT_EQ(intervals[3].start.sec, (unsigned int) 2);
121  EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
122 
123  EXPECT_EQ(intervals[4].start.sec, (unsigned int) 3);
124  EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
125 
126  EXPECT_EQ(intervals[5].start.sec, (unsigned int) 5);
127  EXPECT_EQ(intervals[5].end.sec, (unsigned int) 5);
128 
129  EXPECT_EQ(intervals[6].start.sec, (unsigned int) 6);
130  EXPECT_EQ(intervals[6].end.sec, (unsigned int) 6);
131 
132  EXPECT_EQ(intervals[7].start.sec, (unsigned int) 7);
133  EXPECT_EQ(intervals[7].end.sec, (unsigned int) 7);
134 
135  EXPECT_EQ(intervals[8].start.sec, (unsigned int) 8);
136  EXPECT_EQ(intervals[8].end.sec, (unsigned int) 8);
137 }
138 
139 void doIgnoreFailuresCheck(const vector<calibration_msgs::Interval>& intervals)
140 {
141  ASSERT_EQ(intervals.size(), N);
142  EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
143  EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
144 
145  EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
146  EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
147 
148  EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
149  EXPECT_EQ(intervals[2].end.sec, (unsigned int) 0);
150 
151  EXPECT_EQ(intervals[3].start.sec, (unsigned int) 0);
152  EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
153 
154  EXPECT_EQ(intervals[4].start.sec, (unsigned int) 0);
155  EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
156 
157  EXPECT_EQ(intervals[5].start.sec, (unsigned int) 0);
158  EXPECT_EQ(intervals[5].end.sec, (unsigned int) 5);
159 
160  EXPECT_EQ(intervals[6].start.sec, (unsigned int) 0);
161  EXPECT_EQ(intervals[6].end.sec, (unsigned int) 6);
162 
163  EXPECT_EQ(intervals[7].start.sec, (unsigned int) 0);
164  EXPECT_EQ(intervals[7].end.sec, (unsigned int) 7);
165 
166  EXPECT_EQ(intervals[8].start.sec, (unsigned int) 0);
167  EXPECT_EQ(intervals[8].end.sec, (unsigned int) 8);
168 }
169 
170 void doCatchFailuresCheck(const vector<calibration_msgs::Interval>& intervals)
171 {
172  ASSERT_EQ(intervals.size(), N);
173  EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
174  EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
175 
176  EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
177  EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
178 
179  EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
180  EXPECT_EQ(intervals[2].end.sec, (unsigned int) 0);
181 
182  EXPECT_EQ(intervals[3].start.sec, (unsigned int) 3);
183  EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
184 
185  EXPECT_EQ(intervals[4].start.sec, (unsigned int) 3);
186  EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
187 
188  EXPECT_EQ(intervals[5].start.sec, (unsigned int) 3);
189  EXPECT_EQ(intervals[5].end.sec, (unsigned int) 5);
190 
191  EXPECT_EQ(intervals[6].start.sec, (unsigned int) 3);
192  EXPECT_EQ(intervals[6].end.sec, (unsigned int) 6);
193 
194  EXPECT_EQ(intervals[7].start.sec, (unsigned int) 3);
195  EXPECT_EQ(intervals[7].end.sec, (unsigned int) 7);
196 
197  EXPECT_EQ(intervals[8].start.sec, (unsigned int) 3);
198  EXPECT_EQ(intervals[8].end.sec, (unsigned int) 8);
199 }
200 
201 ConfigGoal config1()
202 {
203  ConfigGoal config;
204 
205  config.tolerance = 2.5;
206 
207  config.ignore_failures = true;
208 
209  config.max_step = ros::Duration(2,0);
210 
211  config.cache_size = 100;
212 
213  return config;
214 }
215 
216 ConfigGoal config2(bool ignore_failures)
217 {
218  ConfigGoal config;
219 
220  config.tolerance = 100.0;
221 
222  config.ignore_failures = ignore_failures;
223 
224  config.max_step = ros::Duration(2,0);
225 
226  config.cache_size = 100;
227 
228  return config;
229 }
230 
231 TEST(MonocamSettler, easyCheck)
232 {
233  MonocamSettler settler;
234 
235  bool config_result = settler.configure(config1());
236  ASSERT_TRUE(config_result);
237 
238  vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, true);
239 
240  doEasyCheck(intervals);
241 }
242 
243 TEST(MonocamSettler, ignoreFailuresCheck)
244 {
245  MonocamSettler settler;
246 
247  bool config_result = settler.configure(config2(true));
248  ASSERT_TRUE(config_result);
249 
250  vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, false);
251 
252  doIgnoreFailuresCheck(intervals);
253 }
254 
255 TEST(MonocamSettler, catchFailuresCheck)
256 {
257  MonocamSettler settler;
258 
259  bool config_result = settler.configure(config2(false));
260  ASSERT_TRUE(config_result);
261 
262  vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, false);
263 
264  doCatchFailuresCheck(intervals);
265 }
266 
267 TEST(MonocamSettler, reconfigureCheck)
268 {
269  MonocamSettler settler;
270 
271  bool config_result = settler.configure(config1());
272  ASSERT_TRUE(config_result);
273 
274  vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, true);
275 
276  doEasyCheck(intervals);
277 
278  config_result = settler.configure(config2(false));
279  ASSERT_TRUE(config_result);
280 
281  intervals = addToSettler(settler, 0, false);
282 
283  doCatchFailuresCheck(intervals);
284 }
285 
286 int main(int argc, char **argv)
287 {
288  testing::InitGoogleTest(&argc, argv);
289  return RUN_ALL_TESTS();
290 }
static const bool success[N]
bool add(const calibration_msgs::CalibrationPatternConstPtr msg, calibration_msgs::Interval &interval)
ConfigGoal config2(bool ignore_failures)
ConfigGoal config1()
ROSCPP_DECL void start()
void doCatchFailuresCheck(const vector< calibration_msgs::Interval > &intervals)
static const unsigned int N
void doEasyCheck(const vector< calibration_msgs::Interval > &intervals)
static const unsigned int times[N][2]
bool configure(const monocam_settler::ConfigGoal &goal)
void doIgnoreFailuresCheck(const vector< calibration_msgs::Interval > &intervals)
static const unsigned int C
int main(int argc, char **argv)
TEST(MonocamSettler, easyCheck)
vector< calibration_msgs::Interval > addToSettler(MonocamSettler &settler, unsigned int time_channel, bool always_success)
static const double data[N][2 *C]


monocam_settler
Author(s): Vijay Pradeep
autogenerated on Fri Apr 2 2021 02:13:05