Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include <ad/map/access/Logging.hpp>
10 : : #include <ad/map/access/Operation.hpp>
11 : : #include <ad/map/lane/Lane.hpp>
12 : : #include <ad/map/lane/LaneOperation.hpp>
13 : : #include <ad/map/match/AdMapMatching.hpp>
14 : : #include <ad/map/match/Types.hpp>
15 : : #include <ad/map/point/Operation.hpp>
16 : : #include <ad/map/route/Planning.hpp>
17 : : #include <ad/map/route/RouteOperation.hpp>
18 : : #include <gtest/gtest.h>
19 : :
20 : : using namespace ::ad;
21 : : using namespace map;
22 : : using namespace map::route;
23 : : using namespace map::route::planning;
24 : : using namespace map::point;
25 : :
26 : : struct RoutePlanningTest : ::testing::Test
27 : : {
28 : 15 : RoutePlanningTest()
29 [ + - + - : 105 : {
+ - + - +
- + - +
- ]
30 : 15 : }
31 : :
32 : 15 : virtual ~RoutePlanningTest() = default;
33 : :
34 : 15 : virtual void SetUp()
35 : : {
36 : 15 : access::cleanup();
37 : : // access::getLogger()->set_level(spdlog::level::trace);
38 : 15 : }
39 : :
40 : 15 : virtual void TearDown()
41 : : {
42 : 15 : access::cleanup();
43 : 15 : }
44 : :
45 : 4 : route::FullRoute prepareTpkRoute()
46 : : {
47 [ - + ]: 4 : if (!access::init(mTestFile))
48 : : {
49 [ # # # # ]: 0 : throw std::runtime_error("Unable to initialize with " + mTestFile);
50 : : }
51 : :
52 : : /* 437 */ startLaneId
53 [ + - ]: 4 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.4404755), Latitude(49.0195732), Altitude(0.))); // id: 2682
54 : : /* 314 */ firstLaneId
55 : 0 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.4406803),
56 : : Latitude(49.0197914),
57 [ + - ]: 4 : Altitude(0.))); // lane inside intersection, todo more description, id: 2462
58 : : /* 402 */ secondLaneId
59 : 0 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.4407235),
60 : : Latitude(49.0197907),
61 [ + - ]: 4 : Altitude(0.))); // lane after intersection, todo more description, id: 3018
62 : : /* 138 */ thirdLaneId
63 : 0 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.4418464),
64 : : Latitude(49.0193326),
65 [ + - ]: 4 : Altitude(0.))); // lane inside intersection, todo more description, id: 2227
66 : : /* 247 */ fourthLaneId
67 : 0 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.4421272),
68 : : Latitude(49.0192331),
69 [ + - ]: 4 : Altitude(0.))); // lane inside intersection, todo more description, id: 1093
70 : : /* 408 */ fifthLaneId
71 : 0 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.4421012),
72 : : Latitude(49.0192655),
73 [ + - ]: 4 : Altitude(0.))); // lane inside intersection, todo more description, id: 1071
74 [ + - ]: 4 : /* 415 */ endLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4422283), Latitude(49.0192052), Altitude(0.)));
75 : :
76 : 4 : return route::planning::planRoute(createParaPoint(startLaneId, physics::ParametricValue(0.2)),
77 [ + - ]: 12 : createParaPoint(endLaneId, physics::ParametricValue(0.7)));
78 : : }
79 : :
80 : : void eraseFirstSegment(route::FullRoute &route);
81 : : enum AlignmentBehavior
82 : : {
83 : : Equal,
84 : : EndSegmentDiffers,
85 : : };
86 : : void compareRoutes(route::FullRoute const &left,
87 : : route::FullRoute const &right,
88 : : AlignmentBehavior const alignmentBehavior = Equal);
89 : : enum RouteBehaviour
90 : : {
91 : : RouteIsConstant,
92 : : RouteIsBroadening,
93 : : RouteIsNarrowing,
94 : : };
95 : : void validateRouteConnections(route::FullRoute const &route, RouteBehaviour const routeBehavior = RouteIsConstant);
96 : : void validateRouteParaPoints(route::FullRoute const &route);
97 : :
98 : : std::string mTestFile{"test_files/TPK.adm.txt"};
99 : :
100 : : std::vector<std::pair<GeoPoint, size_t>> mTestPoints{
101 : 15 : std::make_pair(createGeoPoint(Longitude(8.4400665), Latitude(49.0192005), Altitude(0.)), 0u),
102 : 15 : std::make_pair(createGeoPoint(Longitude(8.4401882), Latitude(49.0191939), Altitude(0.)), 1u),
103 : 15 : std::make_pair(createGeoPoint(Longitude(8.4401510), Latitude(49.0191792), Altitude(0.)), 2u),
104 : 15 : std::make_pair(createGeoPoint(Longitude(8.4401742), Latitude(49.0192009), Altitude(0.)), 3u),
105 : : // approaching middle of the intersection
106 : 30 : std::make_pair(createGeoPoint(Longitude(8.4401540), Latitude(49.0192082), Altitude(0.)), 4u)};
107 : : lane::LaneId startLaneId{};
108 : : lane::LaneId firstLaneId{};
109 : : lane::LaneId secondLaneId{};
110 : : lane::LaneId thirdLaneId{};
111 : : lane::LaneId fourthLaneId{};
112 : : lane::LaneId fifthLaneId{};
113 : : lane::LaneId endLaneId{};
114 : : };
115 : :
116 : 15 : void RoutePlanningTest::eraseFirstSegment(route::FullRoute &route)
117 : : {
118 [ + - ]: 15 : route.roadSegments.erase(route.roadSegments.begin());
119 [ + + ]: 30 : for (auto &laneSegment : route.roadSegments.front().drivableLaneSegments)
120 : : {
121 : 15 : laneSegment.predecessors.clear();
122 : : }
123 : 15 : }
124 : :
125 : 35 : void RoutePlanningTest::compareRoutes(route::FullRoute const &left,
126 : : route::FullRoute const &right,
127 : : AlignmentBehavior const alignmentBehavior)
128 : : {
129 [ + - - + : 35 : EXPECT_EQ(left.roadSegments.size(), right.roadSegments.size());
- - - - -
- ]
130 [ + + ]: 182 : for (std::size_t i = 0u; i < left.roadSegments.size(); ++i)
131 : : {
132 [ + - - + : 147 : EXPECT_EQ(left.roadSegments[i].boundingSphere, right.roadSegments[i].boundingSphere);
- - - - -
- ]
133 [ + - - + : 147 : EXPECT_EQ(left.roadSegments[i].segmentCountFromDestination, right.roadSegments[i].segmentCountFromDestination);
- - - - -
- ]
134 [ + - - + : 147 : EXPECT_EQ(left.roadSegments[i].drivableLaneSegments.size(), right.roadSegments[i].drivableLaneSegments.size());
- - - - -
- ]
135 [ + + ]: 295 : for (std::size_t j = 0u; j < left.roadSegments[i].drivableLaneSegments.size(); ++j)
136 : : {
137 [ + - - + : 148 : EXPECT_EQ(left.roadSegments[i].drivableLaneSegments[j].laneInterval.laneId,
- - - - -
- ]
138 : : right.roadSegments[i].drivableLaneSegments[j].laneInterval.laneId);
139 [ + - - + : 148 : EXPECT_EQ(left.roadSegments[i].drivableLaneSegments[j].laneInterval.wrongWay,
- - - - -
- ]
140 : : right.roadSegments[i].drivableLaneSegments[j].laneInterval.wrongWay);
141 [ + - - + : 148 : EXPECT_EQ(left.roadSegments[i].drivableLaneSegments[j].laneInterval.start,
- - - - -
- ]
142 : : right.roadSegments[i].drivableLaneSegments[j].laneInterval.start);
143 : :
144 [ + + + + : 148 : if ((alignmentBehavior == EndSegmentDiffers) && (i + 1 == left.roadSegments.size()))
+ + ]
145 : : {
146 [ + - - + : 2 : EXPECT_NE(left.roadSegments[i].drivableLaneSegments[j].laneInterval.end,
- - - - -
- ]
147 : : right.roadSegments[i].drivableLaneSegments[j].laneInterval.end);
148 [ + - + - : 2 : EXPECT_LE(std::fabs(left.roadSegments[i].drivableLaneSegments[j].laneInterval.end
- + - - -
- - - ]
149 : : - right.roadSegments[i].drivableLaneSegments[j].laneInterval.end),
150 : : physics::ParametricValue(0.1));
151 : : }
152 : : else
153 : : {
154 [ + - - + : 146 : EXPECT_EQ(left.roadSegments[i].drivableLaneSegments[j].laneInterval.end,
- - - - -
- ]
155 : : right.roadSegments[i].drivableLaneSegments[j].laneInterval.end);
156 : : }
157 : : }
158 : : }
159 [ + - - + : 35 : EXPECT_EQ(left.fullRouteSegmentCount, right.fullRouteSegmentCount);
- - - - -
- ]
160 : 35 : validateRouteParaPoints(left);
161 : 35 : }
162 : :
163 : 49 : void RoutePlanningTest::validateRouteParaPoints(route::FullRoute const &route)
164 : : {
165 [ + + ]: 215 : for (std::size_t i = 0u; i < route.roadSegments.size(); ++i)
166 : : {
167 [ + + ]: 340 : for (std::size_t j = 0u; j < route.roadSegments[i].drivableLaneSegments.size(); ++j)
168 : : {
169 : : // TParam is always 0 or 1; except for the start and the end of the route
170 [ + + + + : 174 : if ((i != 0u) && (i != route.roadSegments.size() - 1u))
+ + ]
171 : : {
172 [ + - + + : 89 : EXPECT_TRUE(
+ - + - -
+ - - - -
- - - - ]
173 : : (physics::ParametricValue(0.) == route.roadSegments[i].drivableLaneSegments[j].laneInterval.start)
174 : : || (physics::ParametricValue(1.) == route.roadSegments[i].drivableLaneSegments[j].laneInterval.start));
175 [ + - + + : 89 : EXPECT_TRUE(
+ - + - -
+ - - - -
- - - - ]
176 : : (physics::ParametricValue(0.) == route.roadSegments[i].drivableLaneSegments[j].laneInterval.end)
177 : : || (physics::ParametricValue(1.) == route.roadSegments[i].drivableLaneSegments[j].laneInterval.end));
178 : : }
179 [ + - + - : 174 : EXPECT_NE(isRouteDirectionAlignedWithDrivingDirection(route.roadSegments[i].drivableLaneSegments[j].laneInterval),
- + - - -
- - - ]
180 : : route.roadSegments[i].drivableLaneSegments[j].laneInterval.wrongWay)
181 [ # # ]: 0 : << route.roadSegments[i].drivableLaneSegments[j].laneInterval;
182 : : }
183 : : }
184 : 49 : }
185 : :
186 : 79 : void RoutePlanningTest::validateRouteConnections(route::FullRoute const &route, RouteBehaviour const routeBehavior)
187 : : {
188 : 79 : std::vector<std::set<lane::LaneId>> actualPresentLaneIds;
189 : 79 : std::vector<std::set<lane::LaneId>> listedPredecessors;
190 : 79 : std::vector<std::set<lane::LaneId>> listedSuccessors;
191 [ + - ]: 79 : actualPresentLaneIds.resize(route.roadSegments.size());
192 [ + - ]: 79 : listedPredecessors.resize(route.roadSegments.size());
193 [ + - ]: 79 : listedSuccessors.resize(route.roadSegments.size());
194 : 79 : bool maxLaneOffsetPresent = route.roadSegments.empty();
195 : 79 : bool minLaneOffsetPresent = route.roadSegments.empty();
196 [ + + ]: 337 : for (std::size_t i = 0u; i < route.roadSegments.size(); ++i)
197 : : {
198 [ + + ]: 258 : if (route.roadSegments[i].drivableLaneSegments[0].routeLaneOffset < 0)
199 : : {
200 [ + - - + : 11 : EXPECT_EQ(RouteIsBroadening, routeBehavior);
- - - - -
- ]
201 : : }
202 [ - + ]: 247 : else if (route.roadSegments[i].drivableLaneSegments[0].routeLaneOffset > 0)
203 : : {
204 [ # # # # : 0 : EXPECT_EQ(RouteIsNarrowing, routeBehavior);
# # # # #
# ]
205 : : }
206 [ + + ]: 538 : for (std::size_t j = 0u; j < route.roadSegments[i].drivableLaneSegments.size(); ++j)
207 : : {
208 [ + - - + : 280 : EXPECT_LE(route.roadSegments[i].drivableLaneSegments[j].routeLaneOffset, route.maxLaneOffset)
- - - - -
- ]
209 [ # # # # : 0 : << "i:" << i << " j:" << j << " route:" << route;
# # # # #
# # # ]
210 [ + - - + : 280 : EXPECT_GE(route.roadSegments[i].drivableLaneSegments[j].routeLaneOffset, route.minLaneOffset)
- - - - -
- ]
211 [ # # # # : 0 : << "i:" << i << " j:" << j << " route:" << route;
# # # # #
# # # ]
212 : 280 : maxLaneOffsetPresent |= route.roadSegments[i].drivableLaneSegments[j].routeLaneOffset == route.maxLaneOffset;
213 : 280 : minLaneOffsetPresent |= route.roadSegments[i].drivableLaneSegments[j].routeLaneOffset == route.minLaneOffset;
214 : :
215 [ + - - + : 280 : EXPECT_EQ(int(j),
- - - - -
- ]
216 : : route.roadSegments[i].drivableLaneSegments[j].routeLaneOffset
217 : : - route.roadSegments[i].drivableLaneSegments[0].routeLaneOffset)
218 [ # # # # : 0 : << "i:" << i << " j:" << j << " route:" << route;
# # # # #
# # # ]
219 : :
220 [ + + ]: 280 : if (static_cast<int32_t>(j) != route.roadSegments[i].drivableLaneSegments[j].routeLaneOffset)
221 : : {
222 [ + - - + : 24 : EXPECT_NE(RouteIsConstant, routeBehavior);
- - - - -
- ]
223 : : }
224 : :
225 [ + - ]: 280 : actualPresentLaneIds[i].insert(route.roadSegments[i].drivableLaneSegments[j].laneInterval.laneId);
226 [ + - ]: 560 : listedPredecessors[i].insert(std::begin(route.roadSegments[i].drivableLaneSegments[j].predecessors),
227 : 280 : std::end(route.roadSegments[i].drivableLaneSegments[j].predecessors));
228 [ + - ]: 560 : listedSuccessors[i].insert(std::begin(route.roadSegments[i].drivableLaneSegments[j].successors),
229 : 280 : std::end(route.roadSegments[i].drivableLaneSegments[j].successors));
230 : : }
231 : : }
232 [ - + - - : 79 : EXPECT_TRUE(maxLaneOffsetPresent);
- - - - -
- ]
233 [ - + - - : 79 : EXPECT_TRUE(minLaneOffsetPresent);
- - - - -
- ]
234 : :
235 [ + + ]: 337 : for (std::size_t i = 0u; i < route.roadSegments.size(); ++i)
236 : : {
237 [ + + ]: 538 : for (std::size_t j = 0u; j < route.roadSegments[i].drivableLaneSegments.size(); ++j)
238 : : {
239 [ + + ]: 469 : for (auto predecessorId : route.roadSegments[i].drivableLaneSegments[j].predecessors)
240 : : {
241 : : // no predecessors in first road segment allowed
242 [ + - - + : 189 : ASSERT_NE(i, 0u);
- - - - -
- ]
243 [ + - - + : 189 : ASSERT_TRUE(actualPresentLaneIds[i - 1].find(predecessorId) != actualPresentLaneIds[i - 1].end());
- - - - -
- - - ]
244 : : }
245 [ + + ]: 469 : for (auto successorId : route.roadSegments[i].drivableLaneSegments[j].successors)
246 : : {
247 : : // no successors in last road segment allowed
248 [ + - - + : 189 : ASSERT_NE(i, route.roadSegments.size() - 1u);
- - - - -
- ]
249 [ + - - + : 189 : ASSERT_TRUE(actualPresentLaneIds[i + 1].find(successorId) != actualPresentLaneIds[i + 1].end());
- - - - -
- - - ]
250 : : }
251 [ + + ]: 280 : if (j == 0u)
252 : : {
253 [ + - - + : 258 : ASSERT_EQ(route.roadSegments[i].drivableLaneSegments[j].rightNeighbor, lane::LaneId());
- - - - -
- ]
254 : : }
255 : : else
256 : : {
257 [ + - - + : 22 : ASSERT_TRUE(actualPresentLaneIds[i].find(route.roadSegments[i].drivableLaneSegments[j].rightNeighbor)
- - - - -
- - - ]
258 : : != actualPresentLaneIds[i].end());
259 : : }
260 [ + + ]: 280 : if (j + 1 == route.roadSegments[i].drivableLaneSegments.size())
261 : : {
262 [ + - - + : 258 : ASSERT_EQ(route.roadSegments[i].drivableLaneSegments[j].leftNeighbor, lane::LaneId());
- - - - -
- ]
263 : : }
264 : : else
265 : : {
266 [ + - - + : 22 : ASSERT_TRUE(actualPresentLaneIds[i].find(route.roadSegments[i].drivableLaneSegments[j].leftNeighbor)
- - - - -
- - - ]
267 : : != actualPresentLaneIds[i].end());
268 : : }
269 : : }
270 [ + + ]: 258 : if (i > 0u)
271 : : {
272 : : // all previous lanes are listed as predecessor in this segment
273 [ + + ]: 187 : if (actualPresentLaneIds[i - 1].size() > listedPredecessors[i].size())
274 : : {
275 : : // there are additional lanes in the previous segment,
276 [ + - - + : 1 : ASSERT_EQ(RouteIsNarrowing, routeBehavior);
- - - - -
- ]
277 : : // ensure that at least all listed predecessors exist
278 [ + - - + : 1 : ASSERT_GE(listedPredecessors[i].size(), 1u);
- - - - -
- ]
279 [ + + ]: 2 : for (auto listedPredecessor : listedPredecessors[i])
280 : : {
281 [ + - ]: 1 : auto findResult = actualPresentLaneIds[i - 1].find(listedPredecessor);
282 [ - + - - : 1 : ASSERT_TRUE(findResult != actualPresentLaneIds[i - 1].end());
- - - - -
- ]
283 : : }
284 : : }
285 : : else
286 : : {
287 [ + - - + : 186 : ASSERT_EQ(actualPresentLaneIds[i - 1].size(), listedPredecessors[i].size());
- - - - -
- ]
288 : : }
289 : : }
290 [ + + ]: 258 : if (i < route.roadSegments.size() - 1u)
291 : : {
292 : : // all next lanes are listed as successors in this segment
293 [ + + ]: 187 : if (actualPresentLaneIds[i + 1].size() > listedSuccessors[i].size())
294 : : {
295 : : // there are additional lanes in the successor segment,
296 [ + - - + : 12 : ASSERT_EQ(RouteIsBroadening, routeBehavior);
- - - - -
- ]
297 : : // ensure that at least all listed successors exist
298 [ + - - + : 12 : ASSERT_GE(listedSuccessors[i].size(), 1u);
- - - - -
- ]
299 [ + + ]: 26 : for (auto listedSuccessor : listedSuccessors[i])
300 : : {
301 [ + - ]: 14 : auto findResult = actualPresentLaneIds[i + 1].find(listedSuccessor);
302 [ - + - - : 14 : ASSERT_TRUE(findResult != actualPresentLaneIds[i + 1].end());
- - - - -
- ]
303 : : }
304 : : }
305 : : else
306 : : {
307 [ + - - + : 175 : ASSERT_EQ(actualPresentLaneIds[i + 1].size(), listedSuccessors[i].size());
- - - - -
- ]
308 : : }
309 : : }
310 : : }
311 : : }
312 : :
313 : 2 : TEST_F(RoutePlanningTest, route_planning)
314 : : {
315 : : using namespace route;
316 : : using namespace route::planning;
317 : :
318 : : struct RouteValues
319 : : {
320 : : size_t routeSize;
321 : : double routeLength;
322 : : };
323 : :
324 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
325 [ + - ]: 1 : access::setENUReferencePoint(mTestPoints.back().first);
326 : :
327 [ + - ]: 1 : startLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4404755), Latitude(49.0195732), Altitude(0.)));
328 [ + - ]: 1 : endLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4422283), Latitude(49.0192052), Altitude(0.)));
329 : : auto startLaneDrivingDirectionNegative
330 [ + - ]: 1 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.43988334), Latitude(49.02012825), Altitude(0.)));
331 : : std::vector<std::pair<std::pair<RoutingParaPoint, RoutingParaPoint>, RouteValues>> routesToPlan{
332 [ + - ]: 1 : std::make_pair(std::make_pair(createRoutingPoint(startLaneId, physics::ParametricValue(0.2)),
333 [ + - + - ]: 2 : createRoutingPoint(startLaneId, physics::ParametricValue(0.7))),
334 [ + - ]: 2 : RouteValues{1u, 35.2}),
335 : : // positive direction should give same results
336 : : std::make_pair(
337 [ + - ]: 1 : std::make_pair(createRoutingPoint(startLaneId, physics::ParametricValue(0.2), RoutingDirection::POSITIVE),
338 [ + - + - ]: 2 : createRoutingPoint(startLaneId, physics::ParametricValue(0.7), RoutingDirection::POSITIVE)),
339 [ + - ]: 2 : RouteValues{1u, 35.2}),
340 : : // ENUHeading of zero should be positive direction (lane is oriented towards north/east
341 : : std::make_pair(
342 [ + - ]: 1 : std::make_pair(createRoutingPoint(createParaPoint(startLaneId, physics::ParametricValue(0.2)), ENUHeading(0.)),
343 [ + - + - ]: 2 : createRoutingPoint(startLaneId, physics::ParametricValue(0.7))),
344 [ + - ]: 2 : RouteValues{1u, 35.2}),
345 : : // negative direction should fail
346 : : std::make_pair(
347 [ + - ]: 1 : std::make_pair(createRoutingPoint(startLaneId, physics::ParametricValue(0.2), RoutingDirection::NEGATIVE),
348 [ + - + - ]: 2 : createRoutingPoint(startLaneId, physics::ParametricValue(0.7))),
349 [ + - ]: 2 : RouteValues{0u, 0.}),
350 : : // ENUHeading of 3.5 should be negative direction (lane is oriented towards north/east
351 : : std::make_pair(
352 [ + - ]: 1 : std::make_pair(createRoutingPoint(createParaPoint(startLaneId, physics::ParametricValue(0.2)), ENUHeading(3.5)),
353 [ + - + - ]: 2 : createRoutingPoint(startLaneId, physics::ParametricValue(0.7))),
354 [ + - ]: 2 : RouteValues{0u, 0.}),
355 : : // using negative driving direction lane
356 [ + - ]: 1 : std::make_pair(std::make_pair(createRoutingPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.7)),
357 [ + - + - ]: 2 : createRoutingPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.2))),
358 [ + - ]: 2 : RouteValues{1u, 95.75}),
359 : 0 : std::make_pair(std::make_pair(createRoutingPoint(startLaneDrivingDirectionNegative,
360 : 0 : physics::ParametricValue(0.7),
361 [ + - ]: 1 : RoutingDirection::NEGATIVE),
362 [ + - + - ]: 2 : createRoutingPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.2))),
363 [ + - ]: 2 : RouteValues{1u, 95.75}),
364 : : // ENUHeading of 0. should be positive lane direction and so negative routing direction (lane is orientated towards
365 : : // south/east)
366 : 0 : std::make_pair(std::make_pair(createRoutingPoint(
367 : 1 : createParaPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.7)),
368 [ + - ]: 2 : ENUHeading(0.)),
369 [ + - + - ]: 2 : createRoutingPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.2))),
370 [ + - ]: 2 : RouteValues{1u, 95.75}),
371 : 0 : std::make_pair(std::make_pair(createRoutingPoint(startLaneDrivingDirectionNegative,
372 : 0 : physics::ParametricValue(0.7),
373 [ + - ]: 1 : RoutingDirection::POSITIVE),
374 [ + - + - ]: 2 : createRoutingPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.2))),
375 [ + - ]: 2 : RouteValues{0u, 0.}),
376 : 0 : std::make_pair(std::make_pair(createRoutingPoint(
377 : 1 : createParaPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.7)),
378 [ + - ]: 2 : ENUHeading(3.5)),
379 [ + - + - ]: 2 : createRoutingPoint(startLaneDrivingDirectionNegative, physics::ParametricValue(0.2))),
380 [ + - ]: 2 : RouteValues{0u, 0.}),
381 [ + - ]: 1 : std::make_pair(std::make_pair(createRoutingPoint(startLaneId, physics::ParametricValue(0.2)),
382 [ + - + - ]: 2 : createRoutingPoint(endLaneId, physics::ParametricValue(0.7))),
383 [ + - ]: 2 : RouteValues{7u, 195.31}),
384 : : std::make_pair(
385 [ + - ]: 1 : std::make_pair(createRoutingPoint(startLaneId, physics::ParametricValue(0.2), RoutingDirection::POSITIVE),
386 [ + - + - ]: 2 : createRoutingPoint(endLaneId, physics::ParametricValue(0.7))),
387 [ + - ]: 2 : RouteValues{7u, 195.31}),
388 : : std::make_pair(
389 [ + - ]: 1 : std::make_pair(createRoutingPoint(createParaPoint(startLaneId, physics::ParametricValue(0.2)), ENUHeading(0.)),
390 [ + - + - ]: 2 : createRoutingPoint(endLaneId, physics::ParametricValue(0.7))),
391 [ + - ]: 2 : RouteValues{7u, 195.31}),
392 [ + - ]: 1 : std::make_pair(std::make_pair(createRoutingPoint(startLaneId, physics::ParametricValue(0.7)),
393 [ + - + - ]: 2 : createRoutingPoint(startLaneId, physics::ParametricValue(0.2))),
394 [ + - ]: 2 : RouteValues{11u, 614.45}),
395 [ + - ]: 1 : };
396 : :
397 [ + + ]: 15 : for (auto routeToPlan : routesToPlan)
398 : : {
399 [ + - ]: 14 : auto routeResult = route::planning::planRoute(routeToPlan.first.first, routeToPlan.first.second);
400 : :
401 [ + - ]: 14 : auto routeLength = route::calcLength(routeResult);
402 : :
403 [ + - - + : 14 : ASSERT_EQ(routeToPlan.second.routeSize, routeResult.roadSegments.size()) << mTestFile;
- - - - -
- - - ]
404 [ + - - + : 14 : ASSERT_NEAR(double(routeToPlan.second.routeLength), double(routeLength), 1.) << mTestFile;
- - - - -
- - - ]
405 : :
406 : : // validate route connections
407 [ + - ]: 14 : validateRouteConnections(routeResult);
408 : : }
409 : : }
410 : :
411 : 2 : TEST_F(RoutePlanningTest, empty_route_following)
412 : : {
413 : 1 : route::FullRoute emptyRoute;
414 : :
415 : 1 : auto const paraPoint = createParaPoint(lane::LaneId(437), physics::ParametricValue(0.1));
416 : :
417 [ + - ]: 1 : route::FindWaypointResult invalidResult(emptyRoute);
418 [ - + - - : 1 : ASSERT_FALSE(invalidResult.isValid());
- - - - -
- ]
419 : :
420 [ + - ]: 1 : auto findWaypointResult = route::findWaypoint(paraPoint, emptyRoute);
421 [ - + - - : 1 : ASSERT_FALSE(findWaypointResult.isValid());
- - - - -
- ]
422 : : }
423 : :
424 : 2 : TEST_F(RoutePlanningTest, empty_mapmatched_bounding_box)
425 : : {
426 : 1 : route::FullRoute emptyRoute;
427 : 1 : match::MapMatchedObjectBoundingBox emptyObjectBoundingBox;
428 [ + - ]: 1 : auto findWaypointResult = route::objectOnRoute(emptyObjectBoundingBox, emptyRoute);
429 [ - + - - : 1 : ASSERT_FALSE(findWaypointResult.isValid());
- - - - -
- ]
430 : : }
431 : :
432 : 2 : TEST_F(RoutePlanningTest, route_following)
433 : : {
434 [ + - ]: 1 : route::FullRoute routeResult = prepareTpkRoute();
435 : :
436 [ + - ]: 1 : route::FullRoute route = routeResult;
437 [ + - ]: 1 : validateRouteConnections(route);
438 [ + - ]: 1 : route::FullRoute compareRoute = routeResult;
439 : :
440 : : // position before the route: keep as is
441 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededBeforeRoute,
- + - - -
- - - ]
442 : : route::shortenRoute(createParaPoint(startLaneId, physics::ParametricValue(0.1)), route));
443 [ + - ]: 1 : compareRoutes(compareRoute, route);
444 [ + - ]: 1 : validateRouteConnections(route);
445 : :
446 [ + - - + : 1 : ASSERT_GT(compareRoute.roadSegments.size(), 0u);
- - - - -
- ]
447 : : // position within the first segment: shorten first segment
448 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
449 : : route::shortenRoute(createParaPoint(startLaneId, physics::ParametricValue(0.4)), route));
450 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(0.4);
451 [ + - ]: 1 : compareRoutes(compareRoute, route);
452 [ + - ]: 1 : validateRouteConnections(route);
453 : :
454 : : // position exactly at the end of the segment: first segment will degenerate, so drop it
455 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
456 : : route::shortenRoute(createParaPoint(startLaneId, physics::ParametricValue(1.)), route));
457 [ + - ]: 1 : eraseFirstSegment(compareRoute);
458 [ + - ]: 1 : compareRoutes(compareRoute, route);
459 [ + - ]: 1 : validateRouteConnections(route);
460 : :
461 : : // actually entering next segment
462 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
463 : : route::shortenRoute(createParaPoint(firstLaneId, physics::ParametricValue(.01)), route));
464 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.01);
465 [ + - ]: 1 : compareRoutes(compareRoute, route);
466 [ + - ]: 1 : validateRouteConnections(route);
467 : :
468 : : // travel in the middle of the segment
469 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
470 : : route::shortenRoute(createParaPoint(firstLaneId, physics::ParametricValue(.5)), route));
471 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.5);
472 [ + - ]: 1 : compareRoutes(compareRoute, route);
473 [ + - ]: 1 : validateRouteConnections(route);
474 : :
475 : : // store current state before entering the next segment (which has negative driving direction!)
476 [ + - ]: 1 : routeResult = compareRoute;
477 : :
478 : : // jump into at the entering point
479 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
480 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(1.)), route));
481 [ + - ]: 1 : eraseFirstSegment(compareRoute);
482 [ + - ]: 1 : compareRoutes(compareRoute, route);
483 [ + - ]: 1 : validateRouteConnections(route);
484 : :
485 : : // jump into at the middle point
486 [ + - ]: 1 : route = routeResult;
487 [ + - ]: 1 : compareRoute = routeResult;
488 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
489 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(.5)), route));
490 [ + - ]: 1 : eraseFirstSegment(compareRoute);
491 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.5);
492 [ + - ]: 1 : compareRoutes(compareRoute, route);
493 [ + - ]: 1 : validateRouteConnections(route);
494 : :
495 : : // jitter a bit back: no change
496 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededBeforeRoute,
- + - - -
- - - ]
497 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(0.51)), route));
498 [ + - ]: 1 : compareRoutes(compareRoute, route);
499 [ + - ]: 1 : validateRouteConnections(route);
500 : :
501 : : // jitter further back: no change
502 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededBeforeRoute,
- + - - -
- - - ]
503 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(0.6)), route));
504 [ + - ]: 1 : compareRoutes(compareRoute, route);
505 [ + - ]: 1 : validateRouteConnections(route);
506 : :
507 : : // jitter a bit forward:
508 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
509 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(0.49)), route));
510 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.49);
511 [ + - ]: 1 : compareRoutes(compareRoute, route);
512 [ + - ]: 1 : validateRouteConnections(route);
513 : :
514 : : // jitter further forward:
515 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
516 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(0.4)), route));
517 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.4);
518 [ + - ]: 1 : compareRoutes(compareRoute, route);
519 [ + - ]: 1 : validateRouteConnections(route);
520 : :
521 : : // jump into the end point, old 3 segment points are removed
522 [ + - ]: 1 : route = routeResult;
523 [ + - ]: 1 : compareRoute = routeResult;
524 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
525 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(0.)), route));
526 [ + - ]: 1 : eraseFirstSegment(compareRoute);
527 [ + - ]: 1 : eraseFirstSegment(compareRoute);
528 [ + - ]: 1 : compareRoutes(compareRoute, route);
529 [ + - ]: 1 : validateRouteConnections(route);
530 : :
531 : : // same real point, but start of the next lane
532 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
533 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.)), route));
534 [ + - ]: 1 : compareRoutes(compareRoute, route);
535 [ + - ]: 1 : validateRouteConnections(route);
536 : :
537 : : // normal forward
538 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
539 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.5)), route));
540 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.5);
541 [ + - ]: 1 : compareRoutes(compareRoute, route);
542 [ + - ]: 1 : validateRouteConnections(route);
543 : :
544 : : // jitter a bit back: no change
545 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
546 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.49)), route));
547 [ + - ]: 1 : compareRoutes(compareRoute, route);
548 [ + - ]: 1 : validateRouteConnections(route);
549 : :
550 : : // jitter further back: no change, but actually before route
551 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededBeforeRoute,
- + - - -
- - - ]
552 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.4)), route));
553 [ + - ]: 1 : compareRoutes(compareRoute, route);
554 [ + - ]: 1 : validateRouteConnections(route);
555 : :
556 : : // jitter the same, but with PrependIfSucceededBeforeRoute shorten route mode, should adapt the route
557 : : // and expand to the front again
558 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
559 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.4)),
560 : : route,
561 : : route::ShortenRouteMode::PrependIfSucceededBeforeRoute));
562 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.4);
563 [ + - ]: 1 : compareRoutes(compareRoute, route);
564 [ + - ]: 1 : validateRouteConnections(route);
565 : :
566 : : // normal forward to .5 again
567 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
568 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.5)), route));
569 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.5);
570 [ + - ]: 1 : compareRoutes(compareRoute, route);
571 [ + - ]: 1 : validateRouteConnections(route);
572 : :
573 : : // jitter a bit forward
574 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
575 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.51)), route));
576 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.51);
577 [ + - ]: 1 : compareRoutes(compareRoute, route);
578 [ + - ]: 1 : validateRouteConnections(route);
579 : :
580 : : // jitter further forward
581 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
582 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.6)), route));
583 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.6);
584 [ + - ]: 1 : compareRoutes(compareRoute, route);
585 [ + - ]: 1 : validateRouteConnections(route);
586 : :
587 : : // jump over one segment completely
588 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
589 : : route::shortenRoute(createParaPoint(fourthLaneId, physics::ParametricValue(0.5)), route));
590 [ + - ]: 1 : eraseFirstSegment(compareRoute);
591 [ + - ]: 1 : eraseFirstSegment(compareRoute);
592 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.5);
593 [ + - ]: 1 : compareRoutes(compareRoute, route);
594 [ + - ]: 1 : validateRouteConnections(route);
595 : :
596 : : // drive into the end segment
597 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
598 : : route::shortenRoute(createParaPoint(endLaneId, physics::ParametricValue(0.2)), route));
599 [ + - ]: 1 : eraseFirstSegment(compareRoute);
600 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.2);
601 [ + - ]: 1 : compareRoutes(compareRoute, route);
602 [ + - ]: 1 : validateRouteConnections(route);
603 : :
604 : : // finishing nearby
605 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
606 : : route::shortenRoute(createParaPoint(endLaneId, physics::ParametricValue(0.69)), route));
607 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.69);
608 [ + - ]: 1 : compareRoutes(compareRoute, route);
609 [ + - ]: 1 : validateRouteConnections(route);
610 : :
611 : : // finishing exactly
612 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededRouteEmpty,
- + - - -
- - - ]
613 : : route::shortenRoute(createParaPoint(endLaneId, physics::ParametricValue(0.7)), route));
614 [ + - ]: 1 : eraseFirstSegment(compareRoute);
615 [ + - ]: 1 : compareRoutes(compareRoute, route);
616 [ - + - - : 1 : ASSERT_TRUE(route.roadSegments.empty());
- - - - -
- ]
617 : :
618 : : // finishing directly after target
619 [ + - ]: 1 : route = routeResult;
620 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededRouteEmpty,
- + - - -
- - - ]
621 : : route::shortenRoute(createParaPoint(endLaneId, physics::ParametricValue(0.71)), route));
622 [ + - ]: 1 : compareRoutes(compareRoute, route);
623 [ - + - - : 1 : ASSERT_TRUE(route.roadSegments.empty());
- - - - -
- ]
624 : :
625 : : // nothing left
626 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::FailedRouteEmpty,
- + - - -
- - - ]
627 : : route::shortenRoute(createParaPoint(endLaneId, physics::ParametricValue(0.7)), route));
628 : :
629 : : // something totally outside
630 [ + - ]: 1 : route = routeResult;
631 [ - + - - : 1 : ASSERT_FALSE(route.roadSegments.empty());
- - - - -
- ]
632 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::FailedRouteEmpty,
- + - - -
- - - ]
633 : : route::shortenRoute(createParaPoint(lane::LaneId(123456), physics::ParametricValue(0.7)), route));
634 [ - + - - : 1 : ASSERT_TRUE(route.roadSegments.empty());
- - - - -
- ]
635 : :
636 : : // DontCutIntersectionAndPrependIfSucceededBeforeRoute mode
637 : : // jump at end of second lane: ensure correct handling on degenerated handling at the beginning
638 [ + - ]: 1 : route = routeResult;
639 [ + - ]: 1 : compareRoute = routeResult;
640 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededIntersectionNotCut,
- + - - -
- - - ]
641 : : route::shortenRoute(createParaPoint(secondLaneId, physics::ParametricValue(0.)),
642 : : route,
643 : : ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute));
644 [ + - ]: 1 : eraseFirstSegment(compareRoute);
645 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(0.);
646 [ + - ]: 1 : compareRoutes(compareRoute, route);
647 [ + - ]: 1 : validateRouteConnections(route);
648 : :
649 : : // enter intersection
650 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededIntersectionNotCut,
- + - - -
- - - ]
651 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.)),
652 : : route,
653 : : ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute));
654 [ + - ]: 1 : compareRoutes(compareRoute, route);
655 [ + - ]: 1 : validateRouteConnections(route);
656 : :
657 : : // further within intersection
658 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededIntersectionNotCut,
- + - - -
- - - ]
659 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(0.5)),
660 : : route,
661 : : ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute));
662 [ + - ]: 1 : compareRoutes(compareRoute, route);
663 [ + - ]: 1 : validateRouteConnections(route);
664 : :
665 : : // end of intersection
666 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededIntersectionNotCut,
- + - - -
- - - ]
667 : : route::shortenRoute(createParaPoint(thirdLaneId, physics::ParametricValue(1.)),
668 : : route,
669 : : ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute));
670 [ + - ]: 1 : compareRoutes(compareRoute, route);
671 [ + - ]: 1 : validateRouteConnections(route);
672 : :
673 : : // now actually leaving the intersection, but jump into the second one
674 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::SucceededIntersectionNotCut,
- + - - -
- - - ]
675 : : route::shortenRoute(createParaPoint(fourthLaneId, physics::ParametricValue(0.5)),
676 : : route,
677 : : ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute));
678 [ + - ]: 1 : eraseFirstSegment(compareRoute);
679 [ + - ]: 1 : eraseFirstSegment(compareRoute);
680 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start
681 : 1 : = compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.end;
682 [ + - ]: 1 : compareRoutes(compareRoute, route);
683 [ + - ]: 1 : validateRouteConnections(route);
684 : :
685 : : // finally leaving also the second intersection
686 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
- + - - -
- - - ]
687 : : route::shortenRoute(createParaPoint(endLaneId, physics::ParametricValue(0.5)),
688 : : route,
689 : : ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute));
690 [ + - ]: 1 : eraseFirstSegment(compareRoute);
691 [ + - ]: 1 : eraseFirstSegment(compareRoute);
692 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(0.5);
693 [ + - ]: 1 : compareRoutes(compareRoute, route);
694 [ + - ]: 1 : validateRouteConnections(route);
695 : : }
696 : :
697 : 2 : TEST_F(RoutePlanningTest, route_following_multiple_choices)
698 : : {
699 [ + - ]: 1 : route::FullRoute routeResult = prepareTpkRoute();
700 [ + - ]: 1 : route::FullRoute route = routeResult;
701 [ + - ]: 1 : route::FullRoute compareRoute = routeResult;
702 : :
703 [ + - ]: 1 : validateRouteConnections(route);
704 : :
705 : : // entering next segment, second choice is nearer
706 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
+ - - + -
- - - -
- ]
707 : : route::shortenRoute(std::vector<ParaPoint>{createParaPoint(firstLaneId, physics::ParametricValue(.8)),
708 : : createParaPoint(firstLaneId, physics::ParametricValue(.5))},
709 : : route));
710 [ + - - + : 1 : ASSERT_GT(compareRoute.roadSegments.size(), 0u);
- - - - -
- ]
711 [ + - ]: 1 : eraseFirstSegment(compareRoute);
712 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.5);
713 [ + - ]: 1 : compareRoutes(compareRoute, route);
714 [ + - ]: 1 : validateRouteConnections(route);
715 : :
716 : : // entering next segment with the second choice, first choice keeps segment and is therefore nearer
717 [ + - + - : 1 : ASSERT_EQ(route::ShortenRouteResult::Succeeded,
+ - - + -
- - - -
- ]
718 : : route::shortenRoute(std::vector<ParaPoint>{createParaPoint(firstLaneId, physics::ParametricValue(.98)),
719 : : createParaPoint(secondLaneId, physics::ParametricValue(.01))},
720 : : route));
721 : 1 : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.98);
722 [ + - ]: 1 : compareRoutes(compareRoute, route);
723 [ + - ]: 1 : validateRouteConnections(route);
724 : : #if 0
725 : : // entering future segments with multiple choices: nearest is 138, 0.5
726 : : ASSERT_TRUE(route::shortenRoute(std::vector<ParaPoint>{createParaPoint(fifthLaneId, physics::ParametricValue(0.5)),
727 : : createParaPoint(fifthLaneId, physics::ParametricValue(0.4)),
728 : : createParaPoint(fifthLaneId, physics::ParametricValue(0.6)),
729 : : createParaPoint(fourthLaneId, physics::ParametricValue(0.5)),
730 : : createParaPoint(fourthLaneId, physics::ParametricValue(1.))},
731 : : route));
732 : : eraseFirstSegment(compareRoute);
733 : : eraseFirstSegment(compareRoute);
734 : : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.5);
735 : : compareRoutes(compareRoute, route);
736 : :
737 : : // calculation on segment with negative direction: nearest is 408, 0.8
738 : : ASSERT_TRUE(route::shortenRoute(std::vector<ParaPoint>{createParaPoint(fifthLaneId, physics::ParametricValue(.6)),
739 : : createParaPoint(fifthLaneId, physics::ParametricValue(.5)),
740 : : createParaPoint(fifthLaneId, physics::ParametricValue(.8)),
741 : : createParaPoint(fifthLaneId, physics::ParametricValue(.8)),
742 : : createParaPoint(endLaneId, physics::ParametricValue(.7))},
743 : : route));
744 : : eraseFirstSegment(compareRoute);
745 : : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.8);
746 : : compareRoutes(compareRoute, route);
747 : :
748 : : // ensure not existing segments don't influence and negative direction is ensured: nearest is 360, 0.4
749 : : ASSERT_TRUE(
750 : : route::shortenRoute(std::vector<ParaPoint>{createParaPoint(lane::LaneId(123456), physics::ParametricValue(.5)),
751 : : createParaPoint(fifthLaneId, physics::ParametricValue(.4)),
752 : : createParaPoint(lane::LaneId(123456), physics::ParametricValue(.7))},
753 : : route));
754 : : compareRoute.roadSegments[0].drivableLaneSegments[0].laneInterval.start = physics::ParametricValue(.4);
755 : : compareRoutes(compareRoute, route);
756 : :
757 : : // but if there are only not existing segments, then for sure, the route is cleaned
758 : : ASSERT_FALSE(
759 : : route::shortenRoute(std::vector<ParaPoint>{createParaPoint(lane::LaneId(123456), physics::ParametricValue(.5)),
760 : : createParaPoint(lane::LaneId(123457), physics::ParametricValue(.7)),
761 : : createParaPoint(lane::LaneId(123458), physics::ParametricValue(.7))},
762 : : route));
763 : : compareRoute.roadSegments.clear();
764 : : compareRoutes(compareRoute, route);
765 : : #endif
766 : : }
767 : :
768 : 2 : TEST_F(RoutePlanningTest, find_waypoint_result)
769 : : {
770 [ + - ]: 1 : route::FullRoute routeResult = prepareTpkRoute();
771 : :
772 : : auto findWaypointResult
773 [ + - ]: 1 : = route::findWaypoint(createParaPoint(firstLaneId, physics::ParametricValue(.5)), routeResult);
774 : :
775 : 1 : uint64_t expectedSegmentCount = 6u;
776 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult.isValid());
- - - - -
- ]
777 : :
778 : 1 : lane::LaneId foundLaneId = findWaypointResult.laneSegmentIterator->laneInterval.laneId;
779 : 1 : route::SegmentCounter foundSegmentCounter = findWaypointResult.roadSegmentIterator->segmentCountFromDestination;
780 [ + - - + : 1 : ASSERT_EQ(foundLaneId, firstLaneId);
- - - - -
- ]
781 [ + - - + : 1 : ASSERT_EQ(foundSegmentCounter, expectedSegmentCount);
- - - - -
- ]
782 : :
783 [ + - ]: 1 : auto leftLane = findWaypointResult.getLeftLane();
784 [ - + - - : 1 : ASSERT_FALSE(leftLane.isValid());
- - - - -
- ]
785 : :
786 [ + - ]: 1 : auto rightLane = findWaypointResult.getRightLane();
787 [ - + - - : 1 : ASSERT_FALSE(rightLane.isValid());
- - - - -
- ]
788 : :
789 : : // iterate the successors
790 : 1 : expectedSegmentCount = findWaypointResult.roadSegmentIterator->segmentCountFromDestination - 1;
791 [ + - + + ]: 6 : for (auto successorLanes = findWaypointResult.getSuccessorLanes(); !successorLanes.empty();
792 [ + - ]: 5 : successorLanes = successorLanes[0].getSuccessorLanes(), expectedSegmentCount--)
793 : : {
794 [ - + - - : 5 : ASSERT_TRUE(successorLanes[0].isValid());
- - - - -
- ]
795 : 5 : foundSegmentCounter = successorLanes[0].roadSegmentIterator->segmentCountFromDestination;
796 [ + - - + : 5 : ASSERT_EQ(foundSegmentCounter, expectedSegmentCount);
- - - - -
- ]
797 : : }
798 [ + - - + : 1 : ASSERT_EQ(0u, expectedSegmentCount);
- - - - -
- ]
799 : :
800 : : // iterate the predecessors
801 : 1 : expectedSegmentCount = findWaypointResult.roadSegmentIterator->segmentCountFromDestination + 1;
802 [ + - + + ]: 2 : for (auto predecessorLanes = findWaypointResult.getPredecessorLanes(); !predecessorLanes.empty();
803 [ + - ]: 1 : predecessorLanes = predecessorLanes[0].getPredecessorLanes(), expectedSegmentCount++)
804 : : {
805 [ - + - - : 1 : ASSERT_TRUE(predecessorLanes[0].isValid());
- - - - -
- ]
806 : 1 : foundSegmentCounter = predecessorLanes[0].roadSegmentIterator->segmentCountFromDestination;
807 [ + - - + : 1 : ASSERT_EQ(foundSegmentCounter, expectedSegmentCount);
- - - - -
- ]
808 : : }
809 [ + - - + : 1 : ASSERT_EQ(8u, expectedSegmentCount);
- - - - -
- ]
810 : : }
811 : :
812 : 2 : TEST_F(RoutePlanningTest, get_route_section)
813 : : {
814 [ + - ]: 1 : route::FullRoute routeResult = prepareTpkRoute();
815 [ + - ]: 1 : validateRouteConnections(routeResult);
816 : :
817 : 1 : route::FullRoute routeSection = route::getRouteSection(createParaPoint(firstLaneId, physics::ParametricValue(.5)),
818 : 1 : physics::Distance(1.),
819 : 1 : physics::Distance(1.),
820 [ + - ]: 1 : routeResult);
821 : :
822 [ + - ]: 1 : validateRouteConnections(routeSection);
823 [ + - + - : 1 : ASSERT_NEAR(2., double(route::calcLength(routeSection)), 0.1);
- + - - -
- - - ]
824 : :
825 [ + - ]: 2 : routeSection = route::getRouteSection(createParaPoint(firstLaneId, physics::ParametricValue(.5)),
826 : 1 : physics::Distance(10.),
827 : 1 : physics::Distance(10.),
828 : 1 : routeResult);
829 : :
830 [ + - ]: 1 : validateRouteConnections(routeSection);
831 [ + - + - : 1 : ASSERT_NEAR(20., double(route::calcLength(routeSection)), 0.1);
- + - - -
- - - ]
832 : :
833 [ + - ]: 2 : routeSection = route::getRouteSection(createParaPoint(firstLaneId, physics::ParametricValue(.5)),
834 : 1 : physics::Distance(50.),
835 : 1 : physics::Distance(80.),
836 : 1 : routeResult);
837 : :
838 [ + - ]: 1 : validateRouteConnections(routeSection);
839 [ + - + - : 1 : ASSERT_NEAR(130., double(route::calcLength(routeSection)), 0.1);
- + - - -
- - - ]
840 : :
841 [ + - ]: 2 : routeSection = route::getRouteSection(createParaPoint(firstLaneId, physics::ParametricValue(.5)),
842 : 1 : physics::Distance(1000.),
843 : 1 : physics::Distance(1000.),
844 : 1 : routeResult);
845 : :
846 [ + - ]: 1 : validateRouteConnections(routeSection);
847 [ + - + - : 1 : ASSERT_NEAR(195.3, double(route::calcLength(routeSection)), 0.1);
- + - - -
- - - ]
848 [ + - ]: 1 : compareRoutes(routeSection, routeResult);
849 : : }
850 : :
851 : 2 : TEST_F(RoutePlanningTest, route_planning_multi_lane_changes_OstringKaeppelstr)
852 : : {
853 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
854 : :
855 [ + - ]: 1 : startLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4396445), Latitude(49.0186033), Altitude(0.)));
856 [ + - ]: 1 : auto rightLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4394605), Latitude(49.0183941), Altitude(0.)));
857 [ + - ]: 1 : auto leftLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.439489), Latitude(49.0183852), Altitude(0.)));
858 : :
859 : 1 : route::FullRoute routeLeft = route::planning::planRoute(createParaPoint(startLaneId, physics::ParametricValue(0.2)),
860 [ + - ]: 3 : createParaPoint(leftLaneId, physics::ParametricValue(.5)));
861 : 1 : route::FullRoute routeRight = route::planning::planRoute(createParaPoint(startLaneId, physics::ParametricValue(0.2)),
862 [ + - ]: 3 : createParaPoint(rightLaneId, physics::ParametricValue(.5)));
863 : :
864 [ + - ]: 1 : validateRouteConnections(routeLeft, RouteIsBroadening);
865 [ + - ]: 1 : validateRouteConnections(routeRight, RouteIsBroadening);
866 [ + - ]: 1 : compareRoutes(routeLeft, routeRight, EndSegmentDiffers);
867 : : }
868 : :
869 : 2 : TEST_F(RoutePlanningTest, extend_route_by_distance)
870 : : {
871 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
872 : :
873 [ + - ]: 1 : startLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4396613), Latitude(49.0185636), Altitude(0.)));
874 : :
875 : : route::FullRoute const initialRoute
876 : 1 : = route::planning::planRoute(createParaPoint(startLaneId, physics::ParametricValue(0.)),
877 [ + - ]: 2 : createParaPoint(startLaneId, physics::ParametricValue(.1)));
878 : :
879 [ + - ]: 1 : auto const initialRouteLength = route::calcLength(initialRoute);
880 : :
881 [ + - ]: 1 : auto route = initialRoute;
882 : 1 : std::vector<route::FullRoute> additionalRoutes;
883 [ + - + - : 1 : ASSERT_TRUE(extendRouteToDistance(route, initialRouteLength + physics::Distance(30.), additionalRoutes));
- + - - -
- - - -
- ]
884 : : // still one road segment and no additional routes, route segment expanded till end
885 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.size(), 1u);
- - - - -
- ]
886 [ - + - - : 1 : ASSERT_TRUE(additionalRoutes.empty());
- - - - -
- ]
887 [ + - ]: 1 : auto routeLength = route::calcLength(route);
888 [ + - + - : 1 : ASSERT_GT(routeLength, initialRouteLength + physics::Distance(30.));
- + - - -
- - - ]
889 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.laneId, startLaneId);
- - - - -
- ]
890 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.start, physics::ParametricValue(0.));
- - - - -
- ]
891 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.end, physics::ParametricValue(1.));
- - - - -
- ]
892 [ + - ]: 1 : validateRouteParaPoints(route);
893 [ + - ]: 1 : validateRouteConnections(route, RouteIsConstant);
894 : :
895 : : // extending further spans into T-intersection and leads to an additional route
896 [ + - + - : 1 : ASSERT_TRUE(extendRouteToDistance(route, routeLength + physics::Distance(30.), additionalRoutes));
- + - - -
- - - -
- ]
897 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.size(), 3u);
- - - - -
- ]
898 [ + - - + : 1 : ASSERT_EQ(additionalRoutes.size(), 1u);
- - - - -
- ]
899 [ + - - + : 1 : ASSERT_EQ(additionalRoutes.front().roadSegments.size(), 3u);
- - - - -
- ]
900 [ + - ]: 1 : auto routeLengthA = route::calcLength(route);
901 [ + - ]: 1 : auto routeLengthB = route::calcLength(additionalRoutes.front());
902 [ + - + - : 1 : ASSERT_GT(routeLengthA, routeLength + physics::Distance(30.));
- + - - -
- - - ]
903 [ + - + - : 1 : ASSERT_GT(routeLengthB, routeLength + physics::Distance(30.));
- + - - -
- - - ]
904 : : // the begin of the routes is identical
905 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.laneId,
- - - - -
- ]
906 : : additionalRoutes.front().roadSegments.front().drivableLaneSegments.front().laneInterval.laneId);
907 : : // the end not
908 [ + - - + : 1 : ASSERT_NE(route.roadSegments.back().drivableLaneSegments.front().laneInterval.laneId,
- - - - -
- ]
909 : : additionalRoutes.back().roadSegments.front().drivableLaneSegments.front().laneInterval.laneId);
910 [ + - ]: 1 : validateRouteParaPoints(route);
911 [ + - ]: 1 : validateRouteConnections(route, RouteIsConstant);
912 [ + - ]: 1 : validateRouteParaPoints(additionalRoutes.back());
913 [ + - ]: 1 : validateRouteConnections(additionalRoutes.back(), RouteIsConstant);
914 : : }
915 : :
916 : 2 : TEST_F(RoutePlanningTest, extend_route_by_destination)
917 : : {
918 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
919 : :
920 [ + - ]: 1 : startLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4396613), Latitude(49.0185636), Altitude(0.)));
921 : :
922 : : route::FullRoute const initialRoute
923 : 1 : = route::planning::planRoute(createParaPoint(startLaneId, physics::ParametricValue(0.)),
924 : 1 : createParaPoint(startLaneId, physics::ParametricValue(.1)),
925 [ + - ]: 1 : RouteCreationMode::AllRoutableLanes);
926 : :
927 [ + - ]: 1 : auto const initialRouteLength = route::calcLength(initialRoute);
928 : :
929 : : // extending by geo points
930 : 1 : auto furtherDownTheSameLane = createGeoPoint(Longitude(8.4400608), Latitude(49.0190709), Altitude(0.));
931 [ + - ]: 1 : auto route = initialRoute;
932 [ + - + - : 1 : ASSERT_TRUE(extendRouteToDestinations(route, {furtherDownTheSameLane}));
- + - - -
- - - -
- ]
933 : : // still one road segment and no additional routes, route segment expanded till end
934 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.size(), 1u);
- - - - -
- ]
935 [ + - ]: 1 : auto routeLength = route::calcLength(route);
936 [ + - - + : 1 : ASSERT_GT(routeLength, initialRouteLength);
- - - - -
- ]
937 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.laneId, startLaneId);
- - - - -
- ]
938 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.start, physics::ParametricValue(0.));
- - - - -
- ]
939 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.end,
- - - - -
- ]
940 : : physics::ParametricValue(.847167));
941 [ + - ]: 1 : validateRouteParaPoints(route);
942 [ + - ]: 1 : validateRouteConnections(route, RouteIsConstant);
943 : :
944 : : // extending by furhter via some intersections geo points
945 : 1 : auto furtherAccrossNextIntersection = createGeoPoint(Longitude(8.4402588), Latitude(49.0193127), Altitude(0.));
946 : 1 : auto nearbyStartOppositeLane = createGeoPoint(Longitude(8.439657), Latitude(49.018624), Altitude(0.));
947 [ + - ]: 1 : route = initialRoute;
948 [ + - + - : 1 : ASSERT_TRUE(extendRouteToDestinations(
- + - - -
- - - -
- ]
949 : : route, {furtherDownTheSameLane, furtherAccrossNextIntersection, nearbyStartOppositeLane}));
950 : : // still one road segment and no additional routes, route segment expanded till end
951 [ + - - + : 1 : ASSERT_GT(route.roadSegments.size(), 1u);
- - - - -
- ]
952 [ + - ]: 1 : routeLength = route::calcLength(route);
953 [ + - - + : 1 : ASSERT_GT(routeLength, initialRouteLength);
- - - - -
- ]
954 : :
955 [ + - - + : 1 : ASSERT_EQ(route.roadSegments.front().drivableLaneSegments.front().laneInterval.laneId,
- - - - -
- ]
956 : : route.roadSegments.back().drivableLaneSegments.back().laneInterval.laneId);
957 : : }
958 : :
959 : 2 : TEST_F(RoutePlanningTest, routing_point)
960 : : {
961 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
962 [ + - ]: 1 : access::setENUReferencePoint(mTestPoints.back().first);
963 : :
964 [ + - ]: 1 : auto lanes = lane::getLanes();
965 [ + - - + : 1 : ASSERT_GT(lanes.size(), 0u);
- - - - -
- ]
966 : 1 : lane::LaneId x11 = lanes[0];
967 : :
968 : 1 : match::LaneOccupiedRegion occupiedRegion;
969 : 1 : occupiedRegion.laneId = x11;
970 : 1 : occupiedRegion.longitudinalRange.minimum = ::ad::physics::ParametricValue(0.2);
971 : 1 : occupiedRegion.longitudinalRange.maximum = ::ad::physics::ParametricValue(0.8);
972 : 1 : occupiedRegion.lateralRange.minimum = ::ad::physics::ParametricValue(0.5);
973 : 1 : occupiedRegion.lateralRange.maximum = ::ad::physics::ParametricValue(0.8);
974 : :
975 : 1 : RoutingParaPoint routingPoint1, routingPoint2;
976 [ + - ]: 1 : routingPoint1 = createRoutingPoint(occupiedRegion, RoutingDirection::POSITIVE);
977 [ + - - + : 1 : ASSERT_EQ(routingPoint1.point.laneId, x11);
- - - - -
- ]
978 [ + - - + : 1 : ASSERT_DOUBLE_EQ((double)routingPoint1.point.parametricOffset, 0.2);
- - - - -
- ]
979 [ + - - + : 1 : ASSERT_EQ(routingPoint1.direction, RoutingDirection::POSITIVE);
- - - - -
- ]
980 : :
981 [ + - + - ]: 1 : routingPoint2 = createRoutingPoint(occupiedRegion, point::createENUHeading(M_PI_2));
982 [ + - - + : 1 : ASSERT_EQ(routingPoint2.point.laneId, x11);
- - - - -
- ]
983 [ + - - + : 1 : ASSERT_DOUBLE_EQ((double)routingPoint2.point.parametricOffset, 0.8);
- - - - -
- ]
984 [ + - - + : 1 : ASSERT_EQ(routingPoint2.direction, RoutingDirection::NEGATIVE);
- - - - -
- ]
985 : : }
986 : :
987 : 2 : TEST_F(RoutePlanningTest, route_para_point)
988 : : {
989 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
990 [ + - ]: 1 : access::setENUReferencePoint(mTestPoints.back().first);
991 : :
992 [ + - ]: 1 : startLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4396445), Latitude(49.0186033), Altitude(0.)));
993 [ + - ]: 1 : auto leftLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.439489), Latitude(49.0183852), Altitude(0.)));
994 : :
995 : 1 : ParaPoint paraPoint1, paraPoint2;
996 : 1 : paraPoint1 = createParaPoint(startLaneId, physics::ParametricValue(0.2));
997 : 1 : paraPoint2 = createParaPoint(leftLaneId, physics::ParametricValue(.5));
998 : :
999 : 1 : physics::Distance dis;
1000 : 1 : route::FullRoute routeLeft, routeEmpty;
1001 : :
1002 [ + - ]: 1 : routeLeft = route::planning::planRoute(paraPoint1, paraPoint2);
1003 [ + - ]: 1 : validateRouteConnections(routeLeft, RouteIsBroadening);
1004 : 1 : RouteParaPoint routeParaPoint1, routeParaPoint2;
1005 [ + - - + : 1 : ASSERT_TRUE(getRouteParaPointFromParaPoint(paraPoint1, routeLeft, routeParaPoint1));
- - - - -
- - - ]
1006 [ + - - + : 1 : ASSERT_TRUE(getRouteParaPointFromParaPoint(paraPoint2, routeLeft, routeParaPoint2));
- - - - -
- - - ]
1007 : :
1008 [ + - ]: 1 : dis = ad::map::route::calcLength(routeParaPoint1, routeParaPoint2, routeLeft);
1009 [ + - - + : 1 : ASSERT_NEAR((double)dis, 22.1776, 0.0001);
- - - - -
- ]
1010 : :
1011 [ + - ]: 1 : FindWaypointResult findWaypointResult = route::findWaypoint(paraPoint2, routeLeft);
1012 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult.isValid());
- - - - -
- ]
1013 [ + - ]: 1 : dis = ad::map::route::calcLength(findWaypointResult);
1014 [ + - - + : 1 : ASSERT_NEAR((double)dis, 27.7613, 0.0001);
- - - - -
- ]
1015 : :
1016 [ + - + - ]: 1 : findWaypointResult = findWaypoint(startLaneId, routeLeft);
1017 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult.isValid());
- - - - -
- ]
1018 : :
1019 : 1 : physics::Duration duration;
1020 [ + - ]: 1 : duration = calcDuration(routeLeft);
1021 [ + - - + : 1 : ASSERT_NEAR((double)duration, 3.3313, 0.0001);
- - - - -
- ]
1022 [ + - - + : 1 : ASSERT_TRUE(isWithinInterval(routeLeft.roadSegments[0], paraPoint1));
- - - - -
- - - ]
1023 : :
1024 [ + - ]: 1 : RouteIterator routeIter0 = getRouteIterator(routeParaPoint1, routeEmpty);
1025 [ + - - + : 1 : ASSERT_FALSE(routeIter0.isValid());
- - - - -
- - - ]
1026 [ + - ]: 1 : RouteIterator routeIter1 = getRouteIterator(routeParaPoint1, routeLeft);
1027 [ + - ]: 1 : RouteIterator routeIter2 = getRouteIterator(routeParaPoint2, routeLeft);
1028 [ + - - + : 1 : ASSERT_TRUE(routeIter1.isValid());
- - - - -
- - - ]
1029 [ + - - + : 1 : ASSERT_TRUE(routeIter2.isValid());
- - - - -
- - - ]
1030 [ + - ]: 1 : dis = ad::map::route::calcLength(routeIter1, routeIter2);
1031 [ + - - + : 1 : ASSERT_NEAR((double)dis, 27.7613, 0.0001);
- - - - -
- ]
1032 : :
1033 : 1 : restriction::SpeedLimitList speedLimits;
1034 [ + - ]: 1 : speedLimits = getSpeedLimits(routeIter1, routeIter2);
1035 [ + - - + : 1 : ASSERT_EQ(speedLimits.size(), 1u);
- - - - -
- ]
1036 [ + - - + : 1 : ASSERT_NEAR((double)speedLimits[0].speedLimit, 8.3333, 0.0001);
- - - - -
- ]
1037 [ + - - + : 1 : ASSERT_NEAR((double)speedLimits[0].lanePiece.minimum, 0., 0.0001);
- - - - -
- ]
1038 [ + - - + : 1 : ASSERT_NEAR((double)speedLimits[0].lanePiece.maximum, 1., 0.0001);
- - - - -
- ]
1039 : :
1040 [ + - ]: 1 : speedLimits = getSpeedLimits(routeLeft);
1041 [ + - - + : 1 : ASSERT_EQ(speedLimits.size(), 3u);
- - - - -
- ]
1042 [ + + ]: 4 : for (auto const &speedLimit : speedLimits)
1043 : : {
1044 [ + - - + : 3 : ASSERT_NEAR((double)speedLimit.speedLimit, 8.3333, 0.0001);
- - - - -
- ]
1045 [ + - - + : 3 : ASSERT_NEAR((double)speedLimit.lanePiece.minimum, 0., 0.0001);
- - - - -
- ]
1046 [ + - - + : 3 : ASSERT_NEAR((double)speedLimit.lanePiece.maximum, 1., 0.0001);
- - - - -
- ]
1047 : : }
1048 : :
1049 : 1 : route::FullRoute bypassingRoute;
1050 [ + - - + : 1 : ASSERT_TRUE(calculateBypassingRoute(routeLeft, bypassingRoute));
- - - - -
- - - ]
1051 [ + - ]: 1 : duration = calcDuration(bypassingRoute);
1052 [ + - - + : 1 : ASSERT_NEAR((double)duration, 3.3136, 0.0001);
- - - - -
- ]
1053 [ + - ]: 1 : validateRouteConnections(bypassingRoute);
1054 : :
1055 : 1 : RouteParaPoint resultingPoint;
1056 [ + - - + : 1 : ASSERT_FALSE(
- - - - -
- - - ]
1057 : : calculateRouteParaPointAtDistance(routeEmpty, routeParaPoint1, physics::Distance(22.1776), resultingPoint));
1058 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
1059 : : calculateRouteParaPointAtDistance(routeLeft, routeParaPoint1, physics::Distance(22.1776), resultingPoint));
1060 [ + - - + : 1 : ASSERT_EQ(routeParaPoint2.routePlanningCounter, resultingPoint.routePlanningCounter);
- - - - -
- ]
1061 [ + - - + : 1 : ASSERT_EQ(routeParaPoint2.segmentCountFromDestination, resultingPoint.segmentCountFromDestination);
- - - - -
- ]
1062 [ + - - + : 1 : ASSERT_NEAR((double)routeParaPoint2.parametricOffset, (double)resultingPoint.parametricOffset, 0.0001);
- - - - -
- ]
1063 : :
1064 : 1 : route::FullRoute expandedRoute;
1065 [ + - ]: 1 : expandedRoute = getRouteExpandedToAllNeighborLanes(routeLeft);
1066 [ + - ]: 1 : dis = ad::map::route::calcLength(routeParaPoint1, routeParaPoint2, routeLeft);
1067 [ + - - + : 1 : ASSERT_NEAR((double)dis, 22.1776, 0.0001);
- - - - -
- ]
1068 [ + - ]: 1 : validateRouteConnections(expandedRoute, RouteIsBroadening);
1069 : :
1070 [ + - ]: 1 : shortenRouteToDistance(routeLeft, physics::Distance(19.1776));
1071 [ + - ]: 1 : dis = ad::map::route::calcLength(routeParaPoint1, routeParaPoint2, routeLeft);
1072 [ + - - + : 1 : ASSERT_NEAR((double)dis, 18.0002, 0.0001);
- - - - -
- ]
1073 [ + - ]: 1 : validateRouteConnections(routeLeft, RouteIsBroadening);
1074 : :
1075 [ + - ]: 1 : dis = addOpposingLaneSegmentToRoadSegment(paraPoint1, physics::Distance(1.), routeLeft.roadSegments[0], routeLeft);
1076 [ + - - + : 1 : ASSERT_NEAR((double)dis, -1., 0.0001);
- - - - -
- ]
1077 [ + - ]: 1 : validateRouteConnections(routeLeft, RouteIsBroadening);
1078 : :
1079 : 2 : RoadSegment roadSegment;
1080 [ + - + - : 2 : EXPECT_THROW(shortenSegmentFromBegin(roadSegment, dis), std::runtime_error);
+ - - + -
+ - - - -
- - - - ]
1081 [ + - + - : 2 : EXPECT_THROW(shortenSegmentFromEnd(roadSegment, dis), std::runtime_error);
+ - - + -
+ - - - -
- - - - ]
1082 : : }
1083 : :
1084 : 2 : TEST_F(RoutePlanningTest, route_match_position)
1085 : : {
1086 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
1087 [ + - ]: 1 : access::setENUReferencePoint(mTestPoints.back().first);
1088 : :
1089 : 1 : physics::Distance dis;
1090 : 1 : route::FullRoute routeLeft;
1091 : 1 : physics::Duration duration;
1092 : :
1093 [ + - ]: 1 : startLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4396445), Latitude(49.0186033), Altitude(0.)));
1094 [ + - ]: 1 : auto leftLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.439489), Latitude(49.0183852), Altitude(0.)));
1095 : :
1096 : 1 : ParaPoint paraPoint1, paraPoint2;
1097 : 1 : paraPoint1 = createParaPoint(startLaneId, physics::ParametricValue(0.2));
1098 : 1 : paraPoint2 = createParaPoint(leftLaneId, physics::ParametricValue(.5));
1099 : :
1100 [ + - ]: 1 : routeLeft = route::planning::planRoute(paraPoint1, paraPoint2);
1101 [ + - ]: 1 : validateRouteConnections(routeLeft, RouteIsBroadening);
1102 : :
1103 : 1 : point::GeoPoint point_geo1, point_geo2;
1104 : 1 : point_geo1 = point::createGeoPoint(Longitude(8.4396445), Latitude(49.0186033), Altitude(0.));
1105 : 1 : point_geo2 = point::createGeoPoint(Longitude(8.439489), Latitude(49.0183852), Altitude(0.));
1106 [ + - ]: 1 : match::AdMapMatching mapMatching;
1107 : 1 : match::MapMatchedPositionConfidenceList matchPosition1, matchPosition2;
1108 [ + - ]: 1 : matchPosition1 = mapMatching.getMapMatchedPositions(point_geo1, physics::Distance(1), physics::Probability(0.5));
1109 [ + - ]: 1 : matchPosition2 = mapMatching.getMapMatchedPositions(point_geo2, physics::Distance(1), physics::Probability(0.5));
1110 [ + - - + : 1 : ASSERT_GT(matchPosition1.size(), 0u);
- - - - -
- ]
1111 [ + - - + : 1 : ASSERT_GT(matchPosition2.size(), 0u);
- - - - -
- ]
1112 : :
1113 [ + - ]: 1 : dis = signedDistanceToLane(startLaneId, routeLeft, matchPosition1);
1114 [ + - - + : 1 : ASSERT_NEAR((double)dis, 0., 0.0001);
- - - - -
- ]
1115 [ + - ]: 1 : dis = signedDistanceToLane(leftLaneId, routeLeft, matchPosition2);
1116 [ + - - + : 1 : ASSERT_NEAR((double)dis, 0., 0.0001);
- - - - -
- ]
1117 : :
1118 [ + - ]: 1 : FindWaypointResult findWaypointResult = findNearestWaypoint(matchPosition1, routeLeft);
1119 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult.isValid());
- - - - -
- ]
1120 : :
1121 : 1 : point::ENUHeading heading1, heading2;
1122 [ + - ]: 1 : heading1 = mapMatching.getLaneENUHeading(matchPosition1.front());
1123 [ + - ]: 1 : heading2 = mapMatching.getLaneENUHeading(matchPosition2.front());
1124 : :
1125 : 1 : match::ENUObjectPosition mObjectPosition1, mObjectPosition2;
1126 [ + - ]: 1 : mObjectPosition1.centerPoint = point::toENU(point_geo1);
1127 : 1 : mObjectPosition1.heading = heading1;
1128 : 1 : mObjectPosition1.dimension.width = physics::Distance(1.);
1129 : 1 : mObjectPosition1.dimension.length = physics::Distance(1.);
1130 : 1 : mObjectPosition1.dimension.height = physics::Distance(0.5);
1131 [ + - ]: 1 : mObjectPosition1.enuReferencePoint = access::getENUReferencePoint();
1132 [ + - ]: 1 : mObjectPosition2.centerPoint = point::toENU(point_geo2);
1133 : 1 : mObjectPosition2.heading = heading2;
1134 : 1 : mObjectPosition2.dimension.width = physics::Distance(1.);
1135 : 1 : mObjectPosition2.dimension.length = physics::Distance(1.);
1136 : 1 : mObjectPosition2.dimension.height = physics::Distance(0.5);
1137 [ + - ]: 1 : mObjectPosition2.enuReferencePoint = access::getENUReferencePoint();
1138 : :
1139 : 1 : match::MapMatchedObjectBoundingBox boundBox1, boundBox2;
1140 [ + - ]: 1 : boundBox1 = mapMatching.getMapMatchedBoundingBox(mObjectPosition1, physics::Distance(1));
1141 [ + - ]: 1 : boundBox2 = mapMatching.getMapMatchedBoundingBox(mObjectPosition2, physics::Distance(1));
1142 [ + - - + : 1 : ASSERT_GT(boundBox1.laneOccupiedRegions.size(), 0u);
- - - - -
- ]
1143 [ + - - + : 1 : ASSERT_GT(boundBox2.laneOccupiedRegions.size(), 0u);
- - - - -
- ]
1144 : :
1145 : 1 : std::vector<FullRoute> fullRouteVec;
1146 [ + - ]: 1 : fullRouteVec = predictRoutesOnDistance(boundBox1, physics::Distance(22.1776));
1147 [ + - - + : 1 : ASSERT_GT(fullRouteVec.size(), 0u);
- - - - -
- ]
1148 [ + + ]: 2 : for (auto const &route : fullRouteVec)
1149 : : {
1150 [ + - ]: 1 : validateRouteConnections(route, RouteIsBroadening);
1151 : : }
1152 : :
1153 [ + - ]: 1 : fullRouteVec = predictRoutesOnDuration(boundBox1, physics::Duration(3.33));
1154 [ + - - + : 1 : ASSERT_GT(fullRouteVec.size(), 0u);
- - - - -
- ]
1155 [ + + ]: 2 : for (auto const &route : fullRouteVec)
1156 : : {
1157 [ + - ]: 1 : validateRouteConnections(route, RouteIsBroadening);
1158 : : }
1159 : :
1160 [ + - ]: 1 : auto findWaypointResult2 = route::objectOnRoute(boundBox1, fullRouteVec[0]);
1161 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult2.isValid());
- - - - -
- ]
1162 : :
1163 : 1 : route::FullRoute routeResult;
1164 [ + - ]: 1 : routeResult = getRouteExpandedToOppositeLanes(routeLeft);
1165 [ + - ]: 1 : validateRouteConnections(routeResult, RouteIsBroadening);
1166 [ + - - + : 1 : ASSERT_FALSE(addOpposingLaneToRoute(paraPoint1, physics::Distance(1.), routeResult, dis));
- - - - -
- - - ]
1167 [ + - - + : 1 : ASSERT_FALSE(addOpposingLaneToRoute(paraPoint2, physics::Distance(10.), routeResult, dis));
- - - - -
- - - ]
1168 : :
1169 : 1 : ParaPoint paraOppoPoint1;
1170 [ + - ]: 1 : auto startLaneId1 = lane::uniqueLaneId(createGeoPoint(Longitude(8.4404330), Latitude(49.0195249), Altitude(0.)));
1171 [ + - ]: 1 : auto rightLaneId1 = lane::uniqueLaneId(createGeoPoint(Longitude(8.4408261), Latitude(49.0200051), Altitude(0.)));
1172 [ + - ]: 1 : auto oppositeLaneId = lane::uniqueLaneId(createGeoPoint(Longitude(8.4403594), Latitude(49.0194988), Altitude(0.)));
1173 : :
1174 : 1 : paraPoint1 = createParaPoint(startLaneId1, physics::ParametricValue(0.2));
1175 : 1 : paraPoint2 = createParaPoint(rightLaneId1, physics::ParametricValue(.5));
1176 [ + - ]: 1 : routeLeft = route::planning::planRoute(paraPoint1, paraPoint2);
1177 [ + - ]: 1 : validateRouteConnections(routeLeft);
1178 : 1 : paraOppoPoint1 = createParaPoint(oppositeLaneId, physics::ParametricValue(0.9));
1179 [ + - - + : 1 : ASSERT_TRUE(addOpposingLaneToRoute(paraOppoPoint1, physics::Distance(100.), routeLeft, dis));
- - - - -
- - - ]
1180 [ + - ]: 1 : validateRouteConnections(routeLeft, RouteIsNarrowing);
1181 [ + - ]: 1 : auto findWaypointResultOppo1 = route::findWaypoint(oppositeLaneId, routeLeft);
1182 [ - + - - : 1 : ASSERT_TRUE(findWaypointResultOppo1.isValid());
- - - - -
- ]
1183 [ + - ]: 1 : dis = calcLength(findWaypointResultOppo1);
1184 [ + - - + : 1 : ASSERT_NEAR((double)dis, 0., 0.0001);
- - - - -
- ]
1185 [ + - ]: 1 : auto findWaypointResultRight = route::findWaypoint(rightLaneId1, routeLeft);
1186 [ - + - - : 1 : ASSERT_TRUE(findWaypointResultRight.isValid());
- - - - -
- ]
1187 [ + - ]: 1 : dis = calcLength(findWaypointResultRight);
1188 [ + - - + : 1 : ASSERT_NEAR((double)dis, 15.4080, 0.0001);
- - - - -
- ]
1189 : : }
1190 : :
1191 : 2 : TEST_F(RoutePlanningTest, route_plan_given_geo)
1192 : : {
1193 [ + - - + : 1 : ASSERT_TRUE(access::init(mTestFile));
- - - - -
- - - ]
1194 [ + - ]: 1 : access::setENUReferencePoint(mTestPoints.back().first);
1195 : :
1196 : 1 : point::GeoPoint point_geo1, point_geo2, point_geo3;
1197 : 1 : point_geo1 = point::createGeoPoint(Longitude(8.4396445), Latitude(49.0186033), Altitude(0.));
1198 : 1 : point_geo2 = point::createGeoPoint(Longitude(8.439489), Latitude(49.0183852), Altitude(0.));
1199 : :
1200 [ + - ]: 1 : startLaneId = lane::uniqueLaneId(point_geo1);
1201 [ + - ]: 1 : auto leftLaneId = lane::uniqueLaneId(point_geo2);
1202 : :
1203 : 1 : RoutingParaPoint startRouting;
1204 [ + - ]: 1 : startRouting = createRoutingPoint(startLaneId, physics::ParametricValue(0.1), RoutingDirection::DONT_CARE);
1205 : 1 : std::vector<point::GeoPoint> dest;
1206 [ + - ]: 1 : dest.push_back(point_geo2);
1207 : 1 : route::FullRoute routeLeft, routeResult;
1208 [ + - ]: 1 : routeLeft = planRoute(startRouting, dest);
1209 : :
1210 : 1 : ParaPoint paraPoint1, paraPoint2;
1211 : 1 : paraPoint1 = createParaPoint(startLaneId, physics::ParametricValue(0.2));
1212 : 1 : paraPoint2 = createParaPoint(leftLaneId, physics::ParametricValue(.5));
1213 : :
1214 : 1 : physics::Distance dis;
1215 [ + - ]: 1 : FindWaypointResult findWaypointResult1 = route::findWaypoint(paraPoint2, routeLeft);
1216 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult1.isValid());
- - - - -
- ]
1217 [ + - ]: 1 : dis = ad::map::route::calcLength(findWaypointResult1);
1218 [ + - - + : 1 : ASSERT_NEAR((double)dis, 19.4644, 0.0001);
- - - - -
- ]
1219 : :
1220 [ + - ]: 1 : routeLeft = planRoute(startRouting, point_geo2);
1221 [ + - ]: 1 : FindWaypointResult findWaypointResult2 = route::findWaypoint(paraPoint2, routeLeft);
1222 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult2.isValid());
- - - - -
- ]
1223 [ + - ]: 1 : dis = ad::map::route::calcLength(findWaypointResult2);
1224 [ + - - + : 1 : ASSERT_NEAR((double)dis, 19.4644, 0.0001);
- - - - -
- ]
1225 : :
1226 : 1 : point_geo1 = point::createGeoPoint(Longitude(8.4396445), Latitude(49.0186033), Altitude(0.));
1227 [ + - ]: 1 : auto startLaneId1 = lane::uniqueLaneId(point_geo1);
1228 [ + - ]: 1 : startRouting = createRoutingPoint(startLaneId1, physics::ParametricValue(0.1), RoutingDirection::DONT_CARE);
1229 : 1 : point_geo2 = point::createGeoPoint(Longitude(8.440442), Latitude(49.0199380), Altitude(0.));
1230 : 1 : dest.clear();
1231 [ + - ]: 1 : dest.push_back(point_geo2);
1232 [ + - ]: 1 : routeLeft = planRoute(startRouting, dest);
1233 [ + - - + : 2 : ASSERT_EQ(routeLeft, route::FullRoute());
- - - - -
- ]
1234 : :
1235 : 1 : point_geo2 = point::createGeoPoint(Longitude(8.4404311), Latitude(49.0199250), Altitude(0.));
1236 [ + - ]: 1 : dest.push_back(point_geo2);
1237 [ + - ]: 1 : routeLeft = planRoute(startRouting, dest);
1238 [ + - - + : 2 : ASSERT_EQ(routeLeft, route::FullRoute());
- - - - -
- ]
1239 : :
1240 : 1 : dest.clear();
1241 : 1 : point_geo1 = point::createGeoPoint(Longitude(8.4404844), Latitude(49.0195607), Altitude(0.));
1242 : 1 : point_geo2 = point::createGeoPoint(Longitude(8.4406460), Latitude(49.0197543), Altitude(0.));
1243 : 1 : point_geo3 = point::createGeoPoint(Longitude(8.4407874), Latitude(49.0199274), Altitude(0.));
1244 [ + - ]: 1 : dest.push_back(point_geo2);
1245 [ + - ]: 1 : dest.push_back(point_geo3);
1246 [ + - ]: 1 : auto startLaneId3 = lane::uniqueLaneId(point_geo1);
1247 [ + - ]: 1 : auto leftLaneId3 = lane::uniqueLaneId(point_geo3);
1248 [ + - ]: 1 : startRouting = createRoutingPoint(startLaneId3, physics::ParametricValue(0.1), RoutingDirection::DONT_CARE);
1249 [ + - ]: 1 : routeLeft = planRoute(startRouting, dest);
1250 : 1 : paraPoint2 = createParaPoint(leftLaneId3, physics::ParametricValue(.5));
1251 [ + - ]: 1 : FindWaypointResult findWaypointResult3 = route::findWaypoint(leftLaneId3, routeLeft);
1252 [ - + - - : 1 : ASSERT_TRUE(findWaypointResult3.isValid());
- - - - -
- ]
1253 [ + - ]: 1 : dis = ad::map::route::calcLength(findWaypointResult3);
1254 [ + - - + : 1 : ASSERT_NEAR((double)dis, 71.7440, 0.0001);
- - - - -
- ]
1255 : : }
1256 : :
1257 : 2 : TEST_F(RoutePlanningTest, connecting_route)
1258 : : {
1259 [ + - + - : 1 : ASSERT_TRUE(access::init("test_files/TPK_PFZ.adm.txt"));
- + - - -
- - - -
- ]
1260 : :
1261 [ + - ]: 2 : match::AdMapMatching mapMatching;
1262 : :
1263 : 1 : physics::Dimension3D vehicleDimension;
1264 : 1 : vehicleDimension.length = physics::Distance(5.0);
1265 : 1 : vehicleDimension.width = physics::Distance(2.0);
1266 : 1 : vehicleDimension.height = physics::Distance(1.5);
1267 : :
1268 : : // Emmy-Noether last section before intersection to Ada-Lovelace
1269 [ + - ]: 1 : auto const emmyToAda = lane::uniqueLaneId(createGeoPoint(Longitude(8.4381157), Latitude(49.0208767), Altitude(0.)));
1270 [ + - + - ]: 1 : auto const opositeToEmmyToAda = getContactLanes(lane::getLane(emmyToAda), lane::ContactLocation::LEFT).front().toLane;
1271 : :
1272 : : // Hirtenweg to Haid-und-Neu Str.
1273 [ + - ]: 1 : auto const hirtToHaid = lane::uniqueLaneId(createGeoPoint(Longitude(8.4401012), Latitude(49.0181061), Altitude(0.)));
1274 : : // Haid-und-Neu Str. crossing Hirtenweg to Haid-und-Neu Str.
1275 : : auto const haidXhirtXHaid
1276 [ + - ]: 1 : = lane::uniqueLaneId(createGeoPoint(Longitude(8.4404876), Latitude(49.0179614), Altitude(0.)));
1277 : :
1278 : : enum
1279 : : {
1280 : : EGO_LANEID,
1281 : : EGO_LANE_OFFSET_LON,
1282 : : EGO_LANE_OFFSET_LAT,
1283 : : EGO_HEADING_OFFSET,
1284 : : OTHER_LANEID,
1285 : : OTHER_LANE_OFFSET_LON,
1286 : : OTHER_LANE_OFFSET_LAT,
1287 : : OTHER_HEADING_OFFSET,
1288 : : MAX_DIST_TOO_SHORT,
1289 : : MAX_DIST_NORMAL,
1290 : : RESULT_TYPE,
1291 : : RESULT_ROUTE_EGO_LENGTH,
1292 : : RESULT_ROUTE_OTHER_LENGTH
1293 : : };
1294 : :
1295 : : std::vector<std::tuple<
1296 : : // ego positioning within the lane (paraPoint, lateralOffset, delta lane heading)
1297 : : lane::LaneId,
1298 : : physics::ParametricValue,
1299 : : physics::ParametricValue,
1300 : : double,
1301 : : // object positioning within the lane (paraPoint, lateralOffset, delta lane heading)
1302 : : lane::LaneId,
1303 : : physics::ParametricValue,
1304 : : physics::ParametricValue,
1305 : : double,
1306 : : // calculate connected route distances: too short (invalid result), normal
1307 : : physics::Distance,
1308 : : physics::Distance,
1309 : : // expected connecting route results: type, routeA length, routeB length
1310 : : route::ConnectingRouteType,
1311 : : physics::Distance,
1312 : : physics::Distance>>
1313 : : testEntries{
1314 : : // simple standard: directly in front same direction, same lane
1315 : : std::make_tuple(emmyToAda,
1316 : 1 : physics::ParametricValue(0.5),
1317 : 0 : physics::ParametricValue(0.5),
1318 : 1 : 0.,
1319 : : emmyToAda,
1320 : 1 : physics::ParametricValue(0.8),
1321 : 0 : physics::ParametricValue(0.5),
1322 : 1 : 0.,
1323 : 1 : physics::Distance(0.),
1324 : 0 : physics::Distance(50.),
1325 : 1 : route::ConnectingRouteType::Following,
1326 : 1 : physics::Distance(4.33336),
1327 : 1 : physics::Distance(0.)),
1328 : : // simple standard: directly at back same direction, same lane
1329 : : std::make_tuple(emmyToAda,
1330 : 1 : physics::ParametricValue(0.5),
1331 : 0 : physics::ParametricValue(0.5),
1332 : 1 : 0.,
1333 : : emmyToAda,
1334 : 1 : physics::ParametricValue(0.2),
1335 : 0 : physics::ParametricValue(0.5),
1336 : 1 : 0.,
1337 : 1 : physics::Distance(0.),
1338 : 0 : physics::Distance(50.),
1339 : 1 : route::ConnectingRouteType::Following,
1340 : 1 : physics::Distance(0.),
1341 : 1 : physics::Distance(4.32631)),
1342 : : // simple standard: directly in front other direction, other lane
1343 : : std::make_tuple(emmyToAda,
1344 : 1 : physics::ParametricValue(0.5),
1345 : 0 : physics::ParametricValue(0.5),
1346 : 1 : 0.,
1347 : : opositeToEmmyToAda,
1348 : 1 : physics::ParametricValue(0.8),
1349 : 0 : physics::ParametricValue(0.5),
1350 : 1 : 0.,
1351 : 1 : physics::Distance(0.),
1352 : 0 : physics::Distance(50.),
1353 : 1 : route::ConnectingRouteType::Opposing,
1354 : 1 : physics::Distance(4.34512),
1355 : 1 : physics::Distance(4.34421)),
1356 : : // simple standard: already at back other direction, other lane
1357 : : std::make_tuple(emmyToAda,
1358 : 1 : physics::ParametricValue(0.5),
1359 : 0 : physics::ParametricValue(0.5),
1360 : 1 : 0.,
1361 : : opositeToEmmyToAda,
1362 : 1 : physics::ParametricValue(0.2),
1363 : 0 : physics::ParametricValue(0.5),
1364 : 1 : 0.,
1365 : 1 : physics::Distance(0.),
1366 : 0 : physics::Distance(50.),
1367 : 1 : route::ConnectingRouteType::Invalid,
1368 : 1 : physics::Distance(0.),
1369 : 1 : physics::Distance(0.)),
1370 : : // simple standard: Y connected route
1371 : : std::make_tuple(hirtToHaid,
1372 : 1 : physics::ParametricValue(0.2),
1373 : 0 : physics::ParametricValue(0.5),
1374 : 1 : 0.,
1375 : : haidXhirtXHaid,
1376 : 1 : physics::ParametricValue(0.2),
1377 : 0 : physics::ParametricValue(0.5),
1378 : 1 : 0.,
1379 : 1 : physics::Distance(5.),
1380 : 0 : physics::Distance(50.),
1381 : 1 : route::ConnectingRouteType::Merging,
1382 : 1 : physics::Distance(53.5544),
1383 : 1 : physics::Distance(35.5292)),
1384 [ + - + - : 6 : };
+ - + - +
- + - ]
1385 : :
1386 [ + + ]: 6 : for (auto const &testEntry : testEntries)
1387 : : {
1388 : 10 : match::Object egoObject;
1389 : : point::ParaPoint egoParaPoint
1390 : 5 : = point::createParaPoint(std::get<EGO_LANEID>(testEntry), std::get<EGO_LANE_OFFSET_LON>(testEntry));
1391 [ + - ]: 5 : egoObject.enuPosition.centerPoint = lane::getENULanePoint(egoParaPoint, std::get<EGO_LANE_OFFSET_LAT>(testEntry));
1392 [ + - ]: 5 : egoObject.enuPosition.heading = point::createENUHeading(static_cast<double>(lane::getLaneENUHeading(egoParaPoint))
1393 [ + - ]: 5 : + std::get<EGO_HEADING_OFFSET>(testEntry));
1394 : 5 : egoObject.enuPosition.dimension = vehicleDimension;
1395 [ + - ]: 5 : egoObject.enuPosition.enuReferencePoint = access::getENUReferencePoint();
1396 : : egoObject.mapMatchedBoundingBox
1397 [ + - ]: 5 : = mapMatching.getMapMatchedBoundingBox(egoObject.enuPosition, physics::Distance(1.));
1398 : :
1399 : 10 : match::Object otherObject;
1400 : : point::ParaPoint otherParaPoint
1401 : 5 : = point::createParaPoint(std::get<OTHER_LANEID>(testEntry), std::get<OTHER_LANE_OFFSET_LON>(testEntry));
1402 : : otherObject.enuPosition.centerPoint
1403 [ + - ]: 5 : = lane::getENULanePoint(otherParaPoint, std::get<OTHER_LANE_OFFSET_LAT>(testEntry));
1404 : : otherObject.enuPosition.heading = point::createENUHeading(
1405 [ + - + - ]: 5 : static_cast<double>(lane::getLaneENUHeading(otherParaPoint)) + std::get<OTHER_HEADING_OFFSET>(testEntry));
1406 : 5 : otherObject.enuPosition.dimension = vehicleDimension;
1407 [ + - ]: 5 : otherObject.enuPosition.enuReferencePoint = access::getENUReferencePoint();
1408 : : otherObject.mapMatchedBoundingBox
1409 [ + - ]: 5 : = mapMatching.getMapMatchedBoundingBox(otherObject.enuPosition, physics::Distance(2.));
1410 : :
1411 : 5 : auto const tooShortDistance = std::get<MAX_DIST_TOO_SHORT>(testEntry);
1412 [ + - ]: 15 : ConnectingRoute connRoute = route::planning::calculateConnectingRoute(egoObject, otherObject, tooShortDistance);
1413 [ + - - + : 5 : EXPECT_EQ(route::ConnectingRouteType::Invalid, connRoute.type);
- - - - -
- ]
1414 [ + - + - : 5 : EXPECT_EQ(physics::Distance::getMax(), calcLength(connRoute));
- + - - -
- - - ]
1415 : :
1416 : 5 : auto const standardDistance = std::get<MAX_DIST_NORMAL>(testEntry);
1417 [ + - ]: 5 : connRoute = route::planning::calculateConnectingRoute(egoObject, otherObject, standardDistance);
1418 [ + - - + : 5 : EXPECT_EQ(std::get<RESULT_TYPE>(testEntry), connRoute.type);
- - - - -
- ]
1419 : 5 : RouteBehaviour routeBehavior = RouteBehaviour::RouteIsConstant;
1420 [ + + ]: 5 : if (route::ConnectingRouteType::Merging == connRoute.type)
1421 : : {
1422 [ + - - + : 1 : EXPECT_EQ(connRoute.routeA.roadSegments.back().drivableLaneSegments.size(),
- - - - -
- ]
1423 : : connRoute.routeB.roadSegments.back().drivableLaneSegments.size());
1424 [ + + ]: 3 : for (auto i = 0u; i < connRoute.routeA.roadSegments.back().drivableLaneSegments.size(); i++)
1425 : : {
1426 [ + - - + : 2 : EXPECT_TRUE(route::isDegenerated(connRoute.routeA.roadSegments.back().drivableLaneSegments[i].laneInterval));
- - - - -
- - - ]
1427 [ + - - + : 2 : EXPECT_EQ(connRoute.routeA.roadSegments.back().drivableLaneSegments[i].laneInterval,
- - - - -
- ]
1428 : : connRoute.routeB.roadSegments.back().drivableLaneSegments[i].laneInterval);
1429 : : }
1430 : 1 : routeBehavior = RouteBehaviour::RouteIsBroadening;
1431 : : }
1432 [ + - + - : 5 : EXPECT_EQ(std::get<RESULT_ROUTE_EGO_LENGTH>(testEntry), calcLength(connRoute.routeA));
- + - - -
- - - ]
1433 [ + - + - : 5 : EXPECT_EQ(std::get<RESULT_ROUTE_OTHER_LENGTH>(testEntry), calcLength(connRoute.routeB));
- + - - -
- - - ]
1434 [ + - ]: 5 : validateRouteConnections(connRoute.routeA, routeBehavior);
1435 [ + - ]: 5 : validateRouteConnections(connRoute.routeB, routeBehavior);
1436 [ + - ]: 5 : validateRouteParaPoints(connRoute.routeA);
1437 [ + - ]: 5 : validateRouteParaPoints(connRoute.routeB);
1438 : : }
1439 : : }
|
