test_grid_astar.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <list>
31 #include <unordered_map>
32 #include <vector>
33 
34 #include <boost/thread.hpp>
35 
36 #include <omp.h>
37 
38 #include <gtest/gtest.h>
39 
41 
42 namespace planner_cspace
43 {
44 TEST(GridAstar, ParallelSearch)
45 {
46  using Vec = CyclicVecInt<1, 1>;
47  GridAstar<1, 1> as(Vec(16));
48  as.setSearchTaskNum(8);
49  omp_set_num_threads(2);
50 
51  class Model : public GridAstarModelBase<1, 1>
52  {
53  private:
54  std::vector<std::vector<Vec>> search_;
55 
56  public:
57  Model()
58  : search_(16)
59  {
60  // create search table of graph edges (relative vector to the connected grid)
61  // 0: connected to 1-14
62  // 1-14: connected to 1-15
63  for (int i = 1; i <= 14; ++i)
64  {
65  search_[0].push_back(Vec(i));
66  for (int j = 1; j <= 15; ++j)
67  {
68  if (i == j)
69  continue;
70  search_[i].push_back(Vec(j - i));
71  }
72  }
73  }
74  float cost(const Vec&, const Vec&, const std::vector<VecWithCost>&, const Vec&) const final
75  {
76  return 1.0;
77  }
78  float costEstim(const Vec& s, const Vec& e) const final
79  {
80  return 0.0;
81  }
82  const std::vector<Vec>& searchGrids(const Vec& p, const std::vector<VecWithCost>&, const Vec&) const final
83  {
84  return search_[p[0]];
85  }
86  };
87  Model::Ptr model(new Model());
88 
89  const auto cb_progress = [](const std::list<Vec>&, const SearchStats&) -> bool
90  {
91  return true;
92  };
93 
94  for (int i = 0; i < 1000; ++i)
95  {
96  std::list<Vec> path;
97  std::vector<Model::VecWithCost> starts;
98  starts.emplace_back(Vec(0));
99  ASSERT_TRUE(
100  as.search(
101  starts, Vec(15), path,
102  model, cb_progress,
103  0, 1.0));
104 
105  ASSERT_EQ(path.size(), 3u);
106  ASSERT_EQ(path.front(), Vec(0));
107  ASSERT_EQ(path.back(), Vec(15));
108  }
109 }
110 
111 TEST(GridAstar, TimeoutAbort)
112 {
113  using Vec = CyclicVecInt<1, 1>;
114  GridAstar<1, 1> as(Vec(2));
115  as.setSearchTaskNum(8);
116  omp_set_num_threads(2);
117 
118  class Model : public GridAstarModelBase<1, 1>
119  {
120  private:
121  std::vector<std::vector<Vec>> search_;
122 
123  public:
124  Model()
125  : search_(3)
126  {
127  search_[0].push_back(Vec(1));
128  search_[1].push_back(Vec(2));
129  }
130  float cost(const Vec&, const Vec&, const std::vector<VecWithCost>&, const Vec&) const final
131  {
132  return 1.0;
133  }
134  float costEstim(const Vec& s, const Vec& e) const final
135  {
136  return 0.0;
137  }
138  const std::vector<Vec>& searchGrids(const Vec& p, const std::vector<VecWithCost>&, const Vec&) const final
139  {
140  return search_[p[0]];
141  }
142  };
143  Model::Ptr model(new Model());
144 
145  int cnt(0);
146  const auto cb_progress = [&cnt](const std::list<Vec>& /* path_grid */, const SearchStats& stats) -> bool
147  {
148  switch (cnt++)
149  {
150  case 0:
151  EXPECT_EQ(1u, stats.num_loop);
152  EXPECT_EQ(1u, stats.num_search_queue);
153  EXPECT_EQ(0u, stats.num_prev_updates);
154  EXPECT_EQ(0u, stats.num_total_updates);
155  return true;
156  case 1:
157  EXPECT_EQ(2u, stats.num_loop);
158  EXPECT_EQ(1u, stats.num_search_queue);
159  EXPECT_EQ(1u, stats.num_prev_updates);
160  EXPECT_EQ(1u, stats.num_total_updates);
161  return false;
162  }
163  EXPECT_TRUE(false) << "Search was not aborted";
164  return false;
165  };
166 
167  std::list<Vec> path;
168  std::vector<Model::VecWithCost> starts;
169  starts.emplace_back(Vec(0));
170  ASSERT_FALSE(as.search(starts, Vec(2), path, model, cb_progress, 0, 0.0));
171 }
172 
173 TEST(GridAstar, SearchWithMultipleStarts)
174 {
175  using Vec = CyclicVecInt<1, 1>;
176  GridAstar<1, 1> as(Vec(16));
177  as.setSearchTaskNum(1);
178 
179  class Model : public GridAstarModelBase<1, 1>
180  {
181  private:
182  std::vector<std::vector<Vec>> search_;
183 
184  public:
185  Model()
186  : search_(16)
187  {
188  // create search table of graph edges (relative vector to the connected grid)
189  // 0: connected to 2-14
190  // 1: connected to 2-14
191  // 2-14: connected to 2-15
192  for (int i = 2; i <= 14; ++i)
193  {
194  search_[0].push_back(Vec(i));
195  search_[1].push_back(Vec(i - 1));
196  for (int j = 2; j <= 15; ++j)
197  {
198  if (i == j)
199  continue;
200  search_[i].push_back(Vec(j - i));
201  }
202  }
203  }
204  float cost(const Vec&, const Vec&, const std::vector<VecWithCost>&, const Vec&) const final
205  {
206  return 1.0;
207  }
208  float costEstim(const Vec& s, const Vec& e) const final
209  {
210  return 0.0;
211  }
212  const std::vector<Vec>& searchGrids(const Vec& p, const std::vector<VecWithCost>&, const Vec&) const final
213  {
214  return search_[p[0]];
215  }
216  };
217  Model::Ptr model(new Model());
218 
219  const auto cb_progress = [](const std::list<Vec>&, const SearchStats&) -> bool
220  {
221  return true;
222  };
223 
224  for (int add_cost_to = 0; add_cost_to < 2; ++add_cost_to)
225  {
226  std::vector<GridAstar<1, 1>::VecWithCost> starts;
227  starts.emplace_back(Vec(0));
228  starts.emplace_back(Vec(1));
229  starts[add_cost_to].c_ = 0.1;
230 
231  std::list<Vec> path;
232  ASSERT_TRUE(
233  as.search(
234  starts, Vec(15), path,
235  model, cb_progress,
236  0, 1.0))
237  << add_cost_to;
238  ASSERT_EQ(path.size(), 3u);
239  ASSERT_EQ(path.back(), Vec(15));
240  if (add_cost_to == 0)
241  ASSERT_EQ(path.front(), Vec(1));
242  else
243  ASSERT_EQ(path.front(), Vec(0));
244  }
245 }
246 
247 class GridAstarTestWrapper : public GridAstar<1, 1>
248 {
249 public:
250  explicit GridAstarTestWrapper(const Vec& size)
251  : GridAstar(size)
252  {
253  }
254  std::unordered_map<Vec, Vec, Vec>& parentMap()
255  {
256  return parents_;
257  }
258  bool findPath(const std::vector<VecWithCost>& ss, const Vec& e, std::list<Vec>& path) const
259  {
260  return GridAstar::findPath(ss, e, path);
261  }
262 };
263 
264 TEST(GridAstar, FindPathLooped)
265 {
267  GridAstarTestWrapper as(Vec(4));
268 
269  as.parentMap()[Vec(3)] = Vec(2);
270  as.parentMap()[Vec(2)] = Vec(1);
271  as.parentMap()[Vec(1)] = Vec(2);
272 
273  std::list<Vec> path;
274  const auto timeout_func = []()
275  {
276  try
277  {
278  boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
279  }
280  catch (boost::thread_interrupted&)
281  {
282  return;
283  }
284  EXPECT_TRUE(false) << "Looks entered endless loop. Test will be aborted.";
285  abort();
286  };
287  boost::thread timeout(timeout_func);
288  std::vector<GridAstarTestWrapper::VecWithCost> starts;
289  starts.emplace_back(Vec(0));
290  ASSERT_FALSE(as.findPath(starts, Vec(3), path));
291  timeout.interrupt();
292 }
293 
294 TEST(GridAstar, FindPathUnconnected)
295 {
297  GridAstarTestWrapper as(Vec(3));
298 
299  as.parentMap()[Vec(2)] = Vec(1);
300 
301  std::list<Vec> path;
302  std::vector<GridAstarTestWrapper::VecWithCost> starts;
303  starts.emplace_back(Vec(0));
304  ASSERT_FALSE(as.findPath(starts, Vec(2), path));
305 }
306 
307 TEST(GridAstar, FindPath)
308 {
310  GridAstarTestWrapper as(Vec(3));
311 
312  as.parentMap()[Vec(2)] = Vec(1);
313  as.parentMap()[Vec(1)] = Vec(0);
314 
315  // findPath must return same result for multiple calls
316  for (int i = 0; i < 2; ++i)
317  {
318  std::list<Vec> path;
319  std::vector<GridAstarTestWrapper::VecWithCost> starts;
320  starts.emplace_back(Vec(0));
321  ASSERT_TRUE(as.findPath(starts, Vec(2), path));
322  ASSERT_EQ(path.size(), 3u);
323  auto it = path.cbegin();
324  ASSERT_EQ(*(it++), Vec(0));
325  ASSERT_EQ(*(it++), Vec(1));
326  ASSERT_EQ(*it, Vec(2));
327  }
328 }
329 } // namespace planner_cspace
330 
331 int main(int argc, char** argv)
332 {
333  testing::InitGoogleTest(&argc, argv);
334 
335  return RUN_ALL_TESTS();
336 }
TEST(BlockmemGridmap, BlockWidth)
void setSearchTaskNum(const size_t &search_task_num)
Definition: grid_astar.h:142
bool search(const std::vector< VecWithCost > &ss, const Vec &e, std::list< Vec > &path, const typename GridAstarModelBase< DIM, NONCYCLIC >::Ptr &model, ProgressCallback cb_progress, const float cost_leave, const float progress_interval, const bool return_best=false)
Definition: grid_astar.h:169
XmlRpcServer s
CyclicVecInt< DIM, NONCYCLIC > Vec
Definition: grid_astar.h:67
std::unordered_map< Vec, Vec, Vec > & parentMap()
int main(int argc, char **argv)
bool findPath(const std::vector< VecWithCost > &ss, const Vec &e, std::list< Vec > &path) const
bool findPath(const std::vector< VecWithCost > &ss, const Vec &e, std::list< Vec > &path) const
Definition: grid_astar.h:385


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42