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/intersection/Intersection.hpp"
10 : :
11 : : #include <math.h>
12 : :
13 : : #include "ad/map/access/Logging.hpp"
14 : : #include "ad/map/access/Operation.hpp"
15 : : #include "ad/map/landmark/LandmarkOperation.hpp"
16 : : #include "ad/map/lane/LaneOperation.hpp"
17 : : #include "ad/map/point/Operation.hpp"
18 : : #include "ad/map/route/RouteOperation.hpp"
19 : :
20 : : namespace ad {
21 : : namespace map {
22 : : namespace intersection {
23 : :
24 : 942 : IntersectionType fromContactTypes(lane::ContactTypeList const &types)
25 : : {
26 [ + + ]: 1408 : for (auto contactType : types)
27 : : {
28 : : // fallthrough is intended here
29 [ + - + + : 942 : switch (contactType)
- + + + +
- ]
30 : : {
31 : : // clang-format off
32 : : // switched off to keep the formatting of the comments
33 : 211 : case lane::ContactType::FREE: ///< There is no physical or legal obstacles between two objects.
34 : 476 : return IntersectionType::HasWay;
35 : 0 : case lane::ContactType::STOP: ///< STOP regulation at the end of the lane.
36 : 0 : return IntersectionType::Stop;
37 : 2 : case lane::ContactType::STOP_ALL: ///< STOP 3-way, 4-way etc regulation at the end of the lane.
38 : 2 : return IntersectionType::AllWayStop;
39 : 185 : case lane::ContactType::YIELD: ///< YIELD regulation at the end of the lane.
40 : 185 : return IntersectionType::Yield;
41 : 0 : case lane::ContactType::CROSSWALK: ///< Crosswalk at the end of the lane.
42 : 0 : return IntersectionType::Crosswalk;
43 : 29 : case lane::ContactType::PRIO_TO_RIGHT: ///< Priority to right at the end of the lane.
44 : 29 : return IntersectionType::PriorityToRight;
45 : 12 : case lane::ContactType::PRIO_TO_RIGHT_AND_STRAIGHT: ///< Priority to right and straight at the end of the lane.
46 : 12 : return IntersectionType::PriorityToRightAndStraight;
47 : 37 : case lane::ContactType::TRAFFIC_LIGHT: ///< Traffic light on the way
48 : 37 : return IntersectionType::TrafficLight;
49 : 466 : case lane::ContactType::SPEED_BUMP: ///< Speed bump at the end of the lane.
50 : : case lane::ContactType::INVALID:
51 : : case lane::ContactType::UNKNOWN: ///< Unknown value.
52 : : case lane::ContactType::LANE_CHANGE: ///< Transition between one lane to direct lateral neighboor.
53 : : case lane::ContactType::LANE_CONTINUATION: ///< Transition between one lane to longitudinal direct neighboor.
54 : : case lane::ContactType::LANE_END: ///< End of the Lane - line.
55 : : case lane::ContactType::SINGLE_POINT: ///< End of the Lane - point.
56 : : case lane::ContactType::GATE_BARRIER: ///< Gate with barrier at the end of the lane.
57 : : case lane::ContactType::GATE_TOLBOOTH: ///< Tolbooth barrier at the end of the lane.
58 : : case lane::ContactType::GATE_SPIKES: ///< Spikes (in direction) at the end of the lane.
59 : : case lane::ContactType::GATE_SPIKES_CONTRA: ///< Spikes (in opposite direction) at the end of the lane.
60 : : case lane::ContactType::CURB_UP: ///< Curb up at the object side.
61 : : case lane::ContactType::CURB_DOWN: ///< Curb down at the object.
62 : : case lane::ContactType::RIGHT_OF_WAY:
63 : 466 : break; // in all other cases, we do nothing, maybe another contact has valid information.
64 : : // clang-format on
65 : 0 : default:
66 [ # # ]: 0 : throw std::invalid_argument("Intersection fromContactTypes: Invalid contact type passed");
67 : : }
68 : : }
69 : 466 : return IntersectionType::Unknown;
70 : : }
71 : :
72 : 476 : IntersectionType getRightOfWayForTransition(lane::LaneId fromLaneId, lane::LaneId toLaneId, bool useSuccessor)
73 : : {
74 [ + - + - ]: 952 : auto fromLane = lane::getLane(fromLaneId);
75 : 476 : auto location = lane::ContactLocation::PREDECESSOR;
76 [ + + ]: 476 : if (useSuccessor)
77 : : {
78 : 273 : location = lane::ContactLocation::SUCCESSOR;
79 : : }
80 [ + - ]: 952 : auto contacts = getContactLanes(fromLane, location);
81 [ + - ]: 2160 : for (auto const &contact : contacts)
82 : : {
83 [ + - + + ]: 2160 : if (contact.toLane == toLaneId)
84 : : {
85 [ + - ]: 942 : auto result = fromContactTypes(contact.types);
86 [ + + ]: 942 : if (result != IntersectionType::Unknown)
87 : : {
88 : 476 : return result;
89 : : }
90 : : }
91 : : }
92 : 0 : return IntersectionType::Unknown;
93 : : }
94 : :
95 : 369 : IntersectionPtr Intersection::getIntersectionForRoadSegment(route::RouteIterator const &routeIterator)
96 : : {
97 : 369 : IntersectionPtr result;
98 : 369 : route::RoadSegmentList::const_iterator routePreviousSegmentIter;
99 [ + - ]: 369 : bool const intersectionStart = isRoadSegmentEnteringIntersection(routeIterator, routePreviousSegmentIter);
100 [ + + ]: 369 : if (intersectionStart)
101 : : {
102 : : try
103 : : {
104 [ + - ]: 222 : result = IntersectionPtr(
105 [ + - + - ]: 222 : new Intersection(routeIterator.route, routePreviousSegmentIter, routeIterator.roadSegmentIterator));
106 : : }
107 : 0 : catch (...)
108 : : {
109 [ - - - - : 0 : access::getLogger()->warn("Failed to create intersection at route iterator {} for route: {}",
- - ]
110 : 0 : *routeIterator.roadSegmentIterator,
111 : : routeIterator.route);
112 : : }
113 : : }
114 : 738 : return result;
115 : : }
116 : :
117 : 110 : std::vector<IntersectionPtr> Intersection::getIntersectionsForRoute(route::FullRoute const &route)
118 : : {
119 : 110 : std::vector<IntersectionPtr> result;
120 [ + + ]: 475 : for (auto roadSegmentIter = route.roadSegments.begin(); roadSegmentIter != route.roadSegments.end();
121 : 365 : roadSegmentIter++)
122 : : {
123 : : // @todo: also consider segments leaving intersection if first segment is already within intersection
124 [ + - ]: 730 : auto roadSegmentResult = getIntersectionForRoadSegment(route::RouteIterator(route, roadSegmentIter));
125 [ + + ]: 365 : if (bool(roadSegmentResult))
126 : : {
127 [ + - ]: 110 : result.push_back(roadSegmentResult);
128 : : }
129 : : }
130 : 110 : return result;
131 : : }
132 : :
133 : 1 : IntersectionPtr Intersection::getNextIntersectionOnRoute(route::FullRoute const &route)
134 : : {
135 : 1 : IntersectionPtr result;
136 [ + - ]: 4 : for (auto roadSegmentIter = route.roadSegments.begin(); roadSegmentIter != route.roadSegments.end();
137 : 3 : roadSegmentIter++)
138 : : {
139 : : // @todo: also consider segments leaving intersection if first segment is already within intersection
140 [ + - ]: 4 : result = getIntersectionForRoadSegment(route::RouteIterator(route, roadSegmentIter));
141 [ + + ]: 4 : if (bool(result))
142 : : {
143 : 1 : return result;
144 : : }
145 : : }
146 : 0 : return result;
147 : : }
148 : :
149 : 300 : bool Intersection::segmentLeavesIntersectionOnRoute(route::FullRoute const &route,
150 : : route::RoadSegmentList::const_iterator const &roadSegmentIt,
151 : : lane::LaneId laneId)
152 : : {
153 : : // end of iterator? -> false
154 : : // lane is of type intersection? -> iterate through successors
155 : : // lane of type normal -> true
156 [ - + ]: 300 : if (roadSegmentIt == route.roadSegments.end())
157 : : {
158 : 0 : return false;
159 : : }
160 [ + + ]: 300 : if (isLanePartOfAnIntersection(laneId))
161 : : {
162 : 176 : mLanesOnRoute.insert(laneId);
163 [ + + ]: 253 : for (auto const &segment : roadSegmentIt->drivableLaneSegments)
164 : : {
165 [ + - + + ]: 233 : if (segment.laneInterval.laneId == laneId)
166 : : {
167 [ + + ]: 176 : for (auto successorId : segment.successors)
168 : : {
169 [ + - + - ]: 156 : if (segmentLeavesIntersectionOnRoute(route, roadSegmentIt + 1, successorId))
170 : : {
171 : 156 : return true;
172 : : }
173 : : }
174 : : }
175 : : }
176 : 20 : return false; // we did not find a (successor of a) a successor that leaves the intersection
177 : : }
178 : : else
179 : : {
180 : : // laneId is outside of the intersection, thus the route left the intersection
181 : 124 : mOutgoingLanesOnRoute.insert(laneId);
182 : 124 : return true;
183 : : }
184 : : return false;
185 : : }
186 : :
187 : : lane::LaneIdSet
188 : 143 : Intersection::successorsOnRouteLeavingIntersection(route::FullRoute const &route,
189 : : route::RoadSegmentList::const_iterator const &roadSegmentIt,
190 : : route::LaneSegment const &laneSegment)
191 : : {
192 : 143 : lane::LaneIdSet result;
193 [ + + ]: 287 : for (auto successorId : laneSegment.successors)
194 : : {
195 [ + - + + ]: 144 : if (segmentLeavesIntersectionOnRoute(route, roadSegmentIt + 1, successorId))
196 : : {
197 [ + - ]: 124 : result.insert(successorId);
198 : : }
199 : : }
200 : 143 : return result;
201 : : }
202 : :
203 : 1 : lane::LaneIdSet Intersection::successorsOnRouteLeavingIntersection(route::LaneSegment const &laneSegment)
204 : : {
205 : 1 : lane::LaneIdSet result;
206 [ + + ]: 2 : for (auto successorId : laneSegment.successors)
207 : : {
208 : : auto allReachableInternalAndOutgoingLanes
209 [ + - ]: 2 : = getAllReachableInternalAndOutgoingLanes(successorId, SuccessorMode::AnyIntersection);
210 [ + - ]: 1 : mLanesOnRoute.insert(allReachableInternalAndOutgoingLanes.first.begin(),
211 : : allReachableInternalAndOutgoingLanes.first.end());
212 [ + - ]: 1 : mOutgoingLanesOnRoute.insert(allReachableInternalAndOutgoingLanes.second.begin(),
213 : : allReachableInternalAndOutgoingLanes.second.end());
214 [ + - ]: 1 : if (!allReachableInternalAndOutgoingLanes.second.empty())
215 : : {
216 [ + - ]: 1 : result.insert(successorId);
217 : : }
218 : : }
219 : 1 : return result;
220 : : }
221 : :
222 : 144 : void Intersection::extractRightOfWayAndCollectTrafficLights(route::LaneInterval const &laneInterval,
223 : : lane::LaneIdSet const &successors,
224 : : lane::LaneId &toLaneId)
225 : : {
226 : 144 : auto beforeLaneId = laneInterval.laneId;
227 [ + - ]: 144 : bool const useSuccessor = (laneInterval.start < laneInterval.end);
228 [ + + ]: 269 : for (auto succId : successors)
229 : : {
230 [ + - ]: 125 : auto intersectionType = getRightOfWayForTransition(beforeLaneId, succId, useSuccessor);
231 [ + + - + ]: 125 : if ((mIntersectionType != IntersectionType::Unknown) && (mIntersectionType != intersectionType))
232 : : {
233 [ # # # # ]: 0 : access::getLogger()->warn("Different types of intersection detected! From {} To {}", beforeLaneId, succId);
234 : : }
235 : 125 : toLaneId = succId;
236 : 125 : mIntersectionType = intersectionType;
237 [ + - ]: 125 : collectTrafficLights(beforeLaneId, toLaneId, useSuccessor);
238 : : }
239 : 144 : }
240 : :
241 : 111 : Intersection::Intersection(route::FullRoute const &route,
242 : : route::RoadSegmentList::const_iterator const &lastSegmentBeforeIntersection,
243 : 111 : route::RoadSegmentList::const_iterator const &firstSegmentWithinIntersection)
244 : 111 : : mRoutePlanningCounter(route.routePlanningCounter)
245 : 222 : , mSegmentCountFromDestination(firstSegmentWithinIntersection->segmentCountFromDestination)
246 : 111 : , mSpeedLimit(std::numeric_limits<physics::Speed>::max())
247 : : {
248 : : // ensure all intersection arms are initialized, to be safe when later accessing with std::map::at()
249 [ + - ]: 111 : mIntersectionArms[TurnDirection::Unknown] = lane::LaneIdSet();
250 [ + - ]: 111 : mIntersectionArms[TurnDirection::Right] = lane::LaneIdSet();
251 [ + - ]: 111 : mIntersectionArms[TurnDirection::Straight] = lane::LaneIdSet();
252 [ + - ]: 111 : mIntersectionArms[TurnDirection::Left] = lane::LaneIdSet();
253 [ + - ]: 111 : mIntersectionArms[TurnDirection::UTurn] = lane::LaneIdSet();
254 : :
255 : : // we don't need to check if the iterators are valid because this is called
256 : : // from within getIntersectionForRoadSegment() only if the checks succeed
257 : 111 : lane::LaneId toLaneId{0};
258 [ + + ]: 254 : for (auto const &segment : lastSegmentBeforeIntersection->drivableLaneSegments)
259 : : {
260 [ + - ]: 143 : mIncomingLanesOnRoute.insert(segment.laneInterval.laneId);
261 [ + - ]: 286 : auto successors = successorsOnRouteLeavingIntersection(route, lastSegmentBeforeIntersection, segment);
262 [ + - ]: 143 : extractRightOfWayAndCollectTrafficLights(segment.laneInterval, successors, toLaneId);
263 : : }
264 [ + - + + ]: 111 : if (toLaneId == lane::LaneId(0))
265 : : {
266 : : // seems as if the route doesn't lead out of the intersection
267 : : // so try to collect all possible ways out
268 [ + + ]: 2 : for (auto const &segment : lastSegmentBeforeIntersection->drivableLaneSegments)
269 : : {
270 [ + - ]: 2 : auto successors = successorsOnRouteLeavingIntersection(segment);
271 [ + - ]: 1 : extractRightOfWayAndCollectTrafficLights(segment.laneInterval, successors, toLaneId);
272 : : }
273 : : }
274 : :
275 [ + - ]: 111 : extractLanesOfCoreIntersection(toLaneId);
276 [ + - - + ]: 111 : if (!isLanePartOfCoreIntersection(toLaneId))
277 : : {
278 [ # # ]: 0 : throw std::runtime_error("Lane is not part of intersection!");
279 : : }
280 : :
281 [ + - ]: 111 : extractIncomingLanes();
282 [ + - ]: 111 : calculateParaPoints();
283 [ + - ]: 111 : extractCrossingLanes();
284 [ + - ]: 111 : orderIntersectionArmsAndExtractTurnDirection();
285 [ + - ]: 111 : extractLanesWithHigherPriority();
286 [ + - ]: 111 : extractLanesWithLowerPriority();
287 [ + - ]: 111 : extractLanesFromSameIntersectionArm();
288 [ + - ]: 111 : calculateEnteringProrityParaPoints();
289 [ + - ]: 111 : calculateSpeedLimit();
290 : 111 : }
291 : :
292 : 201 : IntersectionType Intersection::intersectionType() const
293 : : {
294 : 201 : return mIntersectionType;
295 : : }
296 : :
297 : 3 : TurnDirection Intersection::turnDirection() const
298 : : {
299 : 3 : return mTurnDirection;
300 : : }
301 : :
302 : 23 : landmark::LandmarkIdSet const &Intersection::applicableTrafficLights() const
303 : : {
304 : 23 : return mTrafficLightIds;
305 : : }
306 : :
307 : 3 : lane::LaneIdSet const &Intersection::lanesOnRoute() const
308 : : {
309 : 3 : return mLanesOnRoute;
310 : : }
311 : :
312 : 6 : point::ParaPointList const &Intersection::paraPointsOnRoute() const
313 : : {
314 : 6 : return mParaPointsOnRoute;
315 : : }
316 : :
317 : 3 : lane::LaneIdSet const &Intersection::incomingLanesOnRoute() const
318 : : {
319 : 3 : return mIncomingLanesOnRoute;
320 : : }
321 : :
322 : 3 : lane::LaneIdSet const &Intersection::outgoingLanesOnRoute() const
323 : : {
324 : 3 : return mOutgoingLanesOnRoute;
325 : : }
326 : :
327 : 4 : point::ParaPointList const &Intersection::incomingParaPointsOnRoute() const
328 : : {
329 : 4 : return mIncomingParaPointsOnRoute;
330 : : }
331 : :
332 : 7 : point::ParaPointList const &Intersection::outgoingParaPointsOnRoute() const
333 : : {
334 : 7 : return mOutgoingParaPointsOnRoute;
335 : : }
336 : :
337 : 3 : point::ParaPointList const &Intersection::outgoingParaPoints() const
338 : : {
339 : 3 : return exitParaPoints();
340 : : }
341 : :
342 : 76 : lane::LaneIdSet const &Intersection::internalLanesWithHigherPriority() const
343 : : {
344 : 76 : return mInternalLanesWithHigherPriority;
345 : : }
346 : :
347 : 3 : lane::LaneIdSet const &Intersection::internalLanesWithLowerPriority() const
348 : : {
349 : 3 : return mInternalLanesWithLowerPriority;
350 : : }
351 : :
352 : 98 : lane::LaneIdSet const &Intersection::incomingLanesWithLowerPriority() const
353 : : {
354 : 98 : return mIncomingLanesWithLowerPriority;
355 : : }
356 : :
357 : 98 : lane::LaneIdSet const &Intersection::incomingLanesWithHigherPriority() const
358 : : {
359 : 98 : return mIncomingLanesWithHigherPriority;
360 : : }
361 : :
362 : 99 : lane::LaneIdSet const &Intersection::incomingLanes() const
363 : : {
364 : 99 : return mIncomingLanes;
365 : : }
366 : :
367 : 3 : lane::LaneIdSet const &Intersection::outgoingLanes() const
368 : : {
369 : 3 : return exitLanes();
370 : : }
371 : :
372 : 98 : point::ParaPointList const &Intersection::incomingParaPoints() const
373 : : {
374 : 98 : return mIncomingParaPoints;
375 : : }
376 : :
377 : 100 : point::ParaPointList const &Intersection::incomingParaPointsWithHigherPriority() const
378 : : {
379 : 100 : return mIncomingParaPointsWithHigherPriority;
380 : : }
381 : :
382 : 98 : point::ParaPointList const &Intersection::incomingParaPointsWithLowerPriority() const
383 : : {
384 : 98 : return mIncomingParaPointsWithLowerPriority;
385 : : }
386 : :
387 : 77 : lane::LaneIdSet const &Intersection::crossingLanes() const
388 : : {
389 : 77 : return mCrossingLanes;
390 : : }
391 : :
392 : 7 : route::RoutePlanningCounter Intersection::getRoutePlanningCounter() const
393 : : {
394 : 7 : return mRoutePlanningCounter;
395 : : }
396 : :
397 : 7 : route::SegmentCounter Intersection::getRouteSegmentCountFromDestination() const
398 : : {
399 : 7 : return mSegmentCountFromDestination;
400 : : }
401 : :
402 : 6 : void Intersection::updateRouteCounters(route::RoutePlanningCounter newRoutePlanningCounter,
403 : : route::SegmentCounter newRouteSegmentCounter)
404 : : {
405 : 6 : mRoutePlanningCounter = newRoutePlanningCounter;
406 : 6 : mSegmentCountFromDestination = newRouteSegmentCounter;
407 : 6 : }
408 : :
409 : 1 : route::RouteParaPoint Intersection::getIntersectionStartOnRoute() const
410 : : {
411 : 1 : route::RouteParaPoint result;
412 : 1 : result.routePlanningCounter = getRoutePlanningCounter();
413 : 1 : result.segmentCountFromDestination = getRouteSegmentCountFromDestination();
414 : 1 : result.parametricOffset = physics::ParametricValue(0.);
415 : 1 : return result;
416 : : }
417 : :
418 : 111 : void Intersection::extractIncomingLanes()
419 : : {
420 [ + - + + ]: 594 : for (auto laneId : entryLanes())
421 : : {
422 [ + - + + ]: 483 : if (mIncomingLanesOnRoute.count(laneId) == 0)
423 : : {
424 [ + - ]: 340 : mIncomingLanes.insert(laneId);
425 [ + - + - ]: 340 : mIncomingParaPoints.push_back(getEntryParaPointOfExternalLane(laneId));
426 : : }
427 : : }
428 : 111 : }
429 : :
430 : 111 : void Intersection::calculateParaPoints()
431 : : {
432 [ + + ]: 254 : for (auto laneId : mIncomingLanesOnRoute)
433 : : {
434 [ + - + - ]: 143 : mIncomingParaPointsOnRoute.push_back(getEntryParaPointOfExternalLane(laneId));
435 : : }
436 : :
437 [ + + ]: 236 : for (auto outGoingLaneId : mOutgoingLanesOnRoute)
438 : : {
439 [ + - + - ]: 125 : mOutgoingParaPointsOnRoute.push_back(getExitParaPointOfExternalLane(outGoingLaneId));
440 : : }
441 : :
442 [ + + ]: 287 : for (auto laneId : mLanesOnRoute)
443 : : {
444 [ + - ]: 176 : auto paraPoint = getEntryParaPointOfExternalLane(laneId);
445 [ + - ]: 176 : mParaPointsOnRoute.push_back(paraPoint);
446 : : }
447 : 111 : }
448 : :
449 : 111 : void Intersection::extractCrossingLanes()
450 : : {
451 : : // first, collect all lanes that overlap with any lane of the route
452 [ + + ]: 287 : for (auto insideId : mLanesOnRoute)
453 : : {
454 [ + - + - : 176 : mCrossingLanes.insert(mOverlapping[insideId].begin(), mOverlapping[insideId].end());
+ - ]
455 : : }
456 : : // remove all lanes that are a successor of one of the incoming lanes on the route
457 : : // otherwise vehicles driving in the same or parallel direction would also be considered...
458 : 222 : lane::LaneIdSet internalSuccessorsOfRouteEntry;
459 [ + + ]: 254 : for (auto laneId : mIncomingLanesOnRoute)
460 : : {
461 [ + - ]: 286 : auto succ = getAllSuccessorsInLaneDirectionWithinIntersection(laneId, SuccessorMode::OwnIntersection);
462 [ + - ]: 143 : internalSuccessorsOfRouteEntry.insert(succ.begin(), succ.end());
463 : : }
464 [ + + ]: 488 : for (auto laneId : internalSuccessorsOfRouteEntry)
465 : : {
466 [ + - ]: 377 : mCrossingLanes.erase(laneId);
467 : : }
468 : 111 : }
469 : :
470 : : // this function return the directional angle of laneId
471 : 1034 : point::ENUHeading getLaneDirectionalAngle(lane::LaneId laneId)
472 : : {
473 [ + - + - ]: 1034 : auto lane = lane::getLane(laneId);
474 : :
475 : : // Collect the parapoint of the lane
476 : : physics::ParametricValue laneParapoint
477 [ + + ]: 1034 : = (isLaneDirectionNegative(lane) ? physics::ParametricValue(0.) : physics::ParametricValue(1.));
478 : :
479 : : // get the starting point and end point of the lane
480 [ + - ]: 1034 : point::ECEFPoint laneStartPoint = getParametricPoint(lane, laneParapoint, physics::ParametricValue(0.5));
481 : : point::ECEFPoint laneEndPoint
482 [ + - + - ]: 1034 : = getParametricPoint(lane, physics::ParametricValue(1.) - laneParapoint, physics::ParametricValue(0.5));
483 : :
484 : : // get the directional vector
485 [ + - ]: 1034 : point::ECEFHeading directionalVector = point::createECEFHeading(laneStartPoint, laneEndPoint);
486 [ + - ]: 1034 : point::ENUHeading headingENU = point::createENUHeading(directionalVector, laneStartPoint);
487 : :
488 : 2068 : return headingENU;
489 : : }
490 : :
491 : 222 : static void directionOfLanes(point::ParaPointList const &lanes,
492 : : point::ENUHeading referenceAngle,
493 : : std::vector<std::pair<point::ENUHeading, point::ParaPoint>> &intersectionLanesDirection)
494 : :
495 : : {
496 [ + + ]: 1145 : for (auto const &lane : lanes)
497 : : {
498 [ + - ]: 923 : auto angle = getLaneDirectionalAngle(lane.laneId);
499 : :
500 : : // all angles should be relative to the angle of the arm where the route enters the intersection.
501 [ + - ]: 923 : angle = angle - referenceAngle;
502 [ + - + + ]: 923 : if (angle < point::ENUHeading(0))
503 : : {
504 [ + - ]: 478 : angle += point::ENUHeading(2 * M_PI);
505 : : }
506 [ + - ]: 923 : intersectionLanesDirection.push_back({angle, lane});
507 : : }
508 : 222 : }
509 : :
510 : 3167 : point::ENUHeading degToENUHeading(double angle)
511 : : {
512 : 3167 : return point::ENUHeading(angle * M_PI / 180.);
513 : : }
514 : :
515 : 923 : TurnDirection turnDirectionForAngle(point::ENUHeading const angle)
516 : : {
517 : : // we assume that the angle is in range [0 2 PI] where 0 is the direction of the entering lane
518 [ + - + - : 923 : if ((angle > degToENUHeading(315.)) || (angle < degToENUHeading(45.)))
+ + + - +
- + + +
+ ]
519 : : {
520 : 263 : return TurnDirection::UTurn;
521 : : }
522 [ + - + + ]: 660 : else if (angle > degToENUHeading(225.))
523 : : {
524 : 205 : return TurnDirection::Left;
525 : : }
526 [ + - + + ]: 455 : else if (angle > degToENUHeading(135.))
527 : : {
528 : 224 : return TurnDirection::Straight;
529 : : }
530 [ + - + - ]: 231 : else if (angle > degToENUHeading(45.))
531 : : {
532 : 231 : return TurnDirection::Right;
533 : : }
534 : : else
535 : : {
536 : : // we should never come here
537 : 0 : return TurnDirection::Unknown;
538 : : }
539 : : }
540 : :
541 : 111 : void Intersection::orderIntersectionArmsAndExtractTurnDirection()
542 : : {
543 : 222 : std::vector<std::pair<point::ENUHeading, point::ParaPoint>> intersectionLanesDirection;
544 : :
545 : 111 : auto const egoIntersectionEntryLaneId = *mIncomingLanesOnRoute.begin();
546 : :
547 : : // angle (around up-axis) of the incoming lane, used as reference angle
548 [ + - ]: 111 : auto const referenceAngle = getLaneDirectionalAngle(egoIntersectionEntryLaneId);
549 : :
550 : : // calculate the angle for all incoming lanes of the intersection
551 [ + - ]: 111 : directionOfLanes(mEntryParaPoints, referenceAngle, intersectionLanesDirection);
552 : :
553 : : // calculate the angle for all outgoing lanes of the intersection
554 [ + - + - ]: 111 : directionOfLanes(mExitParaPoints, (referenceAngle + point::ENUHeading(M_PI)), intersectionLanesDirection);
555 : :
556 : : // Group all incoming/outgoing lanes into arms (ordered counterclockwise).
557 : :
558 [ + + ]: 1034 : for (auto laneDirection : intersectionLanesDirection)
559 : : {
560 [ + - ]: 923 : auto turnDirection = turnDirectionForAngle(laneDirection.first);
561 [ + + + - : 923 : if ((mTurnDirection == TurnDirection::Unknown) && (mOutgoingLanesOnRoute.count(laneDirection.second.laneId) > 0))
+ + + + ]
562 : : {
563 : 111 : mTurnDirection = turnDirection;
564 : : }
565 [ + - + - ]: 923 : mIntersectionArms[turnDirection].insert(laneDirection.second.laneId);
566 : : }
567 : 111 : }
568 : :
569 : 111 : void Intersection::extractLanesWithHigherPriority()
570 : : {
571 [ + + + - : 111 : switch (mIntersectionType)
+ + - ]
572 : : {
573 : 13 : case IntersectionType::Yield: // fallthrough intended here
574 : : case IntersectionType::Stop:
575 : 13 : adjustLanesForYield();
576 : 13 : break;
577 : 2 : case IntersectionType::Unknown: // fallthrough intended here, unknown means be careful with crossing lanes
578 : : case IntersectionType::AllWayStop:
579 : : // nothing to be done, simply use the crossing lanes
580 : 2 : mInternalLanesWithHigherPriority = mCrossingLanes;
581 : 2 : break;
582 : 22 : case IntersectionType::HasWay:
583 : 22 : adjustLanesForHasWay();
584 : 22 : break;
585 : 0 : case IntersectionType::Crosswalk: // no need for adaptation, there should not be any crossing lanes
586 : 0 : break;
587 : 41 : case IntersectionType::PriorityToRight:
588 : : case IntersectionType::PriorityToRightAndStraight:
589 : 41 : adjustPriorityToRight();
590 : 41 : break;
591 : 33 : case IntersectionType::TrafficLight:
592 : 33 : adjustLanesForTrafficLight();
593 : 33 : break;
594 : 0 : default:
595 : : // throw exception?
596 : 0 : break;
597 : : }
598 : 111 : }
599 : :
600 : 111 : void Intersection::extractLanesWithLowerPriority()
601 : : {
602 [ + + ]: 1499 : for (auto laneId : mInternalLanes)
603 : : {
604 [ + - ]: 1388 : if ((mLanesOnRoute.find(laneId) == mLanesOnRoute.end())
605 [ + + + - : 1388 : && (mInternalLanesWithHigherPriority.find(laneId) == mInternalLanesWithHigherPriority.end()))
+ + + + ]
606 : : {
607 [ + - ]: 925 : mInternalLanesWithLowerPriority.insert(laneId);
608 : : }
609 : : }
610 : 111 : }
611 : :
612 : 111 : void Intersection::extractLanesFromSameIntersectionArm()
613 : : {
614 [ + + ]: 254 : for (auto laneId : mIncomingLanesOnRoute)
615 : : {
616 [ + - ]: 286 : auto relevantLanes = getAllSuccessorsInLaneDirectionWithinIntersection(laneId, SuccessorMode::OwnIntersection);
617 [ + - ]: 143 : mInternalLanesFromSameIntersectionArm.insert(relevantLanes.begin(), relevantLanes.end());
618 : : }
619 : 111 : }
620 : :
621 : 111 : void Intersection::calculateEnteringProrityParaPoints()
622 : : {
623 [ + + ]: 451 : for (auto laneId : mIncomingLanes)
624 : : {
625 [ + - ]: 680 : auto successorLanes = getAllSuccessorsInLaneDirectionWithinIntersection(laneId, SuccessorMode::OwnIntersection);
626 : 340 : bool internalPriorityFound = false;
627 : :
628 : 1153 : for (auto successorLaneIter = successorLanes.begin();
629 [ + + + + : 1153 : successorLaneIter != successorLanes.end() && !internalPriorityFound;
+ + ]
630 : 813 : successorLaneIter++)
631 : : {
632 [ + - + + ]: 813 : if (mInternalLanesWithHigherPriority.find(*successorLaneIter) != mInternalLanesWithHigherPriority.end())
633 : : {
634 : 103 : internalPriorityFound = true;
635 : : }
636 : : }
637 [ + + ]: 340 : if (internalPriorityFound)
638 : : {
639 [ + - ]: 103 : auto paraPoint = getEntryParaPointOfExternalLane(laneId);
640 [ + - ]: 103 : mIncomingParaPointsWithHigherPriority.push_back(paraPoint);
641 [ + - ]: 103 : mIncomingLanesWithHigherPriority.insert(laneId);
642 : : }
643 : : else
644 : : {
645 [ + - ]: 237 : auto paraPoint = getEntryParaPointOfExternalLane(laneId);
646 [ + - ]: 237 : mIncomingParaPointsWithLowerPriority.push_back(paraPoint);
647 [ + - ]: 237 : mIncomingLanesWithLowerPriority.insert(laneId);
648 : : }
649 : : }
650 : 111 : }
651 : :
652 : 256 : bool Intersection::turnDirectionCrossesStraightTraffic() const
653 : : {
654 : : // right handed traffic
655 [ + + ]: 256 : if (access::isRightHandedTraffic())
656 : : {
657 [ + + + + ]: 160 : return (mTurnDirection == TurnDirection::Left) || (mTurnDirection == TurnDirection::UTurn);
658 : : }
659 : : // left handed traffic
660 : : else
661 : : {
662 [ + + - + ]: 96 : return (mTurnDirection == TurnDirection::Right) || (mTurnDirection == TurnDirection::UTurn);
663 : : }
664 : : }
665 : :
666 : 44 : bool Intersection::outgoingIntersectionArmCanBeReached(lane::LaneId const laneId,
667 : : TurnDirection const outgoingIntersectionArm)
668 : : {
669 [ + - + + ]: 72 : for (auto reachableOutgoingLaneId : getAllReachableOutgoingLanes(laneId, SuccessorMode::OwnIntersection))
670 : : {
671 [ + - + - : 44 : if (mIntersectionArms[outgoingIntersectionArm].count(reachableOutgoingLaneId) > 0)
+ + ]
672 : : {
673 : 16 : return true;
674 : : }
675 : : }
676 : 28 : return false;
677 : : }
678 : :
679 : 108 : void Intersection::addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(
680 : : lane::LaneId const &lane, TurnDirection const restrictToOutgoingIntersectionArm)
681 : : {
682 [ + - ]: 216 : auto lanesToCheck = getLaneAndAllSuccessorsInLaneDirectionWithinIntersection(lane, SuccessorMode::OwnIntersection);
683 [ + + ]: 244 : for (auto laneToCheckId : lanesToCheck)
684 : : {
685 [ + - + + ]: 136 : if (mCrossingLanes.count(laneToCheckId) > 0)
686 : : {
687 : 98 : if ((restrictToOutgoingIntersectionArm == TurnDirection::Unknown)
688 [ + + + - : 98 : || outgoingIntersectionArmCanBeReached(laneToCheckId, restrictToOutgoingIntersectionArm))
+ + + + ]
689 : : {
690 [ + - ]: 86 : mInternalLanesWithHigherPriority.insert(laneToCheckId);
691 : : }
692 : : }
693 : : }
694 : 108 : }
695 : :
696 : 24 : void Intersection::addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(
697 : : lane::LaneIdSet const &lanes, TurnDirection const restrictToOutgoingIntersectionArm)
698 : : {
699 [ + + ]: 75 : for (auto laneId : lanes)
700 : : {
701 [ + - ]: 51 : addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(laneId, restrictToOutgoingIntersectionArm);
702 : : }
703 : 24 : }
704 : :
705 : 168 : void Intersection::addLaneAndSuccessorsToInternalLanesWithHigherPriority(
706 : : lane::LaneId const &lane, TurnDirection const restrictToOutgoingIntersectionArm)
707 : : {
708 [ + - ]: 336 : auto lanesToCheck = getLaneAndAllSuccessorsInLaneDirectionWithinIntersection(lane, SuccessorMode::OwnIntersection);
709 [ + + ]: 377 : for (auto laneToCheckId : lanesToCheck)
710 : : {
711 : 209 : if ((restrictToOutgoingIntersectionArm == TurnDirection::Unknown)
712 [ + + + - : 209 : || outgoingIntersectionArmCanBeReached(laneToCheckId, restrictToOutgoingIntersectionArm))
+ + + + ]
713 : : {
714 [ + - ]: 193 : mInternalLanesWithHigherPriority.insert(laneToCheckId);
715 : : }
716 : : }
717 : 168 : }
718 : :
719 : 45 : void Intersection::addLaneAndSuccessorsToInternalLanesWithHigherPriority(
720 : : lane::LaneIdSet const &lanes, TurnDirection const restrictToOutgoingIntersectionArm)
721 : : {
722 [ + + ]: 133 : for (auto laneId : lanes)
723 : : {
724 [ + - ]: 88 : addLaneAndSuccessorsToInternalLanesWithHigherPriority(laneId, restrictToOutgoingIntersectionArm);
725 : : }
726 : 45 : }
727 : :
728 : 22 : void Intersection::adjustLanesForHasWay()
729 : : {
730 [ + + ]: 132 : for (auto const &intersectionArm : mIntersectionArms)
731 : : {
732 [ + + ]: 264 : for (auto laneId : intersectionArm.second)
733 : : {
734 [ + - ]: 308 : auto sucessorLanes = getDirectSuccessorsInLaneDirectionWithinIntersection(laneId, SuccessorMode::OwnIntersection);
735 [ + + ]: 344 : for (auto sucessorLaneId : sucessorLanes)
736 : : {
737 : : auto trafficRuleAtOtherLane
738 [ + - + - ]: 190 : = getRightOfWayForTransition(laneId, sucessorLaneId, !isLaneDirectionNegative(laneId));
739 : :
740 : : // has way lanes have to be respected if we turn
741 [ + + + - : 190 : if ((trafficRuleAtOtherLane == IntersectionType::HasWay) && turnDirectionCrossesStraightTraffic())
+ + + + ]
742 : : {
743 [ + - ]: 24 : addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(sucessorLaneId);
744 : : }
745 : : }
746 : : }
747 : : }
748 : 22 : }
749 : :
750 : 13 : void Intersection::adjustLanesForYield()
751 : : {
752 [ + + ]: 78 : for (auto const &intersectionArm : mIntersectionArms)
753 : : {
754 [ + + ]: 172 : for (auto laneId : intersectionArm.second)
755 : : {
756 [ + - ]: 214 : auto sucessorLanes = getDirectSuccessorsInLaneDirectionWithinIntersection(laneId, SuccessorMode::OwnIntersection);
757 [ + + ]: 268 : for (auto sucessorLaneId : sucessorLanes)
758 : : {
759 : : auto trafficRuleAtOtherLane
760 [ + - + - ]: 161 : = getRightOfWayForTransition(laneId, sucessorLaneId, !isLaneDirectionNegative(laneId));
761 : : // need to give way to all of lanes that have way
762 [ + + ]: 161 : if (trafficRuleAtOtherLane == IntersectionType::HasWay)
763 : : {
764 [ + - ]: 80 : addLaneAndSuccessorsToInternalLanesWithHigherPriority(sucessorLaneId);
765 : : }
766 : : // yield lanes only have to be respected if we turn
767 [ + - + - : 81 : else if ((trafficRuleAtOtherLane == IntersectionType::Yield) && turnDirectionCrossesStraightTraffic())
+ + + + ]
768 : : {
769 [ + - ]: 33 : addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(sucessorLaneId);
770 : : }
771 : : }
772 : : }
773 : : }
774 : 13 : }
775 : :
776 : 41 : void Intersection::adjustPriorityToRight()
777 : : {
778 [ + + ]: 41 : if ((IntersectionType::PriorityToRightAndStraight == mIntersectionType)
779 [ + + ]: 12 : && (mTurnDirection == TurnDirection::Straight))
780 : : {
781 : : // if we drive straight, only the straight and left turning has prio
782 [ + - + - ]: 4 : addLaneAndSuccessorsToInternalLanesWithHigherPriority(mIntersectionArms[TurnDirection::Right], TurnDirection::Left);
783 [ + - + - ]: 4 : addLaneAndSuccessorsToInternalLanesWithHigherPriority(mIntersectionArms[TurnDirection::Right],
784 : : TurnDirection::UTurn);
785 : : }
786 : : else
787 : : {
788 : : // all traffic from right
789 [ + - + - ]: 37 : addLaneAndSuccessorsToInternalLanesWithHigherPriority(mIntersectionArms[TurnDirection::Right]);
790 : : }
791 : :
792 : : // plus take care on the straight traffic when turning
793 [ + + ]: 41 : if (turnDirectionCrossesStraightTraffic())
794 : : {
795 [ + - + - ]: 12 : addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(mIntersectionArms[TurnDirection::Straight]);
796 [ + + ]: 12 : if (IntersectionType::PriorityToRightAndStraight == mIntersectionType)
797 : : {
798 : : // the other straight driving lanes have priority, too
799 [ + - + - ]: 4 : addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(mIntersectionArms[TurnDirection::Right],
800 : : TurnDirection::Left);
801 [ + - + - ]: 4 : addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(mIntersectionArms[TurnDirection::Left],
802 : : TurnDirection::Right);
803 : : }
804 : : }
805 : 41 : }
806 : :
807 : 33 : void Intersection::adjustLanesForTrafficLight()
808 : : {
809 [ + + + + : 33 : if (turnDirectionCrossesStraightTraffic() && onlySolidTrafficLightsOnRoute())
+ + ]
810 : : {
811 [ + - + - ]: 4 : addLaneAndSuccessorsToInternalLanesWithHigherPriorityIfCrossing(mIntersectionArms[TurnDirection::Straight]);
812 : : }
813 : 33 : }
814 : :
815 : 2 : bool Intersection::objectOnIncomingLane(match::MapMatchedObjectBoundingBox const &object) const
816 : : {
817 [ + + ]: 2 : for (auto const &occupiedLane : object.laneOccupiedRegions)
818 : : {
819 [ + - + - ]: 1 : if (mIncomingLanes.count(occupiedLane.laneId) > 0)
820 : : {
821 : 1 : return true;
822 : : }
823 : : }
824 : 1 : return false;
825 : : }
826 : :
827 : 4 : bool Intersection::objectOnInternalLaneWithLowerPriority(match::MapMatchedObjectBoundingBox const &object) const
828 : : {
829 [ + + ]: 5 : for (auto const &occupiedLane : object.laneOccupiedRegions)
830 : : {
831 [ + - + + ]: 2 : if (mInternalLanesWithLowerPriority.count(occupiedLane.laneId) > 0)
832 : : {
833 : 1 : return true;
834 : : }
835 : : }
836 : 3 : return false;
837 : : }
838 : :
839 : 4 : bool Intersection::objectOnIncomingLaneWithLowerPriority(match::MapMatchedObjectBoundingBox const &object) const
840 : : {
841 [ + + ]: 4 : for (auto const &occupiedLane : object.laneOccupiedRegions)
842 : : {
843 [ + - + - ]: 2 : if (mIncomingLanesWithLowerPriority.count(occupiedLane.laneId) > 0)
844 : : {
845 : 2 : return true;
846 : : }
847 : : }
848 : 2 : return false;
849 : : }
850 : :
851 : 2 : bool Intersection::objectOnLaneWithLowerPriority(match::MapMatchedObjectBoundingBox const &object) const
852 : : {
853 [ + - + + ]: 2 : return objectOnInternalLaneWithLowerPriority(object) || objectOnIncomingLaneWithLowerPriority(object);
854 : : }
855 : :
856 : 3 : bool Intersection::objectOnInternalLaneWithHigherPriority(match::MapMatchedObjectBoundingBox const &object) const
857 : : {
858 [ + + ]: 3 : for (auto const &occupiedLane : object.laneOccupiedRegions)
859 : : {
860 [ + - + - ]: 1 : if (mInternalLanesWithHigherPriority.count(occupiedLane.laneId) > 0)
861 : : {
862 : 1 : return true;
863 : : }
864 : : }
865 : 2 : return false;
866 : : }
867 : :
868 : 3 : bool Intersection::objectOnIncomingLaneWithHigherPriority(match::MapMatchedObjectBoundingBox const &object) const
869 : : {
870 [ + + ]: 3 : for (auto const &occupiedLane : object.laneOccupiedRegions)
871 : : {
872 [ + - + - ]: 1 : if (mIncomingLanesWithHigherPriority.count(occupiedLane.laneId) > 0)
873 : : {
874 : 1 : return true;
875 : : }
876 : : }
877 : 2 : return false;
878 : : }
879 : :
880 : 1 : bool Intersection::objectOnLaneWithHigherPriority(match::MapMatchedObjectBoundingBox const &object) const
881 : : {
882 [ + - - + ]: 1 : return objectOnInternalLaneWithHigherPriority(object) || objectOnIncomingLaneWithHigherPriority(object);
883 : : }
884 : :
885 : 2 : bool Intersection::objectOnCrossingLane(match::MapMatchedObjectBoundingBox const &object) const
886 : : {
887 [ + + ]: 2 : for (auto const &occupiedLane : object.laneOccupiedRegions)
888 : : {
889 [ + - + - ]: 1 : if (mCrossingLanes.count(occupiedLane.laneId) > 0)
890 : : {
891 : 1 : return true;
892 : : }
893 : : }
894 : 1 : return false;
895 : : }
896 : :
897 : 2 : bool Intersection::objectOnIntersectionRoute(match::MapMatchedObjectBoundingBox const &object) const
898 : : {
899 [ + + ]: 2 : for (auto const &occupiedLane : object.laneOccupiedRegions)
900 : : {
901 [ + - + - ]: 1 : if (mLanesOnRoute.count(occupiedLane.laneId) > 0)
902 : : {
903 : 1 : return true;
904 : : }
905 : : }
906 : 1 : return false;
907 : : }
908 : :
909 : 1 : bool Intersection::objectRouteCrossesIntersectionRoute(route::FullRoute const &objectRoute) const
910 : : {
911 [ + + ]: 3 : for (auto crossingLaneId : mCrossingLanes)
912 : : {
913 [ + - ]: 2 : auto findResult = route::findWaypoint(crossingLaneId, objectRoute);
914 [ - + ]: 2 : if (findResult.isValid())
915 : : {
916 : 0 : return true;
917 : : }
918 : : }
919 : 1 : return false;
920 : : }
921 : :
922 : 1 : bool Intersection::objectRouteFromSameArmAsIntersectionRoute(route::FullRoute const &objectRoute) const
923 : : {
924 : 1 : bool routeLaneFound = false;
925 [ + - ]: 6 : for (auto const &roadSegment : objectRoute.roadSegments)
926 : : {
927 [ + + ]: 17 : for (auto const &laneSegment : roadSegment.drivableLaneSegments)
928 : : {
929 [ + - ]: 12 : if ((mInternalLanesFromSameIntersectionArm.count(laneSegment.laneInterval.laneId) > 0)
930 [ + + + - : 12 : || (mIncomingLanesOnRoute.count(laneSegment.laneInterval.laneId) > 0))
+ + + + ]
931 : : {
932 : 8 : routeLaneFound = true;
933 : : }
934 [ + - - + ]: 4 : else if (mInternalLanes.count(laneSegment.laneInterval.laneId) > 0)
935 : : {
936 : : // another internal lane is found in the route, so route differs
937 : 1 : return false;
938 : : }
939 [ + + ]: 4 : else if (routeLaneFound)
940 : : {
941 : : // if we previously have found a route lane and are now again outside the intersection: route is identical
942 : 1 : return routeLaneFound;
943 : : }
944 : : else
945 : : {
946 : : // nothing decided yet since neither a lane on intersection route found nor any other internal lane touched
947 : : }
948 : : }
949 : : }
950 : 0 : return routeLaneFound;
951 : : }
952 : :
953 : 1 : bool Intersection::objectRouteOppositeToIntersectionRoute(route::FullRoute const &objectRoute) const
954 : : {
955 [ + - ]: 1 : route::FindWaypointResult findResult(objectRoute);
956 [ + - + - ]: 1 : for (auto const &laneOnOutgoingIntersectionArm : mIntersectionArms.at(mTurnDirection))
957 : : {
958 [ + - + - ]: 1 : findResult = route::findWaypoint(laneOnOutgoingIntersectionArm, objectRoute);
959 [ + - ]: 1 : if (findResult.isValid())
960 : : {
961 : 1 : break;
962 : : }
963 : : }
964 [ - + ]: 1 : if (!findResult.isValid())
965 : : {
966 : : // check if the object route starts already within the intersection
967 [ # # ]: 0 : if (!objectRoute.roadSegments.empty() && !objectRoute.roadSegments.begin()->drivableLaneSegments.empty()
968 [ # # # # : 0 : && mInternalLanes.count(objectRoute.roadSegments.begin()->drivableLaneSegments.begin()->laneInterval.laneId)
# # # # ]
969 : : > 0)
970 : : {
971 : : // object is already within intersection
972 [ # # # # ]: 0 : for (auto const &laneOnOutgoingIntersectionArm : mIntersectionArms.at(mTurnDirection))
973 : : {
974 : : auto internalLanesFromOutgoingIntersectionArm = getAllSuccessorsInLaneDirectionWithinIntersection(
975 [ # # ]: 0 : laneOnOutgoingIntersectionArm, SuccessorMode::OwnIntersection);
976 : 0 : if (internalLanesFromOutgoingIntersectionArm.count(
977 [ # # ]: 0 : objectRoute.roadSegments.begin()->drivableLaneSegments.begin()->laneInterval.laneId)
978 [ # # ]: 0 : > 0)
979 : : {
980 : : // and object came from outgoing intersection arm
981 : 0 : findResult = route::findWaypoint(
982 [ # # # # ]: 0 : objectRoute.roadSegments.begin()->drivableLaneSegments.begin()->laneInterval.laneId, objectRoute);
983 : : }
984 : : }
985 : : }
986 : : }
987 : :
988 [ + - ]: 1 : if (findResult.isValid())
989 : : {
990 : : // shorten object route
991 [ + - ]: 1 : route::FullRoute remainingObjectRoute(objectRoute);
992 [ + - ]: 1 : route::shortenRoute(findResult.queryPosition, remainingObjectRoute);
993 : :
994 : : // now search for leaving the intersection at the route incoming arm
995 [ + - + + ]: 5 : for (auto const &laneOnIncomingIntersectionArm : mIntersectionArms.at(TurnDirection::UTurn))
996 : : {
997 [ + - ]: 4 : auto result = route::findWaypoint(laneOnIncomingIntersectionArm, remainingObjectRoute);
998 [ - + ]: 4 : if (result.isValid())
999 : : {
1000 : 0 : return true;
1001 : : }
1002 : : }
1003 : : }
1004 : 1 : return false;
1005 : : }
1006 : :
1007 : 1 : physics::Distance Intersection::objectInterpenetrationDistanceWithIntersection(match::Object const &object) const
1008 : : {
1009 : 1 : physics::Distance maxDistance(0.);
1010 : 1 : TurnDirection coveredIntersectionArms = TurnDirection::Unknown;
1011 [ + + ]: 2 : for (auto const &occupiedLane : object.mapMatchedBoundingBox.laneOccupiedRegions)
1012 : : {
1013 [ + - + - ]: 1 : if (mInternalLanes.count(occupiedLane.laneId) > 0u)
1014 : : {
1015 : : // internal lane
1016 : : // if this lane is at the intersection border and the object is still touching that border
1017 : : // then the range covered within this lane has to be considered for the inter penetration distances
1018 : 1 : bool isRelevantBorderLane = false;
1019 [ + - ]: 1 : if ((occupiedLane.longitudinalRange.maximum == physics::ParametricValue(1.))
1020 [ - + - - : 1 : || (occupiedLane.longitudinalRange.minimum == physics::ParametricValue(0.)))
- - + - ]
1021 : : {
1022 : : // check predecessors
1023 [ + - ]: 1 : auto findPredecessors = mPredecessor.find(occupiedLane.laneId);
1024 [ + - ]: 1 : if (findPredecessors != mPredecessor.end())
1025 : : {
1026 [ + + ]: 2 : for (auto predecessorId : findPredecessors->second)
1027 : : {
1028 [ + - - + ]: 1 : if (!isLanePartOfAnIntersection(predecessorId))
1029 : : {
1030 : : // one of the predecessors is outside of the intersection
1031 [ # # ]: 0 : auto const entryBorderParaPoint = getEntryParaPointOfInternalLane(occupiedLane.laneId);
1032 [ # # ]: 0 : if ((entryBorderParaPoint.parametricOffset == occupiedLane.longitudinalRange.maximum)
1033 [ # # # # : 0 : || (entryBorderParaPoint.parametricOffset == occupiedLane.longitudinalRange.minimum))
# # # # ]
1034 : : {
1035 : 0 : isRelevantBorderLane = true;
1036 : 0 : break;
1037 : : }
1038 : : }
1039 : : }
1040 : : }
1041 [ + - ]: 1 : if (!isRelevantBorderLane)
1042 : : {
1043 : : // check successors
1044 [ + - ]: 1 : auto findSuccessors = mSuccessor.find(occupiedLane.laneId);
1045 [ + - ]: 1 : if (findSuccessors != mSuccessor.end())
1046 : : {
1047 [ + - ]: 1 : for (auto successorId : findSuccessors->second)
1048 : : {
1049 [ + - + - ]: 1 : if (!isLanePartOfAnIntersection(successorId))
1050 : : {
1051 : : // one of the successors is outside of the intersection
1052 [ + - ]: 1 : auto const exitBorderParaPoint = getExitParaPointOfInternalLane(occupiedLane.laneId);
1053 [ + - ]: 1 : if ((exitBorderParaPoint.parametricOffset == occupiedLane.longitudinalRange.maximum)
1054 [ - + - - : 1 : || (exitBorderParaPoint.parametricOffset == occupiedLane.longitudinalRange.minimum))
- - + - ]
1055 : : {
1056 : 1 : isRelevantBorderLane = true;
1057 : 1 : break;
1058 : : }
1059 : : }
1060 : : }
1061 : : }
1062 : : }
1063 : : }
1064 : :
1065 [ + - ]: 1 : if (isRelevantBorderLane)
1066 : : {
1067 [ + - ]: 1 : auto const laneLength = lane::getLane(occupiedLane.laneId).length;
1068 : : physics::Distance const penetrationDistance
1069 [ + - + - ]: 1 : = (occupiedLane.longitudinalRange.maximum - occupiedLane.longitudinalRange.minimum) * laneLength;
1070 [ + - ]: 1 : maxDistance = std::max(maxDistance, penetrationDistance);
1071 : : }
1072 : : else
1073 : : {
1074 : : // otherwise this lane is touched as part of an actual internal intersection part and
1075 : : // we only have to ensure that max-distance becomes > 0.
1076 : : // and so, if no outgoing lane is touched (i.e. no intersection arm),
1077 : : // the case fully within intersection is applied
1078 [ # # ]: 0 : maxDistance = std::max(maxDistance, physics::Distance(0.1));
1079 : : }
1080 : : }
1081 : : else
1082 : : {
1083 : : // not an internal lane, check if different intersection arms covered by the vehicle
1084 [ # # ]: 0 : for (auto const &intersectionArm : mIntersectionArms)
1085 : : {
1086 [ # # # # ]: 0 : if (intersectionArm.second.count(occupiedLane.laneId) > 0u)
1087 : : {
1088 [ # # ]: 0 : if (coveredIntersectionArms == TurnDirection::Unknown)
1089 : : {
1090 : 0 : coveredIntersectionArms = intersectionArm.first;
1091 : : }
1092 [ # # ]: 0 : else if (coveredIntersectionArms != intersectionArm.first)
1093 : : {
1094 : : // covering two different intersection arms, actually crossing the intersection
1095 : 0 : return object.enuPosition.dimension.length;
1096 : : }
1097 : : }
1098 : : }
1099 : : }
1100 : : }
1101 : :
1102 [ + - + - : 1 : if ((maxDistance > physics::Distance(0.)) && (coveredIntersectionArms == TurnDirection::Unknown))
+ - + - ]
1103 : : {
1104 : : // not touching incoming/outgoing lane at all, but touching an internal lane
1105 : : // => fully within intersection
1106 : 1 : maxDistance = object.enuPosition.dimension.length;
1107 : : }
1108 : :
1109 : : // result must not extend length of the object
1110 [ + - ]: 1 : maxDistance = std::min(maxDistance, object.enuPosition.dimension.length);
1111 : 1 : return maxDistance;
1112 : : }
1113 : :
1114 : 1 : bool Intersection::objectRouteCrossesLanesWithHigherPriority(route::FullRoute const &objectRoute) const
1115 : : {
1116 [ - + ]: 1 : for (auto laneWithHigherPrioId : mInternalLanesWithHigherPriority)
1117 : : {
1118 [ # # ]: 0 : auto findResult = route::findWaypoint(laneWithHigherPrioId, objectRoute);
1119 [ # # ]: 0 : if (findResult.isValid())
1120 : : {
1121 : 0 : return true;
1122 : : }
1123 : : }
1124 : :
1125 : 1 : return false;
1126 : : }
1127 : :
1128 : 125 : void Intersection::collectTrafficLights(lane::LaneId fromLaneId, lane::LaneId toLaneId, bool useSuccessor)
1129 : : {
1130 : : // keep the first match, we just need any match
1131 [ + - + - ]: 250 : auto fromLane = lane::getLane(fromLaneId);
1132 : 125 : auto location = lane::ContactLocation::PREDECESSOR;
1133 [ + + ]: 125 : if (useSuccessor)
1134 : : {
1135 : 78 : location = lane::ContactLocation::SUCCESSOR;
1136 : : }
1137 [ + - ]: 250 : auto contacts = getContactLanes(fromLane, location);
1138 [ + + ]: 705 : for (auto const &contact : contacts)
1139 : : {
1140 [ + - + + ]: 580 : if (contact.toLane == toLaneId)
1141 : : {
1142 [ + + ]: 564 : for (auto contactType : contact.types)
1143 : : {
1144 [ + + ]: 282 : if (contactType == lane::ContactType::TRAFFIC_LIGHT)
1145 : : {
1146 [ + - ]: 69 : mTrafficLightIds.insert(contact.trafficLightId);
1147 : : }
1148 : : }
1149 : : }
1150 : : }
1151 : 125 : }
1152 : :
1153 : 3 : physics::Speed Intersection::getSpeedLimit() const
1154 : : {
1155 : 3 : return mSpeedLimit;
1156 : : }
1157 : :
1158 : 111 : void Intersection::calculateSpeedLimit()
1159 : : {
1160 [ + + ]: 1499 : for (auto const &internalLaneId : mInternalLanes)
1161 : : {
1162 [ + - ]: 1388 : route::LaneInterval laneInterval;
1163 : 1388 : laneInterval.laneId = internalLaneId;
1164 : 1388 : laneInterval.start = physics::ParametricValue(0.);
1165 : 1388 : laneInterval.end = physics::ParametricValue(1.);
1166 [ + - ]: 2776 : auto speedLimits = route::getSpeedLimits(laneInterval);
1167 [ + + ]: 2776 : for (auto const &speedLimit : speedLimits)
1168 : : {
1169 [ + - ]: 1388 : mSpeedLimit = std::min(mSpeedLimit, speedLimit.speedLimit);
1170 : : }
1171 : : }
1172 : 111 : }
1173 : :
1174 : 18 : landmark::TrafficLightType Intersection::extractTrafficLightType(landmark::LandmarkId trafficLightId)
1175 : : {
1176 [ + - + - ]: 18 : auto trafficLight = landmark::getLandmark(trafficLightId);
1177 : 36 : return trafficLight.trafficLightType;
1178 : : }
1179 : :
1180 : 14 : bool Intersection::isSolidTrafficLight(landmark::LandmarkId trafficLightId)
1181 : : {
1182 [ + + - ]: 14 : switch (extractTrafficLightType(trafficLightId))
1183 : : {
1184 : 8 : case landmark::TrafficLightType::BIKE_PEDESTRIAN_RED_GREEN:
1185 : : case landmark::TrafficLightType::BIKE_PEDESTRIAN_RED_YELLOW_GREEN:
1186 : : case landmark::TrafficLightType::BIKE_RED_GREEN:
1187 : : case landmark::TrafficLightType::BIKE_RED_YELLOW_GREEN:
1188 : : case landmark::TrafficLightType::PEDESTRIAN_RED_GREEN:
1189 : : case landmark::TrafficLightType::PEDESTRIAN_RED_YELLOW_GREEN:
1190 : : case landmark::TrafficLightType::RIGHT_RED_YELLOW_GREEN:
1191 : : case landmark::TrafficLightType::RIGHT_STRAIGHT_RED_YELLOW_GREEN:
1192 : : case landmark::TrafficLightType::STRAIGHT_RED_YELLOW_GREEN:
1193 : : case landmark::TrafficLightType::LEFT_RED_YELLOW_GREEN:
1194 : : case landmark::TrafficLightType::LEFT_STRAIGHT_RED_YELLOW_GREEN:
1195 : 8 : return false;
1196 : : break;
1197 : 6 : case landmark::TrafficLightType::INVALID:
1198 : : case landmark::TrafficLightType::UNKNOWN:
1199 : : case landmark::TrafficLightType::SOLID_RED_YELLOW:
1200 : : case landmark::TrafficLightType::SOLID_RED_YELLOW_GREEN:
1201 : 6 : return true;
1202 : : break;
1203 : 0 : default:
1204 [ # # ]: 0 : throw std::invalid_argument("Intersection isSolidTrafficLight: Invalid traffic light type given");
1205 : : }
1206 : : }
1207 : :
1208 : 12 : bool Intersection::onlySolidTrafficLightsOnRoute()
1209 : : {
1210 [ + + ]: 18 : for (auto trafficLightId : mTrafficLightIds)
1211 : : {
1212 [ + - + + ]: 14 : if (!isSolidTrafficLight(trafficLightId))
1213 : : {
1214 : 8 : return false;
1215 : : }
1216 : : }
1217 : 4 : return true;
1218 : : }
1219 : :
1220 : : } // namespace intersection
1221 : : } // namespace map
1222 : : } // namespace ad
1223 : :
1224 : : namespace std {
1225 : :
1226 : 0 : std::ostream &operator<<(std::ostream &os, ::ad::map::intersection::Intersection const &intersection)
1227 : : {
1228 : 0 : os << static_cast<::ad::map::intersection::CoreIntersection const &>(intersection);
1229 : 0 : os << "Intersection[";
1230 [ # # ]: 0 : os << toString(intersection.intersectionType());
1231 : 0 : os << "]";
1232 : 0 : os << std::endl;
1233 : 0 : os << "->internalLanes: ";
1234 : 0 : os << intersection.internalLanes();
1235 : 0 : os << std::endl;
1236 : 0 : os << "->internalLanesWithHigherPriority: ";
1237 : 0 : os << intersection.internalLanesWithHigherPriority();
1238 : 0 : os << std::endl;
1239 : 0 : os << "->incomingLanes: ";
1240 : 0 : os << intersection.incomingLanes();
1241 : 0 : os << std::endl;
1242 : 0 : os << "->incomingParaPoints: ";
1243 : 0 : os << intersection.incomingParaPoints();
1244 : 0 : os << std::endl;
1245 : 0 : os << "->incomingParaPointsWithHigherPriority: ";
1246 : 0 : os << intersection.incomingParaPointsWithHigherPriority();
1247 : 0 : os << std::endl;
1248 : 0 : os << "->crossingLanes";
1249 : 0 : os << intersection.crossingLanes();
1250 : 0 : os << std::endl;
1251 : 0 : os << "->lanesOnRoute: ";
1252 : 0 : os << intersection.lanesOnRoute();
1253 : 0 : os << std::endl;
1254 : 0 : os << "->incomingLanesOnRoute: ";
1255 : 0 : os << intersection.incomingLanesOnRoute();
1256 : 0 : os << std::endl;
1257 : 0 : os << "->incomingParaPointsOnRoute: ";
1258 : 0 : os << intersection.incomingParaPointsOnRoute();
1259 : 0 : return os;
1260 : : }
1261 : :
1262 : : } // namespace std
|
