test_routing.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <lanelet2_core/primitives/LaneletSequence.h>
3 
4 #include <algorithm>
5 
6 #include "lanelet2_routing/RoutingGraph.h"
7 #include "lanelet2_routing/internal/Graph.h"
8 #include "lanelet2_routing/internal/ShortestPath.h"
9 #include "test_routing_map.h"
10 
11 using namespace lanelet;
12 using namespace lanelet::routing;
13 using namespace lanelet::routing::internal;
14 using namespace lanelet::routing::tests;
15 
16 GraphType getSimpleGraph() {
17  /* 3
18  * (1)---(3)
19  * /1 \1 / \3
20  * (0) X (5)
21  * \2 /1 \ /1
22  * (2)----(4)
23  * 3
24  */
25  GraphType g;
26  auto v0 = boost::add_vertex(g);
27  auto v1 = boost::add_vertex(g);
28  auto v2 = boost::add_vertex(g);
29  auto v3 = boost::add_vertex(g);
30  auto v4 = boost::add_vertex(g);
31  auto v5 = boost::add_vertex(g);
32  auto addEdge = [](auto v0, auto v1, auto& g, double c) {
33  auto e = boost::add_edge(v0, v1, g);
34  g[e.first].routingCost = c;
35  };
36  addEdge(v0, v1, g, 1);
37  addEdge(v0, v2, g, 2);
38  addEdge(v1, v3, g, 3);
39  addEdge(v1, v4, g, 1);
40  addEdge(v2, v3, g, 1);
41  addEdge(v2, v4, g, 3);
42  addEdge(v3, v5, g, 3);
43  addEdge(v4, v5, g, 1);
44  return g;
45 }
46 
47 TEST(DijkstraSearch, onSimpleGraph) {
48  auto g = getSimpleGraph();
49  DijkstraStyleSearch<GraphType> searcher(g);
50  std::vector<double> expCost{0, 1, 2, 3, 2, 6};
51  std::vector<size_t> length{1, 2, 2, 3, 3, 4};
52  std::vector<GraphType::vertex_descriptor> predecessors{0, 0, 0, 2, 1, 3};
53  searcher.query(0, [&](const VertexVisitInformation& v) -> bool {
54  EXPECT_DOUBLE_EQ(expCost[v.vertex], v.cost) << v.vertex;
55  EXPECT_EQ(length[v.vertex], v.length) << v.vertex;
56  EXPECT_EQ(predecessors[v.vertex], v.predecessor) << v.vertex;
57  EXPECT_EQ(v.length, v.numLaneChanges + 1);
58  return v.vertex != 4;
59  });
60  EXPECT_EQ(searcher.getMap().size(), boost::num_vertices(g));
61  for (const auto& v : searcher.getMap()) {
62  EXPECT_EQ(v.second.predicate, v.first != 4) << v.first;
63  EXPECT_EQ(v.second.isLeaf, v.first == 5 || v.first == 4) << v.first;
64  }
65 }
66 
67 TEST_F(GermanPedestrianGraph, NumberOfLanelets) { // NOLINT
68  EXPECT_EQ(graph->passableSubmap()->laneletLayer.size(), 5ul);
69  EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2031));
70  EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2050));
71  EXPECT_EQ(graph->passableSubmap()->areaLayer.size(), 3ul);
72  EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3000));
73  EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3001));
74  EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3002));
75 }
76 
77 TEST_F(GermanBicycleGraph, NumberOfLanelets) { // NOLINT
78  EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2013));
79  EXPECT_FALSE(graph->passableSubmap()->laneletLayer.exists(2022));
80 }
81 
82 TEST_F(GermanVehicleGraph, GetShortestPath) { // NOLINT
83  // Multiple1
84  auto shortestPath = graph->shortestPath(lanelets.at(2001), lanelets.at(2004), 0);
85  ASSERT_TRUE(!!shortestPath);
86  EXPECT_EQ(shortestPath->size(), 3ul);
87  EXPECT_EQ((*shortestPath)[0], lanelets.at(2001));
88  EXPECT_EQ((*shortestPath)[1], lanelets.at(2003));
89  EXPECT_EQ((*shortestPath)[2], lanelets.at(2004));
90 
91  shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2002), 0);
92  ASSERT_TRUE(!!shortestPath);
93  EXPECT_EQ(shortestPath->size(), 3ul);
94  EXPECT_EQ((*shortestPath)[0], lanelets.at(2003));
95  EXPECT_EQ((*shortestPath)[1], lanelets.at(2001));
96  EXPECT_EQ((*shortestPath)[2], lanelets.at(2002));
97 
98  shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2001), 0);
99  ASSERT_TRUE(!!shortestPath);
100  EXPECT_EQ(shortestPath->size(), 2ul);
101  EXPECT_EQ((*shortestPath)[0], lanelets.at(2003));
102  EXPECT_EQ((*shortestPath)[1], lanelets.at(2001));
103 
104  shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2004), 0);
105  ASSERT_TRUE(!!shortestPath);
106  EXPECT_EQ(shortestPath->size(), 2ul);
107  EXPECT_EQ((*shortestPath)[0], lanelets.at(2003));
108  EXPECT_EQ((*shortestPath)[1], lanelets.at(2004));
109 
110  shortestPath = graph->shortestPath(lanelets.at(2001), lanelets.at(2002), 0);
111  ASSERT_TRUE(!!shortestPath);
112  EXPECT_EQ(shortestPath->size(), 2ul);
113  EXPECT_EQ((*shortestPath)[0], lanelets.at(2001));
114  EXPECT_EQ((*shortestPath)[1], lanelets.at(2002));
115 
116  shortestPath = graph->shortestPath(lanelets.at(2004), lanelets.at(2002), 0);
117  EXPECT_FALSE(!!shortestPath);
118 
119  shortestPath = graph->shortestPath(lanelets.at(2002), lanelets.at(2004), 0);
120  EXPECT_FALSE(!!shortestPath);
121 }
122 
123 TEST_F(GermanVehicleGraph, GetShortestPathMaxHose) { // NOLINT
124  auto shortestPath = graph->shortestPath(lanelets.at(2019), lanelets.at(2022), 0);
125  ASSERT_TRUE(!!shortestPath);
126  EXPECT_EQ(shortestPath->size(), 3ul);
127  EXPECT_EQ((*shortestPath)[0], lanelets.at(2019));
128  EXPECT_EQ((*shortestPath)[1], lanelets.at(2020));
129  EXPECT_EQ((*shortestPath)[2], lanelets.at(2022));
130 
131  shortestPath = graph->shortestPath(lanelets.at(2021), lanelets.at(2018), 0);
132  ASSERT_TRUE(!!shortestPath);
133  EXPECT_EQ(shortestPath->size(), 3ul);
134  EXPECT_EQ((*shortestPath)[0], lanelets.at(2021));
135  EXPECT_EQ((*shortestPath)[1], lanelets.at(2020).invert());
136  EXPECT_EQ((*shortestPath)[2], lanelets.at(2018));
137 
138  shortestPath = graph->shortestPath(lanelets.at(2019), lanelets.at(2018), 0);
139  EXPECT_FALSE(!!shortestPath);
140 }
141 
142 TEST_F(GermanVehicleGraph, GetShortestPathInvalid) { // NOLINT
143  ConstLanelet invalidLanelet;
144  auto shortestPath = graph->shortestPath(invalidLanelet, lanelets.at(2004), 0);
145  EXPECT_FALSE(!!shortestPath);
146 
147  EXPECT_THROW(graph->shortestPath(lanelets.at(2001), lanelets.at(2004), numCostModules), InvalidInputError); // NOLINT
148 }
149 
150 TEST_F(GermanVehicleGraph, GetShortestPathVia1) { // NOLINT
151  // Multiple1
152  ConstLanelets interm;
153  interm.push_back(static_cast<ConstLanelet>(lanelets.at(2003)));
154  auto shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2004), 0);
155  ASSERT_TRUE(!!shortestPath);
156  EXPECT_EQ(shortestPath->size(), 3ul);
157  EXPECT_EQ((*shortestPath)[0], lanelets.at(2001));
158  EXPECT_EQ((*shortestPath)[1], lanelets.at(2003));
159  EXPECT_EQ((*shortestPath)[2], lanelets.at(2004));
160 
161  shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2002), 0);
162  ASSERT_TRUE(!!shortestPath);
163  EXPECT_EQ(shortestPath->size(), 4ul);
164  EXPECT_EQ((*shortestPath)[0], lanelets.at(2001));
165  EXPECT_EQ((*shortestPath)[1], lanelets.at(2003));
166  EXPECT_EQ((*shortestPath)[2], lanelets.at(2001));
167  EXPECT_EQ((*shortestPath)[3], lanelets.at(2002));
168 
169  shortestPath = graph->shortestPathVia(lanelets.at(2004), interm, lanelets.at(2002), 0);
170  ASSERT_FALSE(!!shortestPath);
171 
172  shortestPath = graph->shortestPathVia(lanelets.at(2002), interm, lanelets.at(2004), 0);
173  ASSERT_FALSE(!!shortestPath);
174 }
175 
176 TEST_F(GermanVehicleGraph, GetShortestPathVia2) { // NOLINT
177  // Multiple1 -> Multiple2
178  ConstLanelets interm;
179  interm.push_back(static_cast<ConstLanelet>(lanelets.at(2003)));
180  interm.push_back(static_cast<ConstLanelet>(lanelets.at(2007)));
181  auto shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2013), 0);
182  ASSERT_TRUE(!!shortestPath);
183  EXPECT_EQ(shortestPath->size(), 8ul);
184  auto pathIt = shortestPath->begin();
185  EXPECT_EQ(*pathIt, lanelets.at(2001));
186  EXPECT_EQ(*++pathIt, lanelets.at(2003));
187  EXPECT_EQ(*++pathIt, lanelets.at(2004));
188  EXPECT_EQ(*++pathIt, lanelets.at(2005));
189  EXPECT_EQ(*++pathIt, lanelets.at(2007));
190  EXPECT_EQ(*++pathIt, lanelets.at(2008));
191  EXPECT_EQ(*++pathIt, lanelets.at(2010));
192  EXPECT_EQ(*++pathIt, lanelets.at(2013));
193 }
194 
195 TEST_F(GermanVehicleGraph, GetShortestPathViaInvalid) { // NOLINT
196  ConstLanelets interm;
197  interm.push_back(static_cast<ConstLanelet>(lanelets.at(2003)));
198  interm.push_back(static_cast<ConstLanelet>(lanelets.at(2007)));
199  // NOLINTNEXTLINE
200  EXPECT_THROW(graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2002), numCostModules), InvalidInputError);
201 }
202 
203 template <typename LaneletsT>
204 bool containsLanelet(const LaneletsT& reachableSet, Id lltId) {
205  return !!utils::findIf(reachableSet, [lltId](auto& llt) { return llt.id() == lltId; });
206 }
207 
208 TEST_F(GermanVehicleGraph, reachableSet) { // NOLINT
209  auto reachable = graph->reachableSet(lanelets.at(2001), 0., 0);
210  EXPECT_EQ(reachable.size(), 1ul);
211  EXPECT_TRUE(containsLanelet(reachable, 2001));
212 
213  reachable = graph->reachableSet(lanelets.at(2001), 2.1, 0);
214  EXPECT_EQ(reachable.size(), 3ul);
215  EXPECT_TRUE(containsLanelet(reachable, 2002));
216  EXPECT_TRUE(containsLanelet(reachable, 2003));
217 
218  reachable = graph->reachableSet(lanelets.at(2001), 100, 0);
219  EXPECT_EQ(reachable.size(), 15ul); // Will fail if people extend the map
220 
221  reachable = graph->reachableSet(lanelets.at(2002), 100, 0);
222  EXPECT_EQ(reachable.size(), 11ul); // Will fail if people extend the map
223 }
224 
225 TEST_F(GermanVehicleGraph, reachableSetMaxHose) { // NOLINT
226  auto reachable = graph->reachableSet(lanelets.at(2017), 100, 0);
227  EXPECT_EQ(reachable.size(), 22ul); // Will fail if people extend the map
228 
229  reachable = graph->reachableSet(lanelets.at(2021), 100, 0);
230  EXPECT_EQ(reachable.size(), 4ul);
231 }
232 
233 TEST_F(GermanVehicleGraph, reachableSetInvalid) { // NOLINT
234  EXPECT_THROW(graph->reachableSet(lanelets.at(2021), 0.0, numCostModules), InvalidInputError); // NOLINT
235  ConstLanelet invalid;
236  auto reachable = graph->reachableSet(invalid, 0, 0);
237  EXPECT_TRUE(reachable.empty());
238 }
239 
240 TEST_F(GermanPedestrianGraph, reachableSetCrossingWithArea) { // NOLINT
241  auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2050), 100);
242  EXPECT_EQ(reachable.size(), 6ul);
243  EXPECT_TRUE(containsLanelet(reachable, 2050));
244  EXPECT_TRUE(containsLanelet(reachable, 2053));
245  EXPECT_TRUE(containsLanelet(reachable, 2052));
246  EXPECT_TRUE(containsLanelet(reachable, 3000));
247  EXPECT_TRUE(containsLanelet(reachable, 3001));
248  EXPECT_TRUE(containsLanelet(reachable, 3002));
249 }
250 TEST_F(GermanPedestrianGraph, reachableSetStartingFromArea) { // NOLINT
251  auto reachable = graph->reachableSetIncludingAreas(areas.at(3000), 100);
252  EXPECT_EQ(reachable.size(), 5ul);
253 }
254 TEST_F(GermanPedestrianGraph, reachableSetWithAreaFromTwoWayLanelet) { // NOLINT
255  auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2053).invert(), 100);
256  EXPECT_TRUE(containsLanelet(reachable, 2053));
257  EXPECT_EQ(reachable.size(), 6ul);
258 }
259 TEST_F(GermanPedestrianGraph, reachableSetWithAreaFromUnconnectedLanelet) { // NOLINT
260  auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2051), 100);
261  EXPECT_EQ(reachable.size(), 1ul);
262 }
263 
264 TEST_F(GermanPedestrianGraph, possiblePathsWithAreaFromLanelet) { // NOLINT
265  auto reachable = graph->possiblePathsIncludingAreas(lanelets.at(2050), 10, 0, false);
266  ASSERT_EQ(reachable.size(), 3ul);
267  EXPECT_EQ(reachable[0].size(), 3ul);
268  EXPECT_EQ(reachable[1].size(), 3ul);
269  EXPECT_EQ(reachable[2].size(), 3ul);
270 }
271 
272 TEST_F(GermanPedestrianGraph, possiblePathsWithAreaFromUnconnectedLanelet) { // NOLINT
273  auto reachable = graph->possiblePathsIncludingAreas(lanelets.at(2050), 3, false);
274  ASSERT_EQ(reachable.size(), 3ul);
275  EXPECT_EQ(reachable[0].size(), 3ul);
276  EXPECT_EQ(reachable[1].size(), 3ul);
277  EXPECT_EQ(reachable[2].size(), 3ul);
278 }
279 
280 TEST_F(GermanVehicleGraph, possiblePathsMinRoutingCosts) { // NOLINT
281  // MIN ROUTING COST - With lane changes
282  // Multiple 1
283  auto routes = graph->possiblePaths(lanelets.at(2001), 2.2, 0, true);
284  EXPECT_EQ(routes.size(), 2ul);
285 
286  routes = graph->possiblePaths(lanelets.at(2002), 4.0, 0, true);
287  ASSERT_EQ(routes.size(), 1ul);
288  auto& firstRoute = *routes.begin();
289  EXPECT_EQ(firstRoute.size(), 3ul);
290  EXPECT_TRUE(containsLanelet(firstRoute, 2006));
291 }
292 
293 TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterLc) { // NOLINT
294  auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, {}, 0, true, true});
295  EXPECT_EQ(routes.size(), 3);
296  auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); });
297  EXPECT_TRUE(has(lastLLts, lanelets.at(2062)));
298  EXPECT_TRUE(has(lastLLts, lanelets.at(2049)));
299  EXPECT_TRUE(has(lastLLts, lanelets.at(2048)));
300 }
301 
302 TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterAllLimitsLc) { // NOLINT
303  auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, 100, 0, true, true});
304  EXPECT_EQ(routes.size(), 3);
305  auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); });
306  EXPECT_TRUE(has(lastLLts, lanelets.at(2062)));
307  EXPECT_TRUE(has(lastLLts, lanelets.at(2049)));
308  EXPECT_TRUE(has(lastLLts, lanelets.at(2048)));
309 }
310 
311 TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterNoLc) { // NOLINT
312  auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, {}, 0, false, true});
313  EXPECT_EQ(routes.size(), 3);
314  auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); });
315  EXPECT_TRUE(has(lastLLts, lanelets.at(2063)));
316  EXPECT_TRUE(has(lastLLts, lanelets.at(2049)));
317  EXPECT_TRUE(has(lastLLts, lanelets.at(2047)));
318 }
319 
320 TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterAllLimitsNoLc) { // NOLINT
321  auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, 100, 0, false, true});
322  EXPECT_EQ(routes.size(), 3);
323  auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); });
324  EXPECT_TRUE(has(lastLLts, lanelets.at(2063)));
325  EXPECT_TRUE(has(lastLLts, lanelets.at(2049)));
326  EXPECT_TRUE(has(lastLLts, lanelets.at(2047)));
327 }
328 
329 TEST_F(GermanVehicleGraph, possiblePathsLimitLengthNoLc) { // NOLINT
330  auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{2.5, 3, 0, false, false});
331  EXPECT_TRUE(std::all_of(routes.begin(), routes.end(), [](auto& r) { return r.size() <= 3; }));
332  EXPECT_EQ(routes.size(), 3);
333  auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); });
334  EXPECT_TRUE(has(lastLLts, lanelets.at(2042)));
335 }
336 
337 TEST_F(GermanVehicleGraph, possiblePathsMaxHose) { // NOLINT
338  // Max Hose Problem
339  auto routes = graph->possiblePaths(lanelets.at(2017), 10.0, 0, true);
340  ASSERT_EQ(routes.size(), 1ul);
341  auto& firstRoute = *routes.begin();
342  EXPECT_EQ(firstRoute.size(), 5ul);
343  EXPECT_TRUE(containsLanelet(firstRoute, 2024));
344 }
345 
346 TEST_F(GermanVehicleGraph, possiblePathsMinLanelets) { // NOLINT
347  // MIN NUMBER LANELETS - With lane changes
348  auto routes = graph->possiblePaths(lanelets.at(2001), 2, false);
349  EXPECT_EQ(routes.size(), 1ul);
350  auto& firstRoute = *routes.begin();
351  ASSERT_EQ(firstRoute.size(), 2ul);
352  EXPECT_TRUE(containsLanelet(firstRoute, 2002));
353  EXPECT_EQ(firstRoute.getRemainingLane(firstRoute.begin()).size(), firstRoute.size());
354 
355  routes = graph->possiblePaths(lanelets.at(2001), 30, false);
356  EXPECT_EQ(routes.size(), 0ul);
357 
358  routes = graph->possiblePaths(lanelets.at(2001), 10, true);
359  EXPECT_EQ(routes.size(), 0ul);
360 
361  routes = graph->possiblePaths(lanelets.at(2001), 7, true);
362  EXPECT_EQ(routes.size(), 3ul);
363 }
364 
365 TEST_F(GermanVehicleGraph, possiblePathsInvalid) { // NOLINT
366  // Invalid num costs length
367  EXPECT_THROW(graph->possiblePaths(lanelets.at(2002), 0., numCostModules, true), InvalidInputError); // NOLINT
368  auto routes = graph->possiblePaths(lanelets.at(2002), 0.);
369  ASSERT_EQ(routes.size(), 1ul);
370  EXPECT_EQ(routes[0].size(), 1ul);
371  ConstLanelet invalid;
372  routes = graph->possiblePaths(invalid, 10.0, 0, true);
373  EXPECT_EQ(routes.size(), 0ul);
374 
375  // Invalid min number lanelets
376  EXPECT_THROW(graph->possiblePaths(lanelets.at(2002), 1, numCostModules, true), InvalidInputError); // NOLINT
377  routes = graph->possiblePaths(invalid, 10, 0, true);
378  EXPECT_EQ(routes.size(), 0ul);
379 }
380 
381 TEST_F(GermanVehicleGraph, possiblePathsTowardsWithoutLc) { // NOLINT
382  auto routes = graph->possiblePathsTowards(lanelets.at(2024), 9, 0, false);
383  ASSERT_EQ(routes.size(), 1UL);
384  EXPECT_EQ(routes[0].front().id(), 2017);
385  EXPECT_EQ(routes[0].back().id(), 2024);
386 }
387 
388 TEST_F(GermanVehicleGraph, possiblePathsTowardsWithLc) { // NOLINT
389  auto routes = graph->possiblePathsTowards(lanelets.at(2015), 7, 0, true);
390  ASSERT_EQ(routes.size(), 2UL);
391  EXPECT_TRUE(containsLanelet(routes[0], 2009) || containsLanelet(routes[0], 2008));
392  EXPECT_TRUE(containsLanelet(routes[1], 2009) || containsLanelet(routes[1], 2008));
393 }
394 
395 TEST_F(GermanVehicleGraph, possiblePathsTowardsMinLanelets) { // NOLINT
396  auto routes = graph->possiblePathsTowards(lanelets.at(2015), 5, true);
397  ASSERT_EQ(routes.size(), 2UL);
398  EXPECT_TRUE(containsLanelet(routes[0], 2009) || containsLanelet(routes[0], 2008));
399  EXPECT_TRUE(containsLanelet(routes[1], 2009) || containsLanelet(routes[1], 2008));
400 }
401 
402 TEST_F(GermanVehicleGraph, forEachSuccessorIsMonotonic) { // NOLINT
403  double lastVal = 0.;
404  bool lanelet2010Seen = false;
405  bool lanelet2013Seen = false;
406  graph->forEachSuccessor(lanelets.at(2007), [&](const LaneletVisitInformation& i) {
407  if (i.cost == 0.) {
408  EXPECT_EQ(i.lanelet.id(), 2007) << "First lanelet must be 2007";
409  }
410  EXPECT_LE(lastVal, i.cost);
411  lastVal = i.cost;
412  lanelet2010Seen |= i.lanelet.id() == 2010;
413  lanelet2013Seen |= i.lanelet.id() == 2013;
414  return i.lanelet.id() != 2010 && i.lanelet.id() != 2014;
415  });
416  EXPECT_TRUE(lanelet2010Seen);
417  EXPECT_FALSE(lanelet2013Seen);
418 }
419 
420 TEST_F(GermanPedestrianGraph, forEachSuccessorIncludingAreasReachesLanelet) { // NOLINT
421  class TargetFound {};
422  auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) {
423  if (i.laneletOrArea.id() == 2053) {
424  throw TargetFound{};
425  }
426  return true;
427  };
428  EXPECT_THROW(graph->forEachSuccessorIncludingAreas(lanelets.at(2050), throwIfTarget), TargetFound); // NOLINT
429 }
430 
431 TEST_F(GermanPedestrianGraph, forEachPredecessorIncludingAreasReachesLanelet) { // NOLINT
432  class TargetFound {};
433  auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) {
434  if (i.laneletOrArea.id() == 2050) {
435  throw TargetFound{};
436  }
437  return true;
438  };
439  EXPECT_THROW(graph->forEachPredecessorIncludingAreas(lanelets.at(2053), throwIfTarget), TargetFound); // NOLINT
440 }
441 
442 TEST_F(GermanVehicleGraph, forEachPredecessorIncludingAreasReachesLanelet) { // NOLINT
443  class TargetFound {};
444  auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) {
445  if (i.laneletOrArea.id() == 2004) {
446  throw TargetFound{};
447  }
448  return true;
449  };
450  EXPECT_THROW(graph->forEachPredecessorIncludingAreas(lanelets.at(2007), throwIfTarget), TargetFound); // NOLINT
451 }
452 
453 TEST_F(GermanVehicleGraph, forEachPredecessorReachesLanelet) { // NOLINT
454  class TargetFound {};
455  auto throwIfTarget = [&](const LaneletVisitInformation& i) {
456  if (i.lanelet.id() == 2004) {
457  throw TargetFound{};
458  }
459  return true;
460  };
461  EXPECT_THROW(graph->forEachPredecessor(lanelets.at(2007), throwIfTarget), TargetFound); // NOLINT
462 }
463 
464 TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasFromArea) { // NOLINT
465  auto path =
466  graph->shortestPathIncludingAreas(ConstLaneletOrArea(areas.at(3001)), ConstLaneletOrArea(lanelets.at(2053)));
467  ASSERT_TRUE(!!path);
468  EXPECT_EQ(path->size(), 2ul);
469 }
470 
471 TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasThroughAreas) {
472  auto path =
473  graph->shortestPathIncludingAreas(ConstLaneletOrArea(lanelets.at(2050)), ConstLaneletOrArea(lanelets.at(2053)));
474  ASSERT_TRUE(!!path);
475  EXPECT_EQ(path->size(), 4ul);
476 }
477 
478 TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasViaThroughAreas) {
479  auto path =
480  graph->shortestPathIncludingAreasVia(ConstLaneletOrArea(lanelets.at(2050)), {ConstLaneletOrArea(areas.at(3002))},
481  ConstLaneletOrArea(lanelets.at(2053)));
482  ASSERT_TRUE(!!path);
483  EXPECT_EQ(path->size(), 6ul);
484 }
485 
486 TEST(RoutingCostInitialization, NegativeLaneChangeCost) { // NOLINT
487  EXPECT_NO_THROW(RoutingCostDistance(1)); // NOLINT
488  EXPECT_THROW(RoutingCostDistance(-1), InvalidInputError); // NOLINT
489 }
490 
491 class ConfigurationInitialization : public ::testing::Test {
492  public:
493  LaneletMapPtr emptyMap = std::make_shared<LaneletMap>();
494 
495  // Initialize traffic rules and routing cost calculation module
496  traffic_rules::TrafficRulesPtr trafficRules{traffic_rules::TrafficRulesFactory::create(
497  Locations::Germany, Participants::Vehicle, traffic_rules::TrafficRules::Configuration())};
498 
499  RoutingCostPtrs costPtrs{std::make_shared<RoutingCostDistance>(RoutingCostDistance{2.})};
500 
501  // Optional: Initialize config for routing graph:
502  RoutingGraph::Configuration routingGraphConf;
503 };
504 
505 TEST_F(ConfigurationInitialization, EmptyConfig) { // NOLINT
506  EXPECT_NO_THROW(RoutingGraph::build(*emptyMap, *trafficRules, costPtrs, routingGraphConf)); // NOLINT
507 }
508 
509 TEST_F(ConfigurationInitialization, Config) { // NOLINT
510  routingGraphConf.emplace(std::make_pair(RoutingGraph::ParticipantHeight, Attribute("2."))); // NOLINT
511  EXPECT_NO_THROW(RoutingGraph::build(*emptyMap, *trafficRules, costPtrs, routingGraphConf)); // NOLINT
512 }
513 
514 TEST_F(ConfigurationInitialization, NoConfig) { // NOLINT
515  EXPECT_NO_THROW(RoutingGraph::build(*emptyMap, *trafficRules, costPtrs)); // NOLINT
516 }
ConfigurationInitialization::routingGraphConf
RoutingGraph::Configuration routingGraphConf
Definition: test_routing.cpp:502
lanelet::Attribute
lanelet::routing::internal::DijkstraStyleSearch::query
void query(VertexType start, Func &&func)
Definition: ShortestPath.h:117
containsLanelet
bool containsLanelet(const LaneletsT &reachableSet, Id lltId)
Definition: test_routing.cpp:204
RoutingGraph.h
TEST
TEST(DijkstraSearch, onSimpleGraph)
Definition: test_routing.cpp:47
LaneletMapPtr
std::shared_ptr< LaneletMap > LaneletMapPtr
lanelet
lanelet::routing::RoutingCostPtrs
std::vector< RoutingCostPtr > RoutingCostPtrs
Definition: Forward.h:43
lanelet::routing::internal::VertexVisitInformation::cost
double cost
Definition: ShortestPath.h:18
TEST_F
TEST_F(GermanPedestrianGraph, NumberOfLanelets)
Definition: test_routing.cpp:67
lanelet::ConstLaneletOrArea
lanelet::routing::tests::GermanPedestrianGraph
Definition: test_routing_map.h:502
Id
int64_t Id
lanelet::routing::tests
Definition: test_routing_map.h:17
lanelet::routing
Definition: Forward.h:11
lanelet::routing::LaneletVisitInformation::cost
double cost
The accumulated cost from the start along shortest path.
Definition: Types.h:25
ShortestPath.h
lanelet::routing::internal
Definition: Forward.h:12
lanelet::routing::LaneletVisitInformation
This object carries the required information for the graph neighbourhood search.
Definition: Types.h:22
lanelet::routing::internal::VertexVisitInformation::predecessor
RoutingGraphGraph::Vertex predecessor
Definition: ShortestPath.h:17
lanelet::routing::tests::GermanVehicleGraph
Definition: test_routing_map.h:493
lanelet::routing::internal::VertexVisitInformation::length
size_t length
Definition: ShortestPath.h:19
lanelet::utils::transform
auto transform(Container &&c, Func f)
lanelet::routing::LaneletVisitInformation::lanelet
ConstLanelet lanelet
The lanelet or area that is currently visited.
Definition: Types.h:23
lanelet::routing::internal::DijkstraStyleSearch::getMap
const DijkstraSearchMapType & getMap() const
Returns the result.
Definition: ShortestPath.h:130
lanelet::Locations::Germany
static constexpr char Germany[]
lanelet::routing::PossiblePathsParams
Controls the behaviour of the different possible path algorithms in RoutingGraph.
Definition: RoutingGraph.h:50
lanelet::utils::findIf
auto findIf(ContainerT &&c, Func f)
lanelet::routing::tests::GermanBicycleGraph
Definition: test_routing_map.h:511
lanelet::routing::internal::VertexVisitInformation
This object carries the required information for the graph neighbourhood search.
Definition: ShortestPath.h:15
ConfigurationInitialization
Definition: test_routing.cpp:491
LaneletSequence.h
lanelet::Participants::Vehicle
static constexpr const char Vehicle[]
Graph.h
c
double c
Definition: RoutingGraph.cpp:260
lanelet::routing::RoutingCostDistance
A basic distance-based routing cost module. Uses the 2D length and a fixed lane change cost to evalua...
Definition: RoutingCost.h:48
lanelet::traffic_rules::TrafficRulesFactory::create
static TrafficRulesUPtr create(const std::string &location, const std::string &participant, TrafficRules::Configuration configuration=TrafficRules::Configuration())
lanelet::routing::internal::DijkstraStyleSearch
Definition: ShortestPath.h:59
lanelet::traffic_rules::TrafficRules::Configuration
std::map< std::string, Attribute > Configuration
lanelet::routing::internal::VertexVisitInformation::numLaneChanges
size_t numLaneChanges
Definition: ShortestPath.h:20
lanelet::ConstLanelet
lanelet::routing::RoutingGraph::Configuration
std::map< std::string, Attribute > Configuration
Definition: RoutingGraph.h:72
test_routing_map.h
ConstPrimitive< LaneletData >::id
Id id() const noexcept
getSimpleGraph
GraphType getSimpleGraph()
Definition: test_routing.cpp:16
lanelet::routing::LaneletOrAreaVisitInformation
This object carries the required information for the graph neighbourhood search.
Definition: Types.h:13
ConstLanelets
std::vector< ConstLanelet > ConstLanelets
lanelet::routing::internal::has
bool has(const ContainerT &c, const T &t)
Definition: GraphUtils.h:17
lanelet::routing::internal::GraphType
boost::adjacency_list< boost::vecS, boost::vecS, boost::bidirectionalS, VertexInfo, EdgeInfo > GraphType
General graph type definitions.
Definition: Graph.h:43
lanelet::routing::internal::VertexVisitInformation::vertex
RoutingGraphGraph::Vertex vertex
Definition: ShortestPath.h:16
lanelet::InvalidInputError
lanelet::traffic_rules::TrafficRulesPtr
std::shared_ptr< TrafficRules > TrafficRulesPtr

lanelet2_routing
Author(s): Matthias Mayr
autogenerated on Sun Oct 27 2024 02:27:49