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/test_support/IntersectionTestBase.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include "ad/map/access/Operation.hpp"
13 : : #include "ad/map/lane/LaneOperation.hpp"
14 : : #include "ad/map/match/AdMapMatching.hpp"
15 : : #include "ad/map/point/Types.hpp"
16 : : #include "ad/map/route/Planning.hpp"
17 : :
18 : : namespace ad {
19 : : namespace map {
20 : : namespace test_support {
21 : :
22 : : /** Special handling required for intersections as we also test intersections which only have three
23 : : * arms. In that case one direction only provides 'null'-points. These will throw an exception in
24 : : * the underlying map, although the test base assumes that those will be set to LaneId 0
25 : : */
26 : 784 : lane::LaneId laneIdFromPoint(point::GeoPoint const &p)
27 : : {
28 [ + + ]: 784 : if (!point::isValid(p, false))
29 : : {
30 : 66 : return lane::LaneId();
31 : : }
32 : 718 : return lane::uniqueLaneId(p);
33 : : }
34 : :
35 : 98 : void IntersectionTestBase::SetUp()
36 : : {
37 [ + - ]: 98 : prepareMap();
38 : :
39 : : // first transform geo to LaneId
40 [ + - + - ]: 98 : mFromNorth = laneIdFromPoint(getGeoFromNorth());
41 [ + - + - ]: 98 : mToNorth = laneIdFromPoint(getGeoToNorth());
42 [ + - + - ]: 98 : mFromSouth = laneIdFromPoint(getGeoFromSouth());
43 [ + - + - ]: 98 : mToSouth = laneIdFromPoint(getGeoToSouth());
44 [ + - + - ]: 98 : mFromWest = laneIdFromPoint(getGeoFromWest());
45 [ + - + - ]: 98 : mToWest = laneIdFromPoint(getGeoToWest());
46 [ + - + - ]: 98 : mFromEast = laneIdFromPoint(getGeoFromEast());
47 [ + - + - ]: 98 : mToEast = laneIdFromPoint(getGeoToEast());
48 : :
49 : : // then plan the route and extract the intersection
50 : :
51 [ + - ]: 98 : auto const routeStartId = getRouteStart();
52 [ + - + - : 177 : if ((routeStartId != mFromEast) && (routeStartId != mFromWest) && (routeStartId != mFromNorth)
+ + + - +
+ ]
53 [ + + + - : 177 : && (routeStartId != mFromSouth))
- + - + ]
54 : : {
55 [ # # ]: 0 : throw std::runtime_error("IntersectionTestBase::SetUp() getRouteStart must return one of the mFrom* lanes.");
56 : : }
57 : :
58 [ + - ]: 98 : auto const routeEndId = getRouteEnd();
59 [ + - + + : 98 : if ((routeEndId != mToEast) && (routeEndId != mToWest) && (routeEndId != mToNorth) && (routeEndId != mToSouth))
+ - + + +
- + + + -
- + - + ]
60 : : {
61 [ # # ]: 0 : throw std::runtime_error("IntersectionTestBase::SetUp() getRouteEnd must return one of the mTo* lanes.");
62 : : }
63 : :
64 [ + - ]: 98 : mPlannedRoute = planRoute(routeStartId, routeEndId);
65 [ + - ]: 98 : mPlannedRouteValues = getRouteBaseValues(routeStartId, routeEndId);
66 [ + - ]: 196 : auto intersections = intersection::Intersection::getIntersectionsForRoute(mPlannedRoute);
67 [ - + ]: 98 : if (intersections.empty())
68 : : {
69 [ # # ]: 0 : throw std::runtime_error("No intersection for route!");
70 : : }
71 : 98 : mIntersection = intersections[0];
72 : :
73 : : // then calculate the inner lanes
74 [ + - ]: 98 : mNorthToSouth = getRouteBaseValues(mFromNorth, mToSouth);
75 [ + - ]: 98 : mNorthToWest = getRouteBaseValues(mFromNorth, mToWest);
76 [ + - ]: 98 : mNorthToEast = getRouteBaseValues(mFromNorth, mToEast);
77 [ + - ]: 98 : mSouthToNorth = getRouteBaseValues(mFromSouth, mToNorth);
78 [ + - ]: 98 : mSouthToWest = getRouteBaseValues(mFromSouth, mToWest);
79 [ + - ]: 98 : mSouthToEast = getRouteBaseValues(mFromSouth, mToEast);
80 [ + - ]: 98 : mWestToNorth = getRouteBaseValues(mFromWest, mToNorth);
81 [ + - ]: 98 : mWestToSouth = getRouteBaseValues(mFromWest, mToSouth);
82 [ + - ]: 98 : mWestToEast = getRouteBaseValues(mFromWest, mToEast);
83 [ + - ]: 98 : mEastToNorth = getRouteBaseValues(mFromEast, mToNorth);
84 [ + - ]: 98 : mEastToSouth = getRouteBaseValues(mFromEast, mToSouth);
85 [ + - ]: 98 : mEastToWest = getRouteBaseValues(mFromEast, mToWest);
86 : :
87 [ + - ]: 98 : verifyImpossibleWays(mNorthToEast);
88 [ + - ]: 98 : verifyImpossibleWays(mNorthToSouth);
89 [ + - ]: 98 : verifyImpossibleWays(mNorthToWest);
90 [ + - ]: 98 : verifyImpossibleWays(mSouthToEast);
91 [ + - ]: 98 : verifyImpossibleWays(mSouthToNorth);
92 [ + - ]: 98 : verifyImpossibleWays(mSouthToWest);
93 [ + - ]: 98 : verifyImpossibleWays(mWestToEast);
94 [ + - ]: 98 : verifyImpossibleWays(mWestToNorth);
95 [ + - ]: 98 : verifyImpossibleWays(mWestToSouth);
96 [ + - ]: 98 : verifyImpossibleWays(mEastToNorth);
97 [ + - ]: 98 : verifyImpossibleWays(mEastToSouth);
98 [ + - ]: 98 : verifyImpossibleWays(mEastToWest);
99 : 98 : }
100 : :
101 : 98 : void IntersectionTestBase::TearDown()
102 : : {
103 : 98 : access::cleanup();
104 : 98 : }
105 : :
106 : 2662 : route::FullRoute IntersectionTestBase::planRoute(lane::LaneId const &routeStartId, lane::LaneId const &routeEndId) const
107 : : {
108 [ + - - + : 2662 : if ((!lane::isValid(routeStartId)) || (!lane::isValid(routeEndId)))
- + ]
109 : : {
110 : : throw std::runtime_error("IntersectionTestBase::planRoute() route start or route end are invalid."
111 [ # # ]: 0 : "Did you select intersection arms for your route that actually exist?");
112 : : }
113 : 2662 : return route::planning::planRoute(point::createParaPoint(routeStartId, physics::ParametricValue(0.5)),
114 [ + - ]: 7986 : point::createParaPoint(routeEndId, physics::ParametricValue(0.5)));
115 : : }
116 : :
117 : 1274 : IntersectionTestBase::RouteBasedValues IntersectionTestBase::getRouteBaseValues(lane::LaneId const &routeStartId,
118 : : lane::LaneId const &routeEndId) const
119 : : {
120 : 1274 : RouteBasedValues routeBasedValues;
121 [ + - + + : 1274 : if ((!lane::isValid(routeStartId, false)) || (!lane::isValid(routeEndId, false)))
+ - + + +
+ ]
122 : : {
123 : 153 : return routeBasedValues;
124 : : }
125 : :
126 [ + - ]: 2242 : auto fullRoute = planRoute(routeStartId, routeEndId);
127 [ - + ]: 1121 : if (fullRoute.roadSegments.size() < 3u)
128 : : {
129 : 0 : return routeBasedValues;
130 : : }
131 : :
132 [ + + ]: 4826 : for (uint32_t i = 0; i < fullRoute.roadSegments.size(); i++)
133 : : {
134 [ + + + - ]: 7969 : for (auto drivableLaneSegment : fullRoute.roadSegments[i].drivableLaneSegments)
135 : : {
136 [ + + ]: 4264 : if (i == 0)
137 : : {
138 [ + - ]: 1295 : routeBasedValues.mIncomingLanes.insert(drivableLaneSegment.laneInterval.laneId);
139 [ + - ]: 1295 : routeBasedValues.mIncomingParaPoints.push_back(route::getIntervalEnd(drivableLaneSegment.laneInterval));
140 : : }
141 [ + + ]: 2969 : else if (i == (fullRoute.roadSegments.size() - 1))
142 : : {
143 [ + - ]: 1212 : routeBasedValues.mOutgoingLanes.insert(drivableLaneSegment.laneInterval.laneId);
144 [ + - ]: 1212 : routeBasedValues.mOutgoingParaPoints.push_back(route::getIntervalStart(drivableLaneSegment.laneInterval));
145 : : }
146 : : else
147 : : {
148 [ + - ]: 1757 : routeBasedValues.mInternalLanes.insert(drivableLaneSegment.laneInterval.laneId);
149 [ + - ]: 1757 : routeBasedValues.mInternalLaneIntervals.push_back(drivableLaneSegment.laneInterval);
150 : : }
151 : : }
152 : : }
153 [ + + ]: 2416 : for (auto incomingLane : routeBasedValues.mIncomingLanes)
154 : : {
155 [ + + ]: 2738 : for (auto outcomingLane : routeBasedValues.mOutgoingLanes)
156 : : {
157 [ + - ]: 2886 : auto parallelRoute = planRoute(incomingLane, outcomingLane);
158 : :
159 [ + + ]: 3562 : for (uint32_t i = 1; i < parallelRoute.roadSegments.size() - 1; i++)
160 : : {
161 [ + + + - ]: 5032 : for (auto drivableLaneSegment : parallelRoute.roadSegments[i].drivableLaneSegments)
162 : : {
163 [ + - ]: 2913 : routeBasedValues.mInternalLanes.insert(drivableLaneSegment.laneInterval.laneId);
164 [ + - ]: 2913 : routeBasedValues.mInternalLaneIntervals.push_back(drivableLaneSegment.laneInterval);
165 : : }
166 : : }
167 : : }
168 : : }
169 : :
170 : 1121 : return routeBasedValues;
171 : : }
172 : :
173 : 4664 : lane::LaneIdSet IntersectionTestBase::createUnorderedLaneIdSet(std::vector<lane::LaneIdSet> const &lanesVector) const
174 : : {
175 : 4664 : lane::LaneIdSet resultSet;
176 [ + + ]: 30747 : for (auto const &lanes : lanesVector)
177 : : {
178 [ + - ]: 26083 : resultSet.insert(lanes.begin(), lanes.end());
179 : : }
180 : 4664 : return resultSet;
181 : : }
182 : :
183 : 0 : lane::LaneIdSet IntersectionTestBase::createUnorderedLaneIdSet(std::vector<lane::LaneId> const &lanes) const
184 : : {
185 : 0 : lane::LaneIdSet resultSet;
186 [ # # ]: 0 : resultSet.insert(lanes.begin(), lanes.end());
187 : 0 : return resultSet;
188 : : }
189 : :
190 : 0 : lane::LaneIdSet IntersectionTestBase::createUnorderedLaneIdSet(lane::LaneId const &lane) const
191 : : {
192 : 0 : lane::LaneIdSet resultSet;
193 [ # # ]: 0 : resultSet.insert(lane);
194 : 0 : return resultSet;
195 : : }
196 : :
197 : : point::ParaPointList
198 : 294 : IntersectionTestBase::createParaPointVector(std::vector<point::ParaPointList> const ¶PointsVector) const
199 : : {
200 : 294 : point::ParaPointList resultVector;
201 [ + + ]: 3822 : for (auto const ¶Points : paraPointsVector)
202 : : {
203 [ + + ]: 7035 : for (auto const ¶Point : paraPoints)
204 : : {
205 [ + - ]: 3507 : auto findResult = std::find(resultVector.begin(), resultVector.end(), paraPoint);
206 [ + + ]: 3507 : if (findResult == resultVector.end())
207 : : {
208 [ + - ]: 1248 : resultVector.push_back(paraPoint);
209 : : }
210 : : }
211 : : }
212 : 294 : return resultVector;
213 : : }
214 : :
215 : 1176 : void IntersectionTestBase::verifyImpossibleWays(RouteBasedValues &routeBasedValues)
216 : : {
217 : 1176 : lane::LaneIdSet const allOutgoingLanes = createUnorderedLaneIdSet({mNorthToEast.mOutgoingLanes,
218 : 1176 : mNorthToSouth.mOutgoingLanes,
219 : 1176 : mNorthToWest.mOutgoingLanes,
220 : 1176 : mSouthToEast.mOutgoingLanes,
221 : 1176 : mSouthToNorth.mOutgoingLanes,
222 : 1176 : mSouthToWest.mOutgoingLanes,
223 : 1176 : mWestToEast.mOutgoingLanes,
224 : 1176 : mWestToNorth.mOutgoingLanes,
225 : 1176 : mWestToSouth.mOutgoingLanes,
226 : 1176 : mEastToNorth.mOutgoingLanes,
227 : 1176 : mEastToSouth.mOutgoingLanes,
228 [ + - + - : 17640 : mEastToWest.mOutgoingLanes});
+ - + - +
- + - + -
+ - + - +
- + - + -
+ - + - ]
229 [ + + ]: 2735 : for (auto internalLane : routeBasedValues.mInternalLanes)
230 : : {
231 [ + - + + ]: 1562 : if (allOutgoingLanes.count(internalLane) > 0u)
232 : : {
233 : 3 : routeBasedValues.mIncomingLanes.clear();
234 : 3 : routeBasedValues.mIncomingParaPoints.clear();
235 : 3 : routeBasedValues.mInternalLanes.clear();
236 : 3 : routeBasedValues.mInternalLaneIntervals.clear();
237 : 3 : routeBasedValues.mOutgoingLanes.clear();
238 : 3 : routeBasedValues.mOutgoingParaPoints.clear();
239 : :
240 : 3 : break;
241 : : }
242 : : }
243 : 1176 : }
244 : :
245 : 98 : lane::LaneIdSet IntersectionTestBase::getInternalLanes() const
246 : : {
247 : 98 : return createUnorderedLaneIdSet({mNorthToEast.mInternalLanes,
248 : 98 : mNorthToSouth.mInternalLanes,
249 : 98 : mNorthToWest.mInternalLanes,
250 : 98 : mSouthToEast.mInternalLanes,
251 : 98 : mSouthToNorth.mInternalLanes,
252 : 98 : mSouthToWest.mInternalLanes,
253 : 98 : mWestToEast.mInternalLanes,
254 : 98 : mWestToNorth.mInternalLanes,
255 : 98 : mWestToSouth.mInternalLanes,
256 : 98 : mEastToNorth.mInternalLanes,
257 : 98 : mEastToSouth.mInternalLanes,
258 [ + - + - : 1274 : mEastToWest.mInternalLanes});
+ + - - ]
259 : : }
260 : :
261 : 490 : lane::LaneIdSet IntersectionTestBase::getIncomingLanes() const
262 : : {
263 : : lane::LaneIdSet resultSet = createUnorderedLaneIdSet(
264 [ + - + - ]: 2940 : {getIncomingLanesNorth(), getIncomingLanesSouth(), getIncomingLanesEast(), getIncomingLanesWest()});
265 : :
266 : 490 : return resultSet;
267 : : }
268 : :
269 : 622 : lane::LaneIdSet IntersectionTestBase::getIncomingLanesNorth() const
270 : : {
271 : : lane::LaneIdSet resultSet = createUnorderedLaneIdSet(
272 [ + - + - ]: 3110 : {mNorthToEast.mIncomingLanes, mNorthToSouth.mIncomingLanes, mNorthToWest.mIncomingLanes});
273 [ + + ]: 1385 : for (auto const &incomingLaneOnRoute : mPlannedRouteValues.mIncomingLanes)
274 : : {
275 [ + - ]: 763 : resultSet.erase(incomingLaneOnRoute);
276 : : }
277 : 622 : return resultSet;
278 : : }
279 : :
280 : 642 : lane::LaneIdSet IntersectionTestBase::getIncomingLanesSouth() const
281 : : {
282 : : lane::LaneIdSet resultSet = createUnorderedLaneIdSet(
283 [ + - + - ]: 3210 : {mSouthToEast.mIncomingLanes, mSouthToNorth.mIncomingLanes, mSouthToWest.mIncomingLanes});
284 [ + + ]: 1433 : for (auto const &incomingLaneOnRoute : mPlannedRouteValues.mIncomingLanes)
285 : : {
286 [ + - ]: 791 : resultSet.erase(incomingLaneOnRoute);
287 : : }
288 : 642 : return resultSet;
289 : : }
290 : :
291 : 648 : lane::LaneIdSet IntersectionTestBase::getIncomingLanesEast() const
292 : : {
293 : : lane::LaneIdSet resultSet
294 [ + - + - ]: 3240 : = createUnorderedLaneIdSet({mEastToNorth.mIncomingLanes, mEastToSouth.mIncomingLanes, mEastToWest.mIncomingLanes});
295 [ + + ]: 1449 : for (auto const &incomingLaneOnRoute : mPlannedRouteValues.mIncomingLanes)
296 : : {
297 [ + - ]: 801 : resultSet.erase(incomingLaneOnRoute);
298 : : }
299 : 648 : return resultSet;
300 : : }
301 : :
302 : 634 : lane::LaneIdSet IntersectionTestBase::getIncomingLanesWest() const
303 : : {
304 : : lane::LaneIdSet resultSet
305 [ + - + - ]: 3170 : = createUnorderedLaneIdSet({mWestToEast.mIncomingLanes, mWestToNorth.mIncomingLanes, mWestToSouth.mIncomingLanes});
306 [ + + ]: 1421 : for (auto const &incomingLaneOnRoute : mPlannedRouteValues.mIncomingLanes)
307 : : {
308 [ + - ]: 787 : resultSet.erase(incomingLaneOnRoute);
309 : : }
310 : 634 : return resultSet;
311 : : }
312 : :
313 : 294 : point::ParaPointList IntersectionTestBase::getIncomingParaPoints() const
314 : : {
315 : 294 : point::ParaPointList resultVector = createParaPointVector({mNorthToEast.mIncomingParaPoints,
316 : 294 : mNorthToSouth.mIncomingParaPoints,
317 : 294 : mNorthToWest.mIncomingParaPoints,
318 : 294 : mSouthToEast.mIncomingParaPoints,
319 : 294 : mSouthToNorth.mIncomingParaPoints,
320 : 294 : mSouthToWest.mIncomingParaPoints,
321 : 294 : mWestToEast.mIncomingParaPoints,
322 : 294 : mWestToNorth.mIncomingParaPoints,
323 : 294 : mWestToSouth.mIncomingParaPoints,
324 : 294 : mEastToNorth.mIncomingParaPoints,
325 : 294 : mEastToSouth.mIncomingParaPoints,
326 [ + - + - ]: 4116 : mEastToWest.mIncomingParaPoints});
327 [ + + ]: 657 : for (auto const &incomingParaPointOnRoute : mPlannedRouteValues.mIncomingParaPoints)
328 : : {
329 [ + - ]: 363 : resultVector.erase(std::remove(resultVector.begin(), resultVector.end(), incomingParaPointOnRoute),
330 [ + - ]: 726 : resultVector.end());
331 : : }
332 : 294 : return resultVector;
333 : : }
334 : :
335 : : point::ParaPoint
336 : 0 : IntersectionTestBase::getLaneParaPointFromRouteParametricOffset(lane::LaneId const laneId,
337 : : physics::ParametricValue const routeParametricOffset)
338 : : {
339 : 0 : route::LaneInterval relevantLaneInterval;
340 : 0 : relevantLaneInterval.laneId = lane::LaneId();
341 : 0 : relevantLaneInterval.start = physics::ParametricValue(0.);
342 : 0 : relevantLaneInterval.end = physics::ParametricValue(0.);
343 : 0 : for (auto const &routeBasedValue : {mNorthToSouth,
344 : 0 : mNorthToWest,
345 : 0 : mNorthToEast,
346 : 0 : mSouthToNorth,
347 : 0 : mSouthToWest,
348 : 0 : mSouthToEast,
349 : 0 : mWestToNorth,
350 : 0 : mWestToSouth,
351 : 0 : mWestToEast,
352 : 0 : mEastToNorth,
353 : 0 : mEastToSouth,
354 [ # # # # : 0 : mEastToWest})
# # # # #
# # # # #
# # # # #
# # # # #
# # # # #
# ]
355 : : {
356 : : auto incomingFindResult
357 : : = std::find_if(routeBasedValue.mIncomingParaPoints.begin(),
358 : : routeBasedValue.mIncomingParaPoints.end(),
359 [ # # ]: 0 : [laneId](point::ParaPoint const ¶Point) { return paraPoint.laneId == laneId; });
360 [ # # ]: 0 : if (incomingFindResult != routeBasedValue.mIncomingParaPoints.end())
361 : : {
362 : 0 : relevantLaneInterval.laneId = laneId;
363 : 0 : relevantLaneInterval.end = incomingFindResult->parametricOffset;
364 [ # # ]: 0 : relevantLaneInterval.start = physics::ParametricValue(1.) - relevantLaneInterval.end;
365 : 0 : break;
366 : : }
367 : : auto outgoingFindResult
368 : : = std::find_if(routeBasedValue.mOutgoingParaPoints.begin(),
369 : : routeBasedValue.mOutgoingParaPoints.end(),
370 [ # # ]: 0 : [laneId](point::ParaPoint const ¶Point) { return paraPoint.laneId == laneId; });
371 [ # # ]: 0 : if (outgoingFindResult != routeBasedValue.mOutgoingParaPoints.end())
372 : : {
373 : 0 : relevantLaneInterval.laneId = laneId;
374 : 0 : relevantLaneInterval.start = outgoingFindResult->parametricOffset;
375 [ # # ]: 0 : relevantLaneInterval.end = physics::ParametricValue(1.) - relevantLaneInterval.start;
376 : 0 : break;
377 : : }
378 : : auto internalFindResult
379 : : = std::find_if(routeBasedValue.mInternalLaneIntervals.begin(),
380 : : routeBasedValue.mInternalLaneIntervals.end(),
381 [ # # ]: 0 : [laneId](route::LaneInterval const &laneInterval) { return laneInterval.laneId == laneId; });
382 [ # # ]: 0 : if (internalFindResult != routeBasedValue.mInternalLaneIntervals.end())
383 : : {
384 : 0 : relevantLaneInterval = *internalFindResult;
385 : 0 : break;
386 : : }
387 : : }
388 : :
389 [ # # # # ]: 0 : if (route::isDegenerated(relevantLaneInterval))
390 : : {
391 : : throw std::runtime_error(
392 [ # # ]: 0 : "IntersectionTestBase::getLaneParaPointFromRouteParametricOffset() lane not found within intersection.");
393 : : }
394 : :
395 [ # # ]: 0 : return route::getLaneParaPoint(routeParametricOffset, relevantLaneInterval);
396 : : }
397 : :
398 : : } // namespace test_support
399 : : } // namespace map
400 : : } // namespace ad
|
