1 #include <gtest/gtest.h>
11 template <
Id FromId,
Id ToId,
Id... ViaIds>
14 explicit TestRoute(uint16_t routingCostId = 0,
bool withLaneChanges =
true) {
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);
29 const Id From{FromId};
73 const auto map{this->route.getDebugLaneletMap()};
74 for (
const auto& el : this->route.shortestPath()) {
75 ASSERT_NE(map->pointLayer.find(el.id()), map->pointLayer.end());
76 const Point3d point{*map->pointLayer.find(el.id())};
77 EXPECT_TRUE(*point.attribute(
"shortest_path").asBool());
82 const LaneletMapPtr debugMap{this->route.getDebugLaneletMap()};
85 for (
const auto& it : laneletMap->laneletLayer) {
86 ASSERT_NE(debugMap->pointLayer.find(it.id()), debugMap->pointLayer.end());
87 Point3d point{*debugMap->pointLayer.find(it.id())};
88 EXPECT_NO_THROW(point.attribute(
"id"));
89 EXPECT_NO_THROW(point.attribute(
"lane_id"));
92 for (
const auto& it : debugMap->lineStringLayer) {
93 EXPECT_NO_THROW(it.attribute(
"relation_1"));
98 EXPECT_EQ(route.size(), 15ul);
99 EXPECT_GT(route.length2d(), 10.);
100 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
101 EXPECT_EQ(route.remainingShortestPath(start), route.shortestPath());
102 EXPECT_TRUE(route.remainingShortestPath(lanelets.at(2003)).empty());
103 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
104 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
105 EXPECT_NE(route.laneletSubmap(),
nullptr);
106 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
111 return utils::anyOf(relations, [&rel](
auto& relation) {
return rel == relation; });
114 template <
typename ContainerT>
116 return utils::anyOf(llts, [&llt](
auto& curr) {
return curr == llt; });
121 EXPECT_EQ(previous.size(), 2ul);
126 EXPECT_EQ(following.size(), 2ul);
132 auto conflictingInMap{route.conflictingInMap(lanelets.at(2009))};
133 EXPECT_EQ(conflictingInMap.size(), 1ul);
134 EXPECT_TRUE(
hasLanelet(conflictingInMap, lanelets.at(2008)));
136 ConstLanelets conflictingInRoute{route.conflictingInRoute(lanelets.at(2009))};
137 EXPECT_EQ(conflictingInRoute.size(), 1ul);
138 EXPECT_TRUE(
hasLanelet(conflictingInRoute, lanelets.at(2008)));
140 EXPECT_TRUE(route.conflictingInRoute(lanelets.at(2007)).empty());
141 EXPECT_TRUE(route.conflictingInMap(lanelets.at(2007)).empty());
147 EXPECT_TRUE(route.contains(i.lanelet));
148 EXPECT_LE(cLast, i.cost);
155 auto remainingLane{route.remainingLane(lanelets.at(2002))};
156 EXPECT_EQ(remainingLane.size(), 2ul);
157 EXPECT_TRUE(
hasLanelet(remainingLane, lanelets.at(2002)));
159 auto fullLane{route.fullLane(lanelets.at(2001))};
160 EXPECT_EQ(fullLane.size(), 3ul);
161 EXPECT_TRUE(
hasLanelet(fullLane, lanelets.at(2002)));
162 EXPECT_EQ(route.numLanes(), 7ul);
166 EXPECT_EQ(route.size(), 7ul);
167 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0,
false));
168 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
169 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
170 EXPECT_NE(route.laneletSubmap(),
nullptr);
171 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
174 auto remainingLane{route.remainingLane(lanelets.at(2002))};
175 EXPECT_EQ(remainingLane.size(), 6ul);
176 EXPECT_TRUE(
hasLanelet(remainingLane, lanelets.at(2002)));
178 auto fullLane{route.fullLane(lanelets.at(2002))};
179 EXPECT_EQ(fullLane.size(), 7ul);
181 EXPECT_TRUE(route.remainingLane(lanelets.at(2010)).empty());
182 EXPECT_EQ(route.numLanes(), 1ul);
187 EXPECT_EQ(i.lanelet, lanelets.at(2001));
193 EXPECT_EQ(route.size(), 3ul);
194 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
195 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
196 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
197 EXPECT_NE(route.laneletSubmap(),
nullptr);
198 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
202 EXPECT_EQ(route.size(), 3ul);
203 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
204 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
205 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
206 EXPECT_NE(route.laneletSubmap(),
nullptr);
207 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
211 EXPECT_EQ(route.size(), 15ul);
212 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
213 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
214 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
215 EXPECT_NE(route.laneletSubmap(),
nullptr);
216 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
220 EXPECT_EQ(route.size(), 9);
221 EXPECT_FALSE(
hasLanelet(route.laneletSubmap()->laneletLayer, lanelets.at(2065)));
225 EXPECT_EQ(route.size(), 5ul);
226 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
227 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
228 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
229 EXPECT_NE(route.laneletSubmap(),
nullptr);
230 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
234 auto remainingLane{route.remainingLane(lanelets.at(2020))};
235 EXPECT_EQ(remainingLane.size(), 3ul);
236 EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane),
ConstLanelet(lanelets.at(2022))) !=
237 std::end(remainingLane));
239 auto fullLane{route.fullLane(lanelets.at(2020))};
240 EXPECT_EQ(fullLane.size(), 5ul);
241 EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane),
ConstLanelet(lanelets.at(2024))) !=
243 EXPECT_EQ(route.numLanes(), 1ul);
247 auto remainingLane{route.remainingLane(lanelets.at(2020).invert())};
248 EXPECT_TRUE(remainingLane.empty());
252 EXPECT_EQ(route.size(), 5ul);
253 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
254 EXPECT_NE(route.getDebugLaneletMap(),
nullptr);
255 EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size());
256 EXPECT_NE(route.laneletSubmap(),
nullptr);
257 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size());
261 auto remainingLane{route.remainingLane(lanelets.at(2020).invert())};
262 EXPECT_EQ(remainingLane.size(), 3ul);
263 EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane),
ConstLanelet(lanelets.at(2018))) !=
264 std::end(remainingLane));
266 auto fullLane{route.fullLane(lanelets.at(2020).invert())};
267 EXPECT_EQ(fullLane.size(), 5ul);
268 EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane),
ConstLanelet(lanelets.at(2021))) !=
270 EXPECT_EQ(route.numLanes(), 1ul);
274 auto remainingLane{route.remainingLane(lanelets.at(2020))};
275 EXPECT_EQ(remainingLane.size(), 4ul);
276 EXPECT_FALSE(std::find(std::begin(remainingLane), std::end(remainingLane),
ConstLanelet(lanelets.at(2018))) !=
277 std::end(remainingLane));
279 auto fullLane{route.fullLane(lanelets.at(2020))};
280 EXPECT_EQ(fullLane.size(), 6ul);
281 EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane),
ConstLanelet(lanelets.at(2021))) ==
283 EXPECT_EQ(route.numLanes(), 1ul);
287 EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025)));
291 EXPECT_EQ(route.numLanes(), 1ul);
292 EXPECT_TRUE(route.remainingLane(lanelets.at(2026)).empty());
296 EXPECT_EQ(route.size(), 7ul);
300 EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025)));
304 EXPECT_EQ(route.numLanes(), 2ul);
308 EXPECT_EQ(route.numLanes(), 2ul);
312 ASSERT_TRUE(!!route.rightRelation(lanelets.at(2025)));
313 EXPECT_EQ(*route.rightRelation(lanelets.at(2025)),
315 ASSERT_TRUE(!!route.leftRelation(lanelets.at(2026)));
316 EXPECT_EQ(*route.leftRelation(lanelets.at(2026)),
318 ASSERT_TRUE(!!route.rightRelation(lanelets.at(2027)));
319 EXPECT_EQ(*route.rightRelation(lanelets.at(2027)),
321 ASSERT_TRUE(!!route.leftRelation(lanelets.at(2028)));
322 EXPECT_EQ(*route.leftRelation(lanelets.at(2028)),
324 ASSERT_TRUE(!!route.rightRelation(lanelets.at(2029)));
325 EXPECT_EQ(*route.rightRelation(lanelets.at(2029)),
327 ASSERT_TRUE(!!route.leftRelation(lanelets.at(2030)));
328 EXPECT_EQ(*route.leftRelation(lanelets.at(2030)),
333 EXPECT_EQ(route.size(), 15ul);
334 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
338 EXPECT_EQ(route.size(), 15ul);
339 EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0));
343 Optional<Route> route{graph->getRoute(lanelets.at(2014), lanelets.at(2007), 0)};
344 EXPECT_FALSE(!!route);
350 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 7ul);
351 EXPECT_EQ(route.numLanes(), 3ul);
355 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul);
356 EXPECT_EQ(route.numLanes(), 5ul);
360 EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul);
361 EXPECT_EQ(route.numLanes(), 3ul);
362 EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 0ul);
366 EXPECT_EQ(route.size(), 10ul);
368 EXPECT_EQ(route.remainingLane(start).size(), 6ul);
369 EXPECT_EQ(*route.remainingLane(start).begin(), start);
370 EXPECT_EQ(route.remainingLane(lanelets.at(2036)).size(), 7ul);
371 EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).size(), 7ul);
372 EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 1ul);
373 EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).front(), lanelets.at(2067));
374 EXPECT_EQ(route.remainingLane(lanelets.at(2033)).size(), 3ul);
378 EXPECT_EQ(route.size(), 7ul);
379 EXPECT_EQ(route.numLanes(), 1ul);
380 EXPECT_EQ(route.remainingLane(start).size(), 7ul);
381 EXPECT_EQ(route.remainingShortestPath(lanelets.at(2036)).size(), 7ul);
382 EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 7ul);