1 #include <gtest/gtest.h>
11 template <
Id FromId,
Id ToId,
Id... ViaIds>
14 explicit TestRoute(uint16_t routingCostId = 0,
bool withLaneChanges =
true) : withLaneChanges{withLaneChanges} {
15 Ids viaIds{ViaIds...};
16 start = lanelets.at(From);
17 end = lanelets.at(To);
19 auto optRoute = via.empty() ? graph->getRoute(start, end, routingCostId, withLaneChanges)
20 : graph->getRouteVia(start, via, end, routingCostId, withLaneChanges);
21 EXPECT_TRUE(!!optRoute);
22 route = std::move(*optRoute);
24 bool withLaneChanges{
true};
30 const Id From{FromId};
74 auto route2 = this->graph->getRouteVia(this->start, this->via, this->end, 2, this->withLaneChanges);
75 EXPECT_TRUE(!!route2);
76 auto map0 = this->route.laneletSubmap();
77 auto map2 = route2->laneletSubmap();
78 EXPECT_TRUE(std::equal(map0->laneletLayer.begin(), map0->laneletLayer.end(), map2->laneletLayer.begin(),
79 map2->laneletLayer.end()));
83 const auto map{this->route.getDebugLaneletMap()};
84 for (
const auto& el : this->route.shortestPath()) {
85 ASSERT_NE(map->pointLayer.find(el.id()), map->pointLayer.end());
86 const Point3d point{*map->pointLayer.find(el.id())};
87 EXPECT_TRUE(*point.attribute(
"shortest_path").asBool());
92 const LaneletMapPtr debugMap{this->route.getDebugLaneletMap()};
95 for (
const auto& it : laneletMap->laneletLayer) {
96 ASSERT_NE(debugMap->pointLayer.find(it.id()), debugMap->pointLayer.end());
97 Point3d point{*debugMap->pointLayer.find(it.id())};
98 EXPECT_NO_THROW(point.attribute(
"id"));
99 EXPECT_NO_THROW(point.attribute(
"lane_id"));
102 for (
const auto& it : debugMap->lineStringLayer) {
103 EXPECT_NO_THROW(it.attribute(
"relation_1"));
108 EXPECT_EQ(route.size(), 15ul);
109 EXPECT_GT(route.length2d(), 10.);
110 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
111 EXPECT_EQ(route.remainingShortestPath(start), route.shortestPath());
112 EXPECT_TRUE(route.remainingShortestPath(lanelets.at(2003)).empty());
113 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
114 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
115 EXPECT_NE(route.laneletSubmap(),
nullptr);
116 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
121 return utils::anyOf(relations, [&rel](
auto& relation) {
return rel == relation; });
124 template <
typename ContainerT>
126 return utils::anyOf(llts, [&llt](
auto& curr) {
return curr == llt; });
131 EXPECT_EQ(previous.size(), 2ul);
136 EXPECT_EQ(following.size(), 2ul);
142 auto conflictingInMap{route.conflictingInMap(lanelets.at(2009))};
143 EXPECT_EQ(conflictingInMap.size(), 1ul);
144 EXPECT_TRUE(
hasLanelet(conflictingInMap, lanelets.at(2008)));
146 ConstLanelets conflictingInRoute{route.conflictingInRoute(lanelets.at(2009))};
147 EXPECT_EQ(conflictingInRoute.size(), 1ul);
148 EXPECT_TRUE(
hasLanelet(conflictingInRoute, lanelets.at(2008)));
150 EXPECT_TRUE(route.conflictingInRoute(lanelets.at(2007)).empty());
151 EXPECT_TRUE(route.conflictingInMap(lanelets.at(2007)).empty());
157 EXPECT_TRUE(route.contains(i.lanelet));
158 EXPECT_LE(cLast, i.cost);
165 auto remainingLane{route.remainingLane(lanelets.at(2002))};
166 EXPECT_EQ(remainingLane.size(), 2ul);
167 EXPECT_TRUE(
hasLanelet(remainingLane, lanelets.at(2002)));
169 auto fullLane{route.fullLane(lanelets.at(2001))};
170 EXPECT_EQ(fullLane.size(), 3ul);
171 EXPECT_TRUE(
hasLanelet(fullLane, lanelets.at(2002)));
172 EXPECT_EQ(route.numLanes(), 7ul);
176 EXPECT_EQ(route.size(), 7ul);
177 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0,
false));
178 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
179 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
180 EXPECT_NE(route.laneletSubmap(),
nullptr);
181 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
184 auto remainingLane{route.remainingLane(lanelets.at(2002))};
185 EXPECT_EQ(remainingLane.size(), 6ul);
186 EXPECT_TRUE(
hasLanelet(remainingLane, lanelets.at(2002)));
188 auto fullLane{route.fullLane(lanelets.at(2002))};
189 EXPECT_EQ(fullLane.size(), 7ul);
191 EXPECT_TRUE(route.remainingLane(lanelets.at(2010)).empty());
192 EXPECT_EQ(route.numLanes(), 1ul);
197 EXPECT_EQ(i.lanelet, lanelets.at(2001));
203 EXPECT_EQ(route.size(), 3ul);
204 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
205 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
206 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
207 EXPECT_NE(route.laneletSubmap(),
nullptr);
208 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
212 EXPECT_EQ(route.size(), 3ul);
213 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
214 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
215 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
216 EXPECT_NE(route.laneletSubmap(),
nullptr);
217 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
221 EXPECT_EQ(route.size(), 15ul);
222 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
223 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
224 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
225 EXPECT_NE(route.laneletSubmap(),
nullptr);
226 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
230 EXPECT_EQ(route.size(), 9);
231 EXPECT_FALSE(
hasLanelet(route.laneletSubmap()->laneletLayer, lanelets.at(2065)));
235 EXPECT_EQ(route.size(), 5ul);
236 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
237 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
238 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
239 EXPECT_NE(route.laneletSubmap(),
nullptr);
240 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
244 auto remainingLane{route.remainingLane(lanelets.at(2020))};
245 EXPECT_EQ(remainingLane.size(), 3ul);
246 EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane),
ConstLanelet(lanelets.at(2022))) !=
247 std::end(remainingLane));
249 auto fullLane{route.fullLane(lanelets.at(2020))};
250 EXPECT_EQ(fullLane.size(), 5ul);
251 EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane),
ConstLanelet(lanelets.at(2024))) !=
253 EXPECT_EQ(route.numLanes(), 1ul);
257 auto remainingLane{route.remainingLane(lanelets.at(2020).invert())};
258 EXPECT_TRUE(remainingLane.empty());
262 EXPECT_EQ(route.size(), 5ul);
263 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
264 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
265 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
266 EXPECT_NE(route.laneletSubmap(),
nullptr);
267 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
271 auto remainingLane{route.remainingLane(lanelets.at(2020).invert())};
272 EXPECT_EQ(remainingLane.size(), 3ul);
273 EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane),
ConstLanelet(lanelets.at(2018))) !=
274 std::end(remainingLane));
276 auto fullLane{route.fullLane(lanelets.at(2020).invert())};
277 EXPECT_EQ(fullLane.size(), 5ul);
278 EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane),
ConstLanelet(lanelets.at(2021))) !=
280 EXPECT_EQ(route.numLanes(), 1ul);
284 auto remainingLane{route.remainingLane(lanelets.at(2020))};
285 EXPECT_EQ(remainingLane.size(), 4ul);
286 EXPECT_FALSE(std::find(std::begin(remainingLane), std::end(remainingLane),
ConstLanelet(lanelets.at(2018))) !=
287 std::end(remainingLane));
289 auto fullLane{route.fullLane(lanelets.at(2020))};
290 EXPECT_EQ(fullLane.size(), 6ul);
291 EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane),
ConstLanelet(lanelets.at(2021))) ==
293 EXPECT_EQ(route.numLanes(), 1ul);
297 EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025)));
301 EXPECT_EQ(route.numLanes(), 1ul);
302 EXPECT_TRUE(route.remainingLane(lanelets.at(2026)).empty());
306 EXPECT_EQ(route.size(), 7ul);
310 EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025)));
314 EXPECT_EQ(route.numLanes(), 2ul);
318 EXPECT_EQ(route.numLanes(), 2ul);
322 ASSERT_TRUE(!!route.rightRelation(lanelets.at(2025)));
323 EXPECT_EQ(*route.rightRelation(lanelets.at(2025)),
325 ASSERT_TRUE(!!route.leftRelation(lanelets.at(2026)));
326 EXPECT_EQ(*route.leftRelation(lanelets.at(2026)),
328 ASSERT_TRUE(!!route.rightRelation(lanelets.at(2027)));
329 EXPECT_EQ(*route.rightRelation(lanelets.at(2027)),
331 ASSERT_TRUE(!!route.leftRelation(lanelets.at(2028)));
332 EXPECT_EQ(*route.leftRelation(lanelets.at(2028)),
334 ASSERT_TRUE(!!route.rightRelation(lanelets.at(2029)));
335 EXPECT_EQ(*route.rightRelation(lanelets.at(2029)),
337 ASSERT_TRUE(!!route.leftRelation(lanelets.at(2030)));
338 EXPECT_EQ(*route.leftRelation(lanelets.at(2030)),
343 EXPECT_EQ(route.size(), 15ul);
344 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
348 EXPECT_EQ(route.size(), 15ul);
349 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
353 Optional<Route> route{graph->getRoute(lanelets.at(2014), lanelets.at(2007), 0)};
354 EXPECT_FALSE(!!route);
360 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 7ul);
361 EXPECT_EQ(route.numLanes(), 3ul);
365 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul);
366 EXPECT_EQ(route.numLanes(), 5ul);
370 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul);
371 EXPECT_EQ(route.numLanes(), 3ul);
372 EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 0ul);
376 EXPECT_EQ(route.size(), 10ul);
378 EXPECT_EQ(route.remainingLane(start).size(), 6ul);
379 EXPECT_EQ(*route.remainingLane(start).begin(), start);
380 EXPECT_EQ(route.remainingLane(lanelets.at(2036)).size(), 7ul);
381 EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).size(), 7ul);
382 EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 1ul);
383 EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).front(), lanelets.at(2067));
384 EXPECT_EQ(route.remainingLane(lanelets.at(2033)).size(), 3ul);
388 EXPECT_EQ(route.size(), 7ul);
389 EXPECT_EQ(route.numLanes(), 1ul);
390 EXPECT_EQ(route.remainingLane(start).size(), 7ul);
391 EXPECT_EQ(route.remainingShortestPath(lanelets.at(2036)).size(), 7ul);
392 EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 7ul);