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/route/RouteOperation.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include <map>
13 : : #include <set>
14 : :
15 : : #include "RouteOperationPrivate.hpp"
16 : : #include "ad/map/access/Logging.hpp"
17 : : #include "ad/map/access/Operation.hpp"
18 : : #include "ad/map/intersection/Intersection.hpp"
19 : : #include "ad/map/lane/BorderOperation.hpp"
20 : : #include "ad/map/lane/LaneOperation.hpp"
21 : : #include "ad/map/match/MapMatchedOperation.hpp"
22 : : #include "ad/map/point/Operation.hpp"
23 : : #include "ad/map/route/LaneSegment.hpp"
24 : : #include "ad/map/route/Planning.hpp"
25 : :
26 : : namespace ad {
27 : : namespace map {
28 : : namespace route {
29 : :
30 : 11 : RouteIterator getRouteIterator(route::RouteParaPoint const &routePosition, route::FullRoute const &route)
31 : : {
32 : 11 : RouteIterator result(route, route.roadSegments.end());
33 [ + + ]: 11 : if (routePosition.routePlanningCounter != route.routePlanningCounter)
34 : : {
35 : : // route position not matching the route
36 : 2 : return result;
37 : : }
38 : :
39 : 9 : if (route.roadSegments.empty()
40 [ + - ]: 9 : || (route.roadSegments.front().segmentCountFromDestination < routePosition.segmentCountFromDestination)
41 [ + - - + : 18 : || (route.roadSegments.back().segmentCountFromDestination > routePosition.segmentCountFromDestination))
- + ]
42 : : {
43 : : // route position not found within route
44 : 0 : return result;
45 : : }
46 : :
47 [ + - ]: 13 : for (result.roadSegmentIterator = route.roadSegments.begin(); result.roadSegmentIterator != route.roadSegments.end();
48 : 4 : result.roadSegmentIterator++)
49 : : {
50 [ + + ]: 13 : if (result.roadSegmentIterator->segmentCountFromDestination == routePosition.segmentCountFromDestination)
51 : : {
52 : : // found the segment
53 : 9 : break;
54 : : }
55 : : }
56 : 9 : return result;
57 : : }
58 : :
59 : 11 : point::ParaPoint getLaneParaPoint(physics::ParametricValue const &routeParametricOffset,
60 : : route::LaneInterval const &laneInterval)
61 : : {
62 [ + - ]: 11 : point::ParaPoint lanePoint;
63 [ + - + + ]: 11 : if (isDegenerated(laneInterval))
64 : : {
65 [ + - ]: 1 : lanePoint = getIntervalStart(laneInterval);
66 : : }
67 : : else
68 : : {
69 : 10 : lanePoint.laneId = laneInterval.laneId;
70 : : physics::ParametricValue const parametricLengthOffsetFromRouteStart
71 [ + - + - ]: 10 : = calcParametricLength(laneInterval) * routeParametricOffset;
72 [ + - + + ]: 10 : if (isRouteDirectionPositive(laneInterval))
73 : : {
74 [ + - ]: 1 : lanePoint.parametricOffset = laneInterval.start + parametricLengthOffsetFromRouteStart;
75 : : }
76 : : else
77 : : {
78 [ + - ]: 9 : lanePoint.parametricOffset = laneInterval.start - parametricLengthOffsetFromRouteStart;
79 : : }
80 : : }
81 : 11 : return lanePoint;
82 : : }
83 : :
84 : 6 : point::ParaPointList getLaneParaPoints(route::RouteParaPoint const &routePosition, route::FullRoute const &route)
85 : : {
86 : 6 : point::ParaPointList resultPoints;
87 [ + - ]: 6 : auto routeIter = getRouteIterator(routePosition, route);
88 [ + - + - ]: 6 : if (routeIter.isValid())
89 : : {
90 [ + + ]: 15 : for (auto &laneSegment : routeIter.roadSegmentIterator->drivableLaneSegments)
91 : : {
92 [ + - + - ]: 9 : resultPoints.push_back(getLaneParaPoint(routePosition.parametricOffset, laneSegment.laneInterval));
93 : : }
94 : : }
95 : 12 : return resultPoints;
96 : : }
97 : :
98 : 3 : physics::Distance signedDistanceToLane(lane::LaneId const &checkLaneId,
99 : : FullRoute const &route,
100 : : match::MapMatchedPositionConfidenceList const &mapMatchedPositions)
101 : : {
102 [ + - ]: 3 : physics::Distance distance = match::signedDistanceToLane(checkLaneId, mapMatchedPositions);
103 : :
104 [ + - ]: 3 : auto laneInRoute = findWaypoint(checkLaneId, route);
105 : :
106 [ + + ]: 3 : if (!laneInRoute.isValid())
107 : : {
108 [ + - ]: 1 : throw std::runtime_error("::ad::map::route::signedDistanceToLane() laneId not found in route");
109 : : }
110 : :
111 [ + - + - ]: 2 : if (isRouteDirectionNegative(laneInRoute.laneSegmentIterator->laneInterval))
112 : : {
113 [ + - ]: 2 : distance = distance * -1.;
114 : : }
115 : :
116 : 2 : return distance;
117 : : }
118 : :
119 : 47 : physics::Distance calcLength(FullRoute const &fullRoute)
120 : : {
121 : 47 : physics::Distance result(0.);
122 [ + + ]: 155 : for (auto const &roadSegment : fullRoute.roadSegments)
123 : : {
124 [ + - + - ]: 108 : result += calcLength(roadSegment);
125 : : }
126 : 47 : return result;
127 : : }
128 : :
129 : 157 : physics::Distance calcLength(RoadSegment const &roadSegment)
130 : : {
131 : 157 : physics::Distance result = std::numeric_limits<physics::Distance>::max();
132 [ + + ]: 353 : for (auto const &laneSegment : roadSegment.drivableLaneSegments)
133 : : {
134 [ + - ]: 196 : physics::Distance laneSegmentLength = calcLength(laneSegment);
135 [ + - + + ]: 196 : if (laneSegmentLength < result)
136 : : {
137 : 171 : result = laneSegmentLength;
138 : : }
139 : : }
140 : 157 : return result;
141 : : }
142 : :
143 : 5 : physics::Distance calcLength(ConnectingRoute const &connectingRoute)
144 : : {
145 : 5 : physics::Distance result;
146 [ + - ]: 5 : if (connectingRoute.type == ConnectingRouteType::Invalid)
147 : : {
148 : 5 : result = physics::Distance::getMax();
149 : : }
150 : : else
151 : : {
152 [ # # # # : 0 : result = std::max(calcLength(connectingRoute.routeA), calcLength(connectingRoute.routeB));
# # ]
153 : : }
154 : 5 : return result;
155 : : }
156 : :
157 : 1 : physics::Distance calcLength(RouteIterator const &startIterator, RouteIterator const &endIterator)
158 : : {
159 : 1 : physics::Distance distance(0.);
160 [ + - + - : 2 : if (startIterator.isValid() && endIterator.isValid()
+ - ]
161 [ + - + - : 2 : && (std::distance(startIterator.roadSegmentIterator, endIterator.roadSegmentIterator) >= 0u))
+ - + - ]
162 : : {
163 [ + + ]: 2 : for (auto roadSegmentIter = startIterator.roadSegmentIterator; roadSegmentIter < endIterator.roadSegmentIterator;
164 : 1 : roadSegmentIter++)
165 : : {
166 [ + - + - ]: 1 : distance += calcLength(*roadSegmentIter);
167 : : }
168 [ + - + - ]: 1 : distance += calcLength(*endIterator.roadSegmentIterator);
169 : : }
170 : 1 : return distance;
171 : : }
172 : :
173 : : physics::Distance
174 : 3 : calcLength(RouteParaPoint const &startRouteParaPoint, RouteParaPoint const &endRouteParaPoint, FullRoute const &route)
175 : : {
176 : 3 : physics::Distance distance = std::numeric_limits<physics::Distance>::max();
177 : :
178 [ + - ]: 6 : const auto laneParaPointsStart = getLaneParaPoints(startRouteParaPoint, route);
179 [ + - ]: 3 : const auto laneParaPointsEnd = getLaneParaPoints(endRouteParaPoint, route);
180 : :
181 [ + + ]: 6 : for (auto startLaneParaPoint : laneParaPointsStart)
182 : : {
183 [ + - ]: 3 : auto startWaypoint = findWaypoint(startLaneParaPoint, route);
184 [ + - ]: 3 : if (startWaypoint.isValid())
185 : : {
186 [ + + ]: 9 : for (auto endLaneParaPoint : laneParaPointsEnd)
187 : : {
188 [ + - ]: 6 : auto endWaypoint = findWaypoint(endLaneParaPoint, route);
189 [ + - ]: 6 : if (endWaypoint.isValid())
190 : : {
191 [ + - ]: 6 : physics::Distance startDistance = calcLength(startWaypoint);
192 [ + - ]: 6 : physics::Distance endDistance = calcLength(endWaypoint);
193 : :
194 [ + - + - ]: 6 : distance = std::min(std::fabs(startDistance - endDistance), distance);
195 : : }
196 : : }
197 : : }
198 : : }
199 : :
200 : 6 : return distance;
201 : : }
202 : :
203 : 2 : physics::Duration calcDuration(FullRoute const &fullRoute)
204 : : {
205 : 2 : physics::Duration result(0.);
206 [ + + ]: 6 : for (auto const &roadSegment : fullRoute.roadSegments)
207 : : {
208 [ + - + - ]: 4 : result += calcDuration(roadSegment);
209 : : }
210 : 2 : return result;
211 : : }
212 : :
213 : 4 : physics::Duration calcDuration(RoadSegment const &roadSegment)
214 : : {
215 : 4 : physics::Duration result = std::numeric_limits<physics::Duration>::max();
216 [ + + ]: 9 : for (auto const &laneSegment : roadSegment.drivableLaneSegments)
217 : : {
218 [ + - ]: 5 : physics::Duration const laneSegmentDuration = calcDuration(laneSegment);
219 [ + - + - ]: 5 : if (laneSegmentDuration < result)
220 : : {
221 : 5 : result = laneSegmentDuration;
222 : : }
223 : : }
224 : 4 : return result;
225 : : }
226 : :
227 : 0 : physics::Duration calcDuration(ConnectingRoute const &connectingRoute)
228 : : {
229 [ # # # # : 0 : physics::Duration result = std::max(calcDuration(connectingRoute.routeA), calcDuration(connectingRoute.routeB));
# # ]
230 : 0 : return result;
231 : : }
232 : :
233 : 3 : restriction::SpeedLimitList getSpeedLimits(RoadSegment const &roadSegment)
234 : : {
235 : 3 : restriction::SpeedLimitList resultLimits;
236 [ + + ]: 7 : for (auto const &laneSegment : roadSegment.drivableLaneSegments)
237 : : {
238 [ + - ]: 4 : auto const segmentSpeedLimits = getSpeedLimits(laneSegment);
239 [ + - ]: 4 : resultLimits.insert(resultLimits.end(), segmentSpeedLimits.begin(), segmentSpeedLimits.end());
240 : : }
241 : 3 : return resultLimits;
242 : : }
243 : :
244 : 4 : restriction::SpeedLimitList getSpeedLimits(LaneSegment const &laneSegment)
245 : : {
246 : 4 : return getSpeedLimits(laneSegment.laneInterval);
247 : : }
248 : :
249 : 1 : restriction::SpeedLimitList getSpeedLimits(RouteIterator const &startIterator, RouteIterator const &endIterator)
250 : : {
251 : 1 : restriction::SpeedLimitList resultLimits;
252 [ + - + - : 2 : if (startIterator.isValid() && endIterator.isValid()
+ - ]
253 [ + - + - : 2 : && (std::distance(startIterator.roadSegmentIterator, endIterator.roadSegmentIterator) >= 0u))
+ - + - ]
254 : : {
255 [ + + ]: 2 : for (auto roadSegmentIter = startIterator.roadSegmentIterator; roadSegmentIter < endIterator.roadSegmentIterator;
256 : 1 : roadSegmentIter++)
257 : : {
258 [ + - ]: 1 : auto const segmentSpeedLimits = getSpeedLimits(*roadSegmentIter);
259 [ + - ]: 1 : resultLimits.insert(resultLimits.end(), segmentSpeedLimits.begin(), segmentSpeedLimits.end());
260 : : }
261 : : }
262 : 1 : return resultLimits;
263 : : }
264 : :
265 : 1 : restriction::SpeedLimitList getSpeedLimits(FullRoute const &fullRoute)
266 : : {
267 : 1 : restriction::SpeedLimitList resultLimits;
268 [ + + ]: 3 : for (auto const &roadSegment : fullRoute.roadSegments)
269 : : {
270 [ + - ]: 2 : auto const segmentSpeedLimits = getSpeedLimits(roadSegment);
271 [ + - ]: 2 : resultLimits.insert(resultLimits.end(), segmentSpeedLimits.begin(), segmentSpeedLimits.end());
272 : : }
273 : 1 : return resultLimits;
274 : : }
275 : :
276 : 0 : restriction::SpeedLimitList getSpeedLimits(ConnectingRoute const &connectingRoute)
277 : : {
278 [ # # ]: 0 : restriction::SpeedLimitList resultLimits = getSpeedLimits(connectingRoute.routeA);
279 [ # # ]: 0 : auto const speedLimitsB = getSpeedLimits(connectingRoute.routeA);
280 [ # # ]: 0 : resultLimits.insert(resultLimits.end(), speedLimitsB.begin(), speedLimitsB.end());
281 : 0 : return resultLimits;
282 : : }
283 : :
284 : 1 : bool isWithinInterval(RoadSegment const &roadSegment, point::ParaPoint const &point)
285 : : {
286 [ + - ]: 1 : for (auto const &laneSegment : roadSegment.drivableLaneSegments)
287 : : {
288 [ + - + - ]: 1 : if (isWithinInterval(laneSegment.laneInterval, point))
289 : : {
290 : 1 : return true;
291 : : }
292 : : }
293 : :
294 : 0 : return false;
295 : : }
296 : :
297 : 31 : void clearLaneSegmentPredecessors(RoadSegment &roadSegment)
298 : : {
299 [ + + ]: 63 : for (auto &laneSegment : roadSegment.drivableLaneSegments)
300 : : {
301 : 32 : laneSegment.predecessors.clear();
302 : : }
303 : 31 : }
304 : :
305 : 5 : void clearLaneSegmentSuccessors(RoadSegment &roadSegment)
306 : : {
307 [ + + ]: 11 : for (auto &laneSegment : roadSegment.drivableLaneSegments)
308 : : {
309 : 6 : laneSegment.successors.clear();
310 : : }
311 : 5 : }
312 : :
313 : 14 : void updateLaneSegmentNeighbors(RoadSegment &roadSegment)
314 : : {
315 [ - + ]: 14 : if (roadSegment.drivableLaneSegments.empty())
316 : : {
317 : 0 : return;
318 : : }
319 : :
320 : 14 : roadSegment.drivableLaneSegments.front().leftNeighbor = lane::LaneId();
321 : 14 : roadSegment.drivableLaneSegments.back().rightNeighbor = lane::LaneId();
322 : :
323 : 14 : auto leftLaneSegmentIter = roadSegment.drivableLaneSegments.begin();
324 : 14 : auto rightLaneSegmentIter = leftLaneSegmentIter;
325 : 14 : rightLaneSegmentIter++;
326 : :
327 [ - + ]: 14 : while (rightLaneSegmentIter != std::end(roadSegment.drivableLaneSegments))
328 : : {
329 : 0 : leftLaneSegmentIter->rightNeighbor = rightLaneSegmentIter->laneInterval.laneId;
330 : 0 : rightLaneSegmentIter->leftNeighbor = leftLaneSegmentIter->laneInterval.laneId;
331 : :
332 : 0 : leftLaneSegmentIter = rightLaneSegmentIter;
333 : 0 : rightLaneSegmentIter++;
334 : : }
335 : : }
336 : :
337 : 10 : void updateLaneSegmentSuccessors(RoadSegment &roadSegment, RoadSegment const &successorSegment)
338 : : {
339 [ + + ]: 20 : for (auto &laneSegment : roadSegment.drivableLaneSegments)
340 : : {
341 : 0 : laneSegment.successors.erase(std::remove_if(std::begin(laneSegment.successors),
342 : 10 : std::end(laneSegment.successors),
343 : 30 : [&successorSegment](lane::LaneId const &successorId) {
344 : : auto findResult
345 : 10 : = std::find_if(std::begin(successorSegment.drivableLaneSegments),
346 : 10 : std::end(successorSegment.drivableLaneSegments),
347 : 10 : [&successorId](LaneSegment const &segment) {
348 : 10 : return segment.laneInterval.laneId == successorId;
349 [ + - ]: 10 : });
350 : 10 : return findResult == std::end(successorSegment.drivableLaneSegments);
351 [ + - ]: 10 : }),
352 [ + - ]: 20 : laneSegment.successors.end());
353 : : }
354 : 10 : }
355 : :
356 : 10 : void updateLaneSegmentPredecessors(RoadSegment &roadSegment, RoadSegment const &predecessorSegment)
357 : : {
358 [ + + ]: 20 : for (auto &laneSegment : roadSegment.drivableLaneSegments)
359 : : {
360 : : laneSegment.predecessors.erase(
361 : 0 : std::remove_if(std::begin(laneSegment.predecessors),
362 : 10 : std::end(laneSegment.predecessors),
363 : 30 : [&predecessorSegment](lane::LaneId const &predecessorId) {
364 : 10 : auto findResult = std::find_if(std::begin(predecessorSegment.drivableLaneSegments),
365 : 10 : std::end(predecessorSegment.drivableLaneSegments),
366 : 10 : [&predecessorId](LaneSegment const &segment) {
367 : 10 : return segment.laneInterval.laneId == predecessorId;
368 [ + - ]: 10 : });
369 : 10 : return findResult == std::end(predecessorSegment.drivableLaneSegments);
370 [ + - ]: 10 : }),
371 [ + - ]: 20 : laneSegment.predecessors.end());
372 : : }
373 : 10 : }
374 : :
375 : 4 : void updateLaneConnections(FullRoute &fullRoute)
376 : : {
377 [ - + ]: 4 : if (fullRoute.roadSegments.empty())
378 : : {
379 : 0 : return;
380 : : }
381 : :
382 [ + - ]: 4 : clearLaneSegmentPredecessors(fullRoute.roadSegments.front());
383 [ + - ]: 4 : clearLaneSegmentSuccessors(fullRoute.roadSegments.back());
384 [ + - ]: 4 : updateLaneSegmentNeighbors(fullRoute.roadSegments.front());
385 : :
386 : 4 : auto previousSegment = fullRoute.roadSegments.begin();
387 : 4 : auto nextSegment = previousSegment;
388 : 4 : nextSegment++;
389 : :
390 [ + + ]: 14 : while (nextSegment != std::end(fullRoute.roadSegments))
391 : : {
392 [ + - ]: 10 : updateLaneSegmentSuccessors(*previousSegment, *nextSegment);
393 [ + - ]: 10 : updateLaneSegmentPredecessors(*nextSegment, *previousSegment);
394 [ + - ]: 10 : updateLaneSegmentNeighbors(*nextSegment);
395 : :
396 : 10 : previousSegment = nextSegment;
397 : 10 : nextSegment++;
398 : : }
399 : : }
400 : :
401 : 18 : physics::Distance calcLength(FindWaypointResult const &findWaypointResult)
402 : : {
403 : 18 : physics::Distance result(0.);
404 [ + - ]: 18 : if (findWaypointResult.isValid())
405 : : {
406 : 18 : auto roadSegmentIter = findWaypointResult.queryRoute.roadSegments.begin();
407 : : // all segments before
408 : 31 : for (; roadSegmentIter != findWaypointResult.queryRoute.roadSegments.end()
409 [ + - + + : 31 : && roadSegmentIter != findWaypointResult.roadSegmentIterator;
+ + ]
410 : 13 : ++roadSegmentIter)
411 : : {
412 [ + - + - ]: 13 : result += calcLength(*roadSegmentIter);
413 : : }
414 : :
415 : : // the result segment interval
416 [ + - ]: 18 : if (roadSegmentIter == findWaypointResult.roadSegmentIterator)
417 : : {
418 : : // ensure that the interval is actually within the range
419 : 18 : auto laneSegmentIter = roadSegmentIter->drivableLaneSegments.begin();
420 : 25 : while (laneSegmentIter != roadSegmentIter->drivableLaneSegments.end()
421 [ + - + + : 25 : && laneSegmentIter != findWaypointResult.laneSegmentIterator)
+ + ]
422 : : {
423 : 7 : ++laneSegmentIter;
424 : : }
425 : :
426 [ + - ]: 18 : if (laneSegmentIter == findWaypointResult.laneSegmentIterator)
427 : : {
428 : 18 : LaneInterval calcInterval = laneSegmentIter->laneInterval;
429 : 18 : calcInterval.end = findWaypointResult.queryPosition.parametricOffset;
430 [ + - + - ]: 18 : result += calcLength(calcInterval);
431 : : }
432 : : else
433 : : {
434 : : throw(std::runtime_error(
435 [ # # ]: 0 : "::ad::map::route::calcLength(FindWaypointResult) intervalIter of the result is not valid"));
436 : : }
437 : : }
438 : : else
439 : : {
440 : : throw(std::runtime_error(
441 [ # # ]: 0 : "::ad::map::route::calcLength(FindWaypointResult) roadSegmentIterator of the result is not valid"));
442 : : }
443 : : }
444 : 18 : return result;
445 : : }
446 : :
447 : 127 : FindWaypointResult::FindWaypointResult(FullRoute const &route)
448 : : : queryRoute(route)
449 : 127 : , roadSegmentIterator(queryRoute.roadSegments.end())
450 : : {
451 : 127 : }
452 : :
453 : 103 : FindWaypointResult &FindWaypointResult::operator=(FindWaypointResult const &other)
454 : : {
455 [ - + ]: 103 : if ((&this->queryRoute) != (&other.queryRoute))
456 : : {
457 [ # # ]: 0 : throw std::invalid_argument("FindWaypointResult::operator= incompatible input parameter");
458 : : }
459 [ + - ]: 103 : if (this != &other)
460 : : {
461 : 103 : this->queryPosition = other.queryPosition;
462 : 103 : this->roadSegmentIterator = other.roadSegmentIterator;
463 : 103 : this->laneSegmentIterator = other.laneSegmentIterator;
464 : : }
465 : 103 : return *this;
466 : : }
467 : :
468 : 7 : FindWaypointResult FindWaypointResult::getLeftLane() const
469 : : {
470 : 7 : FindWaypointResult result(queryRoute);
471 [ + - + + : 7 : if (!isValid() || (!lane::isValid(laneSegmentIterator->leftNeighbor, false)))
+ + ]
472 : : {
473 : 5 : return result;
474 : : }
475 : :
476 : 2 : result = *this;
477 : 2 : result.laneSegmentIterator++;
478 [ + - - + : 2 : if (!result.isValid() || (result.laneSegmentIterator->laneInterval.laneId != laneSegmentIterator->leftNeighbor))
- + ]
479 : : {
480 : : throw std::runtime_error("ad::map::route::FindWaypointResult::getLeftLane()>> Route inconsistent: "
481 [ # # ]: 0 : "left lane not found");
482 : : }
483 : 2 : result.queryPosition.laneId = result.laneSegmentIterator->laneInterval.laneId;
484 : 2 : return result;
485 : : }
486 : :
487 : 6 : FindWaypointResult FindWaypointResult::getRightLane() const
488 : : {
489 : 6 : FindWaypointResult result(queryRoute);
490 [ + - + + : 6 : if (!isValid() || (!lane::isValid(laneSegmentIterator->rightNeighbor, false)))
+ + ]
491 : : {
492 : 4 : return result;
493 : : }
494 : :
495 : 2 : result = *this;
496 : 2 : result.laneSegmentIterator--;
497 [ + - - + : 2 : if (!result.isValid() || (result.laneSegmentIterator->laneInterval.laneId != laneSegmentIterator->rightNeighbor))
- + ]
498 : : {
499 : : throw std::runtime_error("ad::map::route::FindWaypointResult::getRightLane()>> Route inconsistent: "
500 [ # # ]: 0 : "right lane not found");
501 : : }
502 : 2 : result.queryPosition.laneId = result.laneSegmentIterator->laneInterval.laneId;
503 : 2 : return result;
504 : : }
505 : :
506 : : // supporting function used commonly by getSuccessorLanes() and getPredecessorLanes()
507 : 36 : std::vector<FindWaypointResult> getLanesOfCurrentRoadSegment(FindWaypointResult &result,
508 : : lane::LaneIdList const &expectedLanesVector)
509 : : {
510 : 36 : std::vector<FindWaypointResult> resultList;
511 : :
512 : 72 : std::set<lane::LaneId> expectedLanes;
513 [ + - ]: 36 : expectedLanes.insert(expectedLanesVector.begin(), expectedLanesVector.end());
514 : 78 : for (result.laneSegmentIterator = result.roadSegmentIterator->drivableLaneSegments.begin();
515 [ + + ]: 78 : result.laneSegmentIterator != result.roadSegmentIterator->drivableLaneSegments.end();
516 : 42 : result.laneSegmentIterator++)
517 : : {
518 [ + - ]: 42 : auto const findResult = expectedLanes.find(result.laneSegmentIterator->laneInterval.laneId);
519 [ + + ]: 42 : if (findResult != expectedLanes.end())
520 : : {
521 : : // found a predecessor
522 [ - + ]: 36 : if (!result.isValid())
523 : : {
524 : : throw std::runtime_error(
525 [ # # ]: 0 : "ad::map::route::FindWaypointResult::getLanesOfCurrentRoadSegment()>> unexpected error");
526 : : }
527 : 36 : result.queryPosition.laneId = result.laneSegmentIterator->laneInterval.laneId;
528 [ + - ]: 36 : resultList.push_back(result);
529 [ + - ]: 36 : expectedLanes.erase(findResult);
530 : : }
531 : : }
532 : :
533 [ - + ]: 36 : if (!expectedLanes.empty())
534 : : {
535 : : throw std::runtime_error("ad::map::route::FindWaypointResult::getLanesOfCurrentRoadSegment()>> Route inconsistent: "
536 [ # # ]: 0 : "not all expected lanes found within current road segment");
537 : : }
538 : 72 : return resultList;
539 : : }
540 : :
541 : 35 : std::vector<FindWaypointResult> FindWaypointResult::getSuccessorLanes() const
542 : : {
543 : 70 : std::vector<FindWaypointResult> resultList;
544 [ + - + + : 35 : if (!isValid() || (laneSegmentIterator->successors.size() == 0u))
+ + ]
545 : : {
546 : 6 : return resultList;
547 : : }
548 : :
549 [ + - ]: 29 : FindWaypointResult result(queryRoute);
550 [ + - ]: 29 : result = *this;
551 : 29 : result.roadSegmentIterator++;
552 [ - + ]: 29 : if (result.roadSegmentIterator == result.queryRoute.roadSegments.end())
553 : : {
554 : : throw std::runtime_error("ad::map::route::FindWaypointResult::getSuccessorLanes()>> Route inconsistent: "
555 [ # # ]: 0 : "next road segment not found");
556 : : }
557 [ + - ]: 29 : return getLanesOfCurrentRoadSegment(result, laneSegmentIterator->successors);
558 : : }
559 : :
560 : 14 : std::vector<FindWaypointResult> FindWaypointResult::getPredecessorLanes() const
561 : : {
562 [ + - + + : 14 : if (!isValid() || (laneSegmentIterator->predecessors.size() == 0u))
+ + ]
563 : : {
564 : 7 : return std::vector<FindWaypointResult>();
565 : : }
566 : :
567 [ + - ]: 7 : FindWaypointResult result(queryRoute);
568 [ + - ]: 7 : result = *this;
569 [ - + ]: 7 : if (result.roadSegmentIterator == result.queryRoute.roadSegments.begin())
570 : : {
571 : : throw std::runtime_error("ad::map::route::FindWaypointResult::getPredecessorLanes()>> Route inconsistent: "
572 [ # # ]: 0 : "previous road segment not found");
573 : : }
574 : 7 : result.roadSegmentIterator--;
575 [ + - ]: 7 : return getLanesOfCurrentRoadSegment(result, laneSegmentIterator->predecessors);
576 : : }
577 : :
578 : : FindWaypointResult
579 : 89 : findWaypointImpl(point::ParaPoint const &position, route::FullRoute const &route, bool considerRouteStart)
580 : : {
581 : 218 : for (route::RoadSegmentList::const_iterator roadSegmentIter = route.roadSegments.begin();
582 [ + + ]: 218 : roadSegmentIter != route.roadSegments.end();
583 : 129 : ++roadSegmentIter)
584 : : {
585 : 397 : for (route::LaneSegmentList::const_iterator laneSegmentIter = roadSegmentIter->drivableLaneSegments.begin();
586 [ + + ]: 397 : laneSegmentIter != roadSegmentIter->drivableLaneSegments.end();
587 : 195 : ++laneSegmentIter)
588 : : {
589 [ + - + + ]: 268 : if (position.laneId == laneSegmentIter->laneInterval.laneId)
590 : : {
591 [ + + ]: 78 : if (!considerRouteStart)
592 : : {
593 [ + - ]: 7 : point::ParaPoint startPoint;
594 : 7 : startPoint.laneId = position.laneId;
595 [ + - + + ]: 7 : if (isRouteDirectionPositive(laneSegmentIter->laneInterval))
596 : : {
597 : 4 : startPoint.parametricOffset = laneSegmentIter->laneInterval.start;
598 : : }
599 : : else
600 : : {
601 : 3 : startPoint.parametricOffset = laneSegmentIter->laneInterval.end;
602 : : }
603 : 7 : return FindWaypointResult(route, startPoint, roadSegmentIter, laneSegmentIter);
604 : : }
605 : : else
606 : : {
607 : : // due to numeric inaccuracies we need to check not just the internal itself
608 : : // but also some surroundings
609 : : // @todo: Ideally this has to be covered by the laneInterval operations
610 : : // For now, just extend the Interval by 0.5 meter on each end
611 : 71 : LaneInterval laneInterval = laneSegmentIter->laneInterval;
612 [ + - ]: 71 : laneInterval = extendIntervalFromStart(laneInterval, physics::Distance(0.5));
613 [ + - ]: 71 : laneInterval = extendIntervalFromEnd(laneInterval, physics::Distance(0.5));
614 : :
615 [ + - + + ]: 71 : if (isWithinInterval(laneInterval, position))
616 : : {
617 : 66 : return FindWaypointResult(route, position, roadSegmentIter, laneSegmentIter);
618 : : }
619 : : }
620 : : }
621 : : }
622 : : }
623 : 16 : return FindWaypointResult(route);
624 : : }
625 : :
626 : 75 : FindWaypointResult findWaypoint(point::ParaPoint const &position, route::FullRoute const &route)
627 : : {
628 : 75 : return findWaypointImpl(position, route, true);
629 : : }
630 : :
631 : 14 : FindWaypointResult findWaypoint(lane::LaneId const &laneId, route::FullRoute const &route)
632 : : {
633 [ + - ]: 14 : point::ParaPoint findPoint;
634 : 14 : findPoint.laneId = laneId;
635 : 14 : findPoint.parametricOffset = physics::ParametricValue(0.5);
636 [ + - ]: 28 : return findWaypointImpl(findPoint, route, false);
637 : : }
638 : :
639 : 43 : FindWaypointResult findNearestWaypoint(point::ParaPointList const &positions, route::FullRoute const &route)
640 : : {
641 : 43 : FindWaypointResult resultWaypoint(route);
642 : :
643 [ + + ]: 90 : for (auto const &position : positions)
644 : : {
645 [ + - ]: 47 : auto findResult = findWaypoint(position, route);
646 [ + + ]: 47 : if (findResult.isValid())
647 : : {
648 : 41 : if ( // no other result yet
649 : 41 : (!resultWaypoint.isValid())
650 : : // new result is nearer already on route segment level
651 [ + + - + : 41 : || (findResult.roadSegmentIterator < resultWaypoint.roadSegmentIterator))
+ + ]
652 : : {
653 [ + - ]: 36 : resultWaypoint = findResult;
654 : : }
655 [ + + ]: 5 : else if (findResult.roadSegmentIterator == resultWaypoint.roadSegmentIterator)
656 : : {
657 : : // new result is on the same segment, so let the parametricOffset value of the query decide
658 [ + - + + ]: 4 : if (isRouteDirectionPositive(resultWaypoint.laneSegmentIterator->laneInterval))
659 : : {
660 [ + - + + ]: 3 : if (findResult.queryPosition.parametricOffset < resultWaypoint.queryPosition.parametricOffset)
661 : : {
662 [ + - ]: 1 : resultWaypoint = findResult;
663 : : }
664 : : }
665 : : else
666 : : {
667 [ + - + - ]: 1 : if (findResult.queryPosition.parametricOffset > resultWaypoint.queryPosition.parametricOffset)
668 : : {
669 [ + - ]: 1 : resultWaypoint = findResult;
670 : : }
671 : : }
672 : : }
673 : : }
674 : : }
675 : :
676 : 43 : return resultWaypoint;
677 : : }
678 : :
679 : 1 : FindWaypointResult findNearestWaypoint(match::MapMatchedPositionConfidenceList const &mapMatchedPositions,
680 : : route::FullRoute const &route)
681 : : {
682 [ + - ]: 2 : return findNearestWaypoint(match::getParaPoints(mapMatchedPositions), route);
683 : : }
684 : :
685 : 0 : FindWaypointResult findCenterWaypoint(match::Object const &object, route::FullRoute const &route)
686 : : {
687 : 0 : FindWaypointResult resultWaypoint(route);
688 : : // first try to search the center point as main reference of the object
689 : 0 : if (object.mapMatchedBoundingBox.referencePointPositions.size()
690 [ # # ]: 0 : > static_cast<std::size_t>(match::ObjectReferencePoints::Center))
691 : : {
692 : : resultWaypoint
693 : 0 : = findNearestWaypoint(object.mapMatchedBoundingBox
694 : 0 : .referencePointPositions[static_cast<std::size_t>(match::ObjectReferencePoints::Center)],
695 [ # # ]: 0 : route);
696 : : }
697 [ # # ]: 0 : if (!resultWaypoint.isValid())
698 : : {
699 : : // sort the occupied region center points by the length of the region
700 : : // to start the search with the largest longitudinal region (which is most likely the main lane)
701 : 0 : std::multimap<physics::Distance, point::ParaPoint, std::greater<physics::Distance>> occupiedRegionCenters;
702 [ # # ]: 0 : for (auto const &occupiedRegion : object.mapMatchedBoundingBox.laneOccupiedRegions)
703 : : {
704 [ # # ]: 0 : auto const occupiedRegionLength = lane::calcLength(occupiedRegion);
705 [ # # ]: 0 : auto const occupiedRegionCenter = match::getCenterParaPoint(occupiedRegion);
706 [ # # ]: 0 : occupiedRegionCenters.insert({occupiedRegionLength, occupiedRegionCenter});
707 : : }
708 [ # # ]: 0 : for (auto const &occupiedRegionCenter : occupiedRegionCenters)
709 : : {
710 [ # # # # ]: 0 : resultWaypoint = findWaypoint(occupiedRegionCenter.second, route);
711 [ # # ]: 0 : if (resultWaypoint.isValid())
712 : : {
713 : 0 : break;
714 : : }
715 : : }
716 : : }
717 [ # # ]: 0 : if (!resultWaypoint.isValid())
718 : : {
719 : : // still nothing found, so we take the nearest of the rest of the reference point positions
720 : 0 : point::ParaPointList mapMatchedParaPoints;
721 [ # # ]: 0 : for (auto i = 0u; i < object.mapMatchedBoundingBox.referencePointPositions.size(); ++i)
722 : : {
723 [ # # ]: 0 : if (i != static_cast<std::size_t>(match::ObjectReferencePoints::Center))
724 : : {
725 [ # # ]: 0 : auto paraPoints = match::getParaPoints(object.mapMatchedBoundingBox.referencePointPositions[i]);
726 [ # # ]: 0 : mapMatchedParaPoints.insert(mapMatchedParaPoints.end(), paraPoints.begin(), paraPoints.end());
727 : : }
728 : : }
729 [ # # # # ]: 0 : resultWaypoint = findNearestWaypoint(mapMatchedParaPoints, route);
730 : : }
731 [ # # ]: 0 : if (!resultWaypoint.isValid())
732 : : {
733 : : // still nothing found, so search if any point of the route is within our occupied regions
734 [ # # ]: 0 : for (auto const &occupiedRegion : object.mapMatchedBoundingBox.laneOccupiedRegions)
735 : : {
736 [ # # ]: 0 : auto laneWaypoint = findWaypoint(occupiedRegion.laneId, route);
737 [ # # ]: 0 : if (laneWaypoint.isValid())
738 : : {
739 [ # # ]: 0 : auto const routeParmetricLaneRange = route::toParametricRange(laneWaypoint.laneSegmentIterator->laneInterval);
740 : : auto const intersectingRange
741 [ # # ]: 0 : = physics::getIntersectionRange(routeParmetricLaneRange, occupiedRegion.longitudinalRange);
742 [ # # # # ]: 0 : if (physics::isRangeValid(intersectingRange))
743 : : {
744 : 0 : laneWaypoint.queryPosition.parametricOffset = intersectingRange.minimum;
745 [ # # ]: 0 : resultWaypoint = laneWaypoint;
746 : 0 : break;
747 : : }
748 : : }
749 : : }
750 : : }
751 : 0 : return resultWaypoint;
752 : : }
753 : :
754 : 2 : FindWaypointResult objectOnRoute(match::MapMatchedObjectBoundingBox const &object, route::FullRoute const &route)
755 : : {
756 : 4 : point::ParaPointList positions;
757 [ + + ]: 3 : for (auto const &occupiedRegion : object.laneOccupiedRegions)
758 : : {
759 [ + - ]: 1 : point::ParaPoint point;
760 : 1 : point.laneId = occupiedRegion.laneId;
761 : 1 : point.parametricOffset = occupiedRegion.longitudinalRange.minimum;
762 [ + - ]: 1 : positions.push_back(point);
763 : 1 : point.parametricOffset = occupiedRegion.longitudinalRange.maximum;
764 [ + - ]: 1 : positions.push_back(point);
765 : : }
766 : :
767 [ + - ]: 4 : return findNearestWaypoint(positions, route);
768 : : }
769 : :
770 : 2 : FindWaypointResult intersectionOnRoute(intersection::Intersection const &intersection, route::FullRoute const &route)
771 : : {
772 [ + - ]: 2 : FindWaypointResult result(route);
773 : :
774 [ - + ]: 2 : if (route.roadSegments.empty())
775 : : {
776 : 0 : return result;
777 : : }
778 : :
779 : : /**
780 : : * Check if we are already inside the intersection
781 : : */
782 : 2 : auto findResult = std::find_if(std::begin(route.roadSegments.front().drivableLaneSegments),
783 : 4 : std::end(route.roadSegments.front().drivableLaneSegments),
784 : 4 : [&intersection](LaneSegment const &laneSegment) {
785 [ + - + - ]: 2 : return intersection.internalLanes().find(laneSegment.laneInterval.laneId)
786 : 4 : != intersection.internalLanes().end();
787 [ + - ]: 4 : });
788 [ + + ]: 2 : if (findResult != std::end(route.roadSegments.front().drivableLaneSegments))
789 : : {
790 : 1 : result.laneSegmentIterator = findResult;
791 : 1 : result.roadSegmentIterator = route.roadSegments.begin();
792 : 1 : result.queryPosition.laneId = result.laneSegmentIterator->laneInterval.laneId;
793 : 1 : result.queryPosition.parametricOffset = result.laneSegmentIterator->laneInterval.start;
794 : : }
795 : : else
796 : : {
797 [ + - + - : 1 : result = findNearestWaypoint(intersection.incomingParaPointsOnRoute(), route);
+ - ]
798 [ - + ]: 1 : if (!result.isValid())
799 : : {
800 [ # # # # : 0 : result = findNearestWaypoint(intersection.incomingParaPoints(), route);
# # ]
801 : : }
802 : : }
803 : :
804 : 2 : return result;
805 : : }
806 : :
807 : 0 : bool isConnectedRoutePartOfAnIntersection(route::ConnectingRoute const &connectingRoute)
808 : : {
809 : 0 : return intersection::Intersection::isRoutePartOfAnIntersection(connectingRoute.routeA)
810 [ # # # # ]: 0 : || intersection::Intersection::isRoutePartOfAnIntersection(connectingRoute.routeB);
811 : : }
812 : :
813 : 2887 : void alignRouteStartingPoints(point::ParaPoint const &startAlignmentParaPoint, route::FullRoute &route)
814 : : {
815 [ - + ]: 2887 : if (route.roadSegments.empty())
816 : : {
817 : 0 : return;
818 : : }
819 : :
820 : : // we need to ensure, that the starting point of all segments are aligned,
821 : : // i.e. the starting points are located on an imaginary line on the curve radius
822 [ + - + - ]: 5774 : auto const startAlignmentLane = lane::getLane(startAlignmentParaPoint.laneId);
823 : : auto const startAlignmentPoint = lane::getProjectedParametricPoint(
824 [ + - ]: 2887 : startAlignmentLane, startAlignmentParaPoint.parametricOffset, physics::ParametricValue(0.5));
825 : :
826 : 6594 : for (route::LaneSegmentList::iterator laneSegmentIter = route.roadSegments.front().drivableLaneSegments.begin();
827 [ + + ]: 6594 : laneSegmentIter != route.roadSegments.front().drivableLaneSegments.end();
828 : 3707 : ++laneSegmentIter)
829 : : {
830 : : // don't touch alignment interval
831 : : // don't touch degenerated intervals
832 [ + - ]: 3707 : if ((laneSegmentIter->laneInterval.laneId != startAlignmentParaPoint.laneId)
833 [ + + + - : 3707 : && (!route::isDegenerated(laneSegmentIter->laneInterval)))
+ + + + ]
834 : : {
835 [ + - + - ]: 1638 : auto const lane = lane::getLane(laneSegmentIter->laneInterval.laneId);
836 [ + - ]: 819 : auto const rightT = point::findNearestPointOnEdge(lane.edgeRight, startAlignmentPoint);
837 [ + - ]: 819 : auto const leftT = point::findNearestPointOnEdge(lane.edgeLeft, startAlignmentPoint);
838 [ + - + - ]: 819 : auto const newStart = 0.5 * (rightT + leftT);
839 : : // Be aware: only update the start if the values actually differ considering the ParametricValue::getPrecision()
840 : : // to prevent from 0. becoming 1e-10
841 [ + - + + ]: 819 : if (newStart != laneSegmentIter->laneInterval.start)
842 : : {
843 : : // and ensure not to revert the interval direction by pushing the start after the interval end
844 [ + - + - ]: 818 : if (!route::isAfterInterval(laneSegmentIter->laneInterval, newStart))
845 : : {
846 : 818 : laneSegmentIter->laneInterval.start = newStart;
847 : : }
848 : : }
849 : : }
850 : : }
851 : : }
852 : :
853 : 2865 : void alignRouteEndingPoints(point::ParaPoint const &alignmentParaPoint, route::FullRoute &route)
854 : : {
855 [ - + ]: 2865 : if (route.roadSegments.empty())
856 : : {
857 : 0 : return;
858 : : }
859 : :
860 : : // we need to ensure, that the end point of all segments are aligned,
861 : : // i.e. the end points are located on an imaginary line on the curve radius
862 [ + - + - ]: 5730 : auto const alignmentLane = lane::getLane(alignmentParaPoint.laneId);
863 : : auto const alignmentPoint = lane::getProjectedParametricPoint(
864 [ + - ]: 2865 : alignmentLane, alignmentParaPoint.parametricOffset, physics::ParametricValue(0.5));
865 : :
866 : 6291 : for (route::LaneSegmentList::iterator laneSegmentIter = route.roadSegments.back().drivableLaneSegments.begin();
867 [ + + ]: 6291 : laneSegmentIter != route.roadSegments.back().drivableLaneSegments.end();
868 : 3426 : ++laneSegmentIter)
869 : : {
870 : : // don't touch alignment interval
871 : : // don't touch degenerated intervals
872 [ + - ]: 3426 : if ((laneSegmentIter->laneInterval.laneId != alignmentParaPoint.laneId)
873 [ + + + - : 3426 : && (!route::isDegenerated(laneSegmentIter->laneInterval)))
+ + + + ]
874 : : {
875 [ + - + - ]: 1018 : auto const lane = lane::getLane(laneSegmentIter->laneInterval.laneId);
876 [ + - ]: 509 : auto const rightT = point::findNearestPointOnEdge(lane.edgeRight, alignmentPoint);
877 [ + - ]: 509 : auto const leftT = point::findNearestPointOnEdge(lane.edgeLeft, alignmentPoint);
878 [ + - + - ]: 509 : auto const newEnd = 0.5 * (rightT + leftT);
879 : : // Be aware: only update the end if the values actually differ considering the ParametricValue::getPrecision()
880 : : // to prevent from 1. becoming 0.9999999999
881 [ + - + + ]: 509 : if (newEnd != laneSegmentIter->laneInterval.end)
882 : : {
883 : : // and ensure not to revert the interval direction by pushing the end before the interval start
884 [ + - + - ]: 454 : if (!route::isBeforeInterval(laneSegmentIter->laneInterval, newEnd))
885 : : {
886 : 454 : laneSegmentIter->laneInterval.end = newEnd;
887 : : }
888 : : }
889 : : }
890 : : }
891 : : }
892 : :
893 : 36 : ShortenRouteResult shortenRoute(point::ParaPointList const ¤tPositions,
894 : : route::FullRoute &route,
895 : : ShortenRouteMode const shortenRouteMode)
896 : : {
897 [ + + ]: 36 : if (route.roadSegments.empty())
898 : : {
899 : 1 : return ShortenRouteResult::FailedRouteEmpty;
900 : : }
901 : :
902 [ + - ]: 35 : auto findWaypointResult = findNearestWaypoint(currentPositions, route);
903 : :
904 [ + + ]: 35 : if (findWaypointResult.isValid())
905 : : {
906 : 29 : ShortenRouteResult result = ShortenRouteResult::Succeeded;
907 : 29 : if ((shortenRouteMode == ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute)
908 [ + + + - : 35 : && (intersection::Intersection::isLanePartOfAnIntersection(
+ + + + ]
909 : 6 : findWaypointResult.laneSegmentIterator->laneInterval.laneId)))
910 : : {
911 : : // check for intersection
912 : : // check if we are right before the route ---> do not clear in this case
913 [ + - ]: 10 : for (auto roadSegmentIter = route.roadSegments.begin(); roadSegmentIter != route.roadSegments.end();
914 : 6 : roadSegmentIter++)
915 : : {
916 : 10 : RouteIterator routeIterator(route, roadSegmentIter);
917 : 10 : route::RoadSegmentList::const_iterator routePreviousSegmentIter;
918 : : auto intersectionStarts
919 [ + - ]: 10 : = intersection::Intersection::isRoadSegmentEnteringIntersection(routeIterator, routePreviousSegmentIter);
920 [ + + ]: 10 : if (intersectionStarts)
921 : : {
922 : : // erase leading route in front of the intersection
923 [ + - ]: 5 : route.roadSegments.erase(route.roadSegments.begin(), routePreviousSegmentIter);
924 : :
925 : : // and shorten the previous segments lane intervals
926 : 5 : for (route::LaneSegmentList::iterator laneSegmentIter
927 : 5 : = route.roadSegments.front().drivableLaneSegments.begin();
928 [ + + ]: 10 : laneSegmentIter != route.roadSegments.front().drivableLaneSegments.end();
929 : 5 : ++laneSegmentIter)
930 : : {
931 : 5 : laneSegmentIter->laneInterval.start = laneSegmentIter->laneInterval.end;
932 : : }
933 : :
934 : : // continue loop to search for further intersections
935 : 5 : result = ShortenRouteResult::SucceededIntersectionNotCut;
936 : : }
937 : :
938 [ + + ]: 10 : if (roadSegmentIter == findWaypointResult.roadSegmentIterator)
939 : : {
940 : 4 : break;
941 : : }
942 : : }
943 : : }
944 : :
945 [ + + ]: 29 : if (result != ShortenRouteResult::SucceededIntersectionNotCut)
946 : : {
947 : : // erase leading route
948 [ + - ]: 25 : route.roadSegments.erase(route.roadSegments.begin(), findWaypointResult.roadSegmentIterator);
949 : :
950 : : // in addition, we have to shorten the parametric offsets
951 : 51 : for (route::LaneSegmentList::iterator laneSegmentIter = route.roadSegments.front().drivableLaneSegments.begin();
952 [ + + ]: 51 : laneSegmentIter != route.roadSegments.front().drivableLaneSegments.end();
953 : 26 : ++laneSegmentIter)
954 : : {
955 [ + - + + ]: 26 : if (isWithinInterval(laneSegmentIter->laneInterval, findWaypointResult.queryPosition.parametricOffset))
956 : : {
957 : 24 : laneSegmentIter->laneInterval.start = findWaypointResult.queryPosition.parametricOffset;
958 : : }
959 [ + - ]: 2 : else if (!isDegenerated(laneSegmentIter->laneInterval)
960 [ + - + - : 2 : && isAfterInterval(laneSegmentIter->laneInterval, findWaypointResult.queryPosition.parametricOffset))
+ + + + ]
961 : : {
962 : 1 : laneSegmentIter->laneInterval.start = laneSegmentIter->laneInterval.end;
963 : : }
964 : : }
965 : :
966 [ + - + + ]: 25 : if (route::isDegenerated(route.roadSegments.front().drivableLaneSegments.front().laneInterval))
967 : : {
968 : 5 : bool removeDegeneratedRouteStart = true;
969 [ + + ]: 5 : if (shortenRouteMode == ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute)
970 : : {
971 [ + - ]: 1 : if (route.roadSegments.size() > 1u)
972 : : {
973 : 1 : RouteIterator routeIterator(route, route.roadSegments.begin() + 1);
974 : 1 : route::RoadSegmentList::const_iterator routePreviousSegmentIter;
975 : : auto intersectionStarts
976 [ + - ]: 1 : = intersection::Intersection::isRoadSegmentEnteringIntersection(routeIterator, routePreviousSegmentIter);
977 : 1 : removeDegeneratedRouteStart = !intersectionStarts;
978 : : }
979 : : }
980 : :
981 [ + + ]: 5 : if (removeDegeneratedRouteStart)
982 : : {
983 : : // remove also degenerated route start segments
984 [ + - ]: 4 : route.roadSegments.erase(route.roadSegments.begin());
985 : : }
986 : : else
987 : : {
988 : 1 : result = ShortenRouteResult::SucceededIntersectionNotCut;
989 : : }
990 : : }
991 : : else
992 : : {
993 [ + - ]: 20 : alignRouteStartingPoints(findWaypointResult.queryPosition, route);
994 : : }
995 : : }
996 : :
997 [ + + ]: 29 : if (route.roadSegments.empty())
998 : : {
999 : 2 : return ShortenRouteResult::SucceededRouteEmpty;
1000 : : }
1001 : : else
1002 : : {
1003 : : // remove predecessors
1004 [ + - ]: 27 : clearLaneSegmentPredecessors(route.roadSegments.front());
1005 : 27 : return result;
1006 : : }
1007 : : }
1008 : :
1009 : : // check if we are right before the route ---> do not clear in this case
1010 [ + + ]: 7 : for (auto &laneSegment : route.roadSegments.front().drivableLaneSegments)
1011 : : {
1012 [ + + ]: 7 : for (auto const ¤tPosition : currentPositions)
1013 : : {
1014 [ + - + + ]: 6 : if (isBeforeInterval(laneSegment.laneInterval, currentPosition))
1015 : : {
1016 [ + + ]: 5 : if (shortenRouteMode == ShortenRouteMode::Normal)
1017 : : {
1018 : 5 : return ShortenRouteResult::SucceededBeforeRoute;
1019 : : }
1020 : : else
1021 : : {
1022 : 1 : laneSegment.laneInterval.start = currentPosition.parametricOffset;
1023 [ + - ]: 1 : alignRouteStartingPoints(currentPosition, route);
1024 : 1 : return ShortenRouteResult::Succeeded;
1025 : : }
1026 : : }
1027 : : }
1028 : : }
1029 : :
1030 : : // check if we are right after the route ---> clear in this case, but still return true
1031 [ + + ]: 2 : for (auto const &laneSegment : route.roadSegments.back().drivableLaneSegments)
1032 : : {
1033 [ + + ]: 2 : for (auto const ¤tPosition : currentPositions)
1034 : : {
1035 [ + - + - : 1 : if (!isDegenerated(laneSegment.laneInterval) && isAfterInterval(laneSegment.laneInterval, currentPosition))
+ - - + -
+ ]
1036 : : {
1037 : 0 : route.roadSegments.clear();
1038 : 0 : return ShortenRouteResult::SucceededRouteEmpty;
1039 : : }
1040 : : }
1041 : : }
1042 : :
1043 : : // we are neither right before nor right after nor on the route --> clear
1044 : 1 : route.roadSegments.clear();
1045 : 1 : return ShortenRouteResult::FailedRouteEmpty;
1046 : : }
1047 : :
1048 : : ShortenRouteResult
1049 : 34 : shortenRoute(point::ParaPoint const ¤tPosition, route::FullRoute &route, ShortenRouteMode const shortenRouteMode)
1050 : : {
1051 [ + - ]: 68 : point::ParaPointList currentPositions = {currentPosition};
1052 [ + - ]: 68 : return shortenRoute(currentPositions, route, shortenRouteMode);
1053 : : }
1054 : :
1055 : 0 : ShortenRouteResult shortenRoute(match::MapMatchedPositionConfidenceList const &mapMatchedPositions,
1056 : : route::FullRoute &route,
1057 : : ShortenRouteMode const shortenRouteMode)
1058 : : {
1059 [ # # ]: 0 : return shortenRoute(match::getParaPoints(mapMatchedPositions), route, shortenRouteMode);
1060 : : }
1061 : :
1062 : 1 : void shortenRouteToDistance(route::FullRoute &route, const physics::Distance &length)
1063 : : {
1064 : 1 : auto roadSegmentIterator = route.roadSegments.begin();
1065 : 1 : auto remainingLength = length;
1066 [ + + + - : 3 : while ((roadSegmentIterator != route.roadSegments.end()) && (remainingLength > physics::Distance(0)))
+ - + + ]
1067 : : {
1068 [ + - ]: 2 : auto const segmentLength = calcLength(*roadSegmentIterator);
1069 [ + - + + ]: 2 : if (segmentLength <= remainingLength)
1070 : : {
1071 [ + - ]: 1 : remainingLength -= segmentLength;
1072 : 1 : roadSegmentIterator++;
1073 : : }
1074 : : else
1075 : : {
1076 : : // this is the last remaining segment; it has to be shortened
1077 [ + - - + ]: 1 : if (intersection::Intersection::isLanePartOfAnIntersection(
1078 : 1 : roadSegmentIterator->drivableLaneSegments.front().laneInterval.laneId))
1079 : : {
1080 : : // don't cut in between of intersections, rather keep the whole intersection within the route
1081 [ # # ]: 0 : for (roadSegmentIterator++; (roadSegmentIterator != route.roadSegments.end()); roadSegmentIterator++)
1082 : : {
1083 [ # # # # ]: 0 : if (!intersection::Intersection::isLanePartOfAnIntersection(
1084 : 0 : roadSegmentIterator->drivableLaneSegments.front().laneInterval.laneId))
1085 : : {
1086 : : // outside the intersection, so we can cut the rest
1087 : 0 : break;
1088 : : }
1089 : : }
1090 : : }
1091 : : else
1092 : : {
1093 [ + - ]: 1 : auto const deltaLength = segmentLength - remainingLength;
1094 [ + - ]: 1 : shortenSegmentFromEnd(*roadSegmentIterator, deltaLength);
1095 : : // and push the iterator to the next, where we then can cut
1096 : 1 : roadSegmentIterator++;
1097 : : }
1098 : : // nothing remains, loop ends now
1099 : 1 : remainingLength = physics::Distance(0);
1100 : : }
1101 : : }
1102 [ + - ]: 1 : route.roadSegments.erase(roadSegmentIterator, route.roadSegments.end());
1103 [ + - ]: 1 : if (!route.roadSegments.empty())
1104 : : {
1105 : : // remove successors from last road segment
1106 [ + - ]: 1 : clearLaneSegmentSuccessors(route.roadSegments.back());
1107 : : }
1108 : 1 : }
1109 : :
1110 : 7 : void removeLastRoadSegment(route::FullRoute &route)
1111 : : {
1112 [ + - ]: 7 : if (!route.roadSegments.empty())
1113 : : {
1114 : 7 : route.roadSegments.pop_back();
1115 [ - + ]: 7 : if (!route.roadSegments.empty())
1116 : : {
1117 : : // remove successors from last road segment
1118 : 0 : clearLaneSegmentSuccessors(route.roadSegments.back());
1119 : : }
1120 : : }
1121 : 7 : }
1122 : :
1123 : 5 : void removeLastRoadSegmentIfDegenerated(route::FullRoute &route)
1124 : : {
1125 : : // first drop degenerated road segments at the end of route to ensure we get proper direction
1126 : 5 : if (!route.roadSegments.empty()
1127 [ + - + - : 10 : && (route.roadSegments.back().drivableLaneSegments.empty()
- + ]
1128 [ - + ]: 5 : || isDegenerated(route.roadSegments.back().drivableLaneSegments.front().laneInterval)))
1129 : : {
1130 : 0 : removeLastRoadSegment(route);
1131 : : }
1132 : 5 : }
1133 : :
1134 : 5 : bool prepareRouteForExtension(route::FullRoute &route,
1135 : : route::planning::RoutingParaPoint &routeExtensionStartPoint,
1136 : : route::LaneSegment &extensionReferenceLaneSegment)
1137 : : {
1138 : : // first drop degenerated road segments at the end of route to ensure we get proper direction
1139 : 5 : removeLastRoadSegmentIfDegenerated(route);
1140 : :
1141 : : // abort on degenerated route
1142 [ + - - + : 5 : if (route.roadSegments.empty() || route.roadSegments.back().drivableLaneSegments.empty())
- + ]
1143 : : {
1144 : 0 : return false;
1145 : : }
1146 : :
1147 : 5 : extensionReferenceLaneSegment = route.roadSegments.back().drivableLaneSegments.front();
1148 : 5 : routeExtensionStartPoint = route::planning::createRoutingPoint(
1149 : 5 : extensionReferenceLaneSegment.laneInterval.laneId,
1150 : 5 : extensionReferenceLaneSegment.laneInterval.start,
1151 [ + - ]: 5 : (isRouteDirectionPositive(extensionReferenceLaneSegment.laneInterval) ? planning::RoutingDirection::POSITIVE
1152 [ + - ]: 5 : : planning::RoutingDirection::NEGATIVE));
1153 : 5 : return true;
1154 : : }
1155 : :
1156 : 7 : route::FullRoute mergeRouteExtension(route::FullRoute const &route,
1157 : : route::LaneSegment const &extensionReferenceLaneSegment,
1158 : : route::FullRoute const &routeExtension)
1159 : : {
1160 : 7 : FullRoute newRoute(route);
1161 [ + - ]: 7 : removeLastRoadSegment(newRoute);
1162 [ + + ]: 36 : for (auto const &roadSegment : routeExtension.roadSegments)
1163 : : {
1164 [ + - ]: 87 : route::appendRoadSegmentToRoute(roadSegment.drivableLaneSegments.front().laneInterval,
1165 : 29 : extensionReferenceLaneSegment.routeLaneOffset
1166 : 58 : + roadSegment.drivableLaneSegments.front().routeLaneOffset,
1167 : : newRoute,
1168 : : {});
1169 : : }
1170 [ + - ]: 7 : planning::updateRoutePlanningCounters(newRoute);
1171 : 7 : return newRoute;
1172 : : }
1173 : :
1174 : 2 : template <typename T> bool extendRouteToDestinationsT(route::FullRoute &route, const std::vector<T> &dest)
1175 : : {
1176 [ + - ]: 2 : route::planning::RoutingParaPoint routeExtensionStartPoint;
1177 [ + - ]: 4 : route::LaneSegment extensionReferenceLaneSegment;
1178 [ + - - + ]: 2 : if (!prepareRouteForExtension(route, routeExtensionStartPoint, extensionReferenceLaneSegment))
1179 : : {
1180 : 0 : return false;
1181 : : }
1182 : :
1183 [ + - ]: 2 : auto routeExtension = route::planning::planRoute(routeExtensionStartPoint, dest, route.routeCreationMode);
1184 : :
1185 [ + - ]: 2 : route = mergeRouteExtension(route, extensionReferenceLaneSegment, routeExtension);
1186 : :
1187 : 2 : return true;
1188 : : }
1189 : :
1190 : 0 : bool extendRouteToDestinations(route::FullRoute &route, const std::vector<route::planning::RoutingParaPoint> &dest)
1191 : : {
1192 : 0 : return extendRouteToDestinationsT(route, dest);
1193 : : }
1194 : :
1195 : 2 : bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::GeoPoint> &dest)
1196 : : {
1197 : 2 : return extendRouteToDestinationsT(route, dest);
1198 : : }
1199 : :
1200 : 0 : bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::ENUPoint> &dest)
1201 : : {
1202 : 0 : return extendRouteToDestinationsT(route, dest);
1203 : : }
1204 : :
1205 : 3 : bool extendRouteToDistance(route::FullRoute &route,
1206 : : const physics::Distance &length,
1207 : : route::FullRouteList &additionalRoutes,
1208 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
1209 : : {
1210 [ - + ]: 3 : if (!additionalRoutes.empty())
1211 : : {
1212 [ # # # # ]: 0 : access::getLogger()->error("ad::map::route::extendRouteToDistance: additional Routes parameter is not empty, "
1213 : : "containing {} elements. Aborting.",
1214 : 0 : additionalRoutes.size());
1215 : 0 : return false;
1216 : : }
1217 : :
1218 : : // check length of route and abort if long enough
1219 [ + - ]: 3 : auto routeLength = route::calcLength(route);
1220 [ + - ]: 3 : auto distance = length - routeLength;
1221 [ + - - + ]: 3 : if (distance < physics::Distance(0))
1222 : : {
1223 : 0 : return true;
1224 : : }
1225 : :
1226 [ + - ]: 3 : route::planning::RoutingParaPoint routeExtensionStartPoint;
1227 [ + - ]: 6 : route::LaneSegment extensionReferenceLaneSegment;
1228 [ + - - + ]: 3 : if (!prepareRouteForExtension(route, routeExtensionStartPoint, extensionReferenceLaneSegment))
1229 : : {
1230 : 0 : return false;
1231 : : }
1232 : :
1233 [ + - + - ]: 3 : distance += route::calcLength(extensionReferenceLaneSegment.laneInterval);
1234 : :
1235 : : // the shorter version of the route is kept to obey
1236 : : // potential intersections which already might have been entered by one of the prediction paths e.g.
1237 : : // because of a slightly shorter lane entering in that intersection arm
1238 : : auto routeExtensions
1239 : : = route::planning::predictRoutesOnDistance(routeExtensionStartPoint,
1240 : : distance,
1241 : : route.routeCreationMode,
1242 : : planning::FilterDuplicatesMode::SubRoutesPreferShorterOnes,
1243 [ + - ]: 3 : relevantLanes);
1244 : :
1245 : 3 : auto it = routeExtensions.begin();
1246 [ + - ]: 3 : if (it != routeExtensions.end())
1247 : : {
1248 : 3 : ++it;
1249 : : }
1250 [ + + ]: 5 : for (; it != routeExtensions.end(); ++it)
1251 : : {
1252 : 2 : auto const &routeExtension = *it;
1253 [ + - ]: 4 : auto newRoute = mergeRouteExtension(route, extensionReferenceLaneSegment, routeExtension);
1254 [ + - ]: 2 : additionalRoutes.push_back(newRoute);
1255 : : }
1256 : :
1257 : : // extend route itself
1258 [ + - ]: 3 : if (routeExtensions.size() > 0)
1259 : : {
1260 : 3 : auto const &routeExtension = routeExtensions.front();
1261 [ + - ]: 3 : route = mergeRouteExtension(route, extensionReferenceLaneSegment, routeExtension);
1262 : : }
1263 : :
1264 : 3 : return true;
1265 : : }
1266 : :
1267 : 3 : void shortenSegmentFromBegin(RoadSegment &roadSegment, physics::Distance const &distance)
1268 : : {
1269 [ + + ]: 3 : if (roadSegment.drivableLaneSegments.empty())
1270 : : {
1271 : : throw std::runtime_error("ad::map::route::shortenSegmentFromBegin>> Route inconsistent: "
1272 [ + - ]: 1 : "route contains no drivableLaneSegments");
1273 : : }
1274 : :
1275 [ + - ]: 2 : auto shortenedInterval = shortenIntervalFromBegin(roadSegment.drivableLaneSegments.front().laneInterval, distance);
1276 [ + + ]: 4 : for (auto &laneSegment : roadSegment.drivableLaneSegments)
1277 : : {
1278 : 2 : laneSegment.laneInterval.start = shortenedInterval.start;
1279 : : }
1280 : 2 : }
1281 : :
1282 : 4 : void shortenSegmentFromEnd(RoadSegment &roadSegment, physics::Distance const &distance)
1283 : : {
1284 [ + + ]: 4 : if (roadSegment.drivableLaneSegments.empty())
1285 : : {
1286 : : throw std::runtime_error("ad::map::route::shortenSegmentFromBegin>> Route inconsistent: "
1287 [ + - ]: 1 : "route contains no drivableLaneSegments");
1288 : : }
1289 : :
1290 [ + - ]: 3 : auto shortenedInterval = shortenIntervalFromEnd(roadSegment.drivableLaneSegments.front().laneInterval, distance);
1291 [ + + ]: 7 : for (auto &laneSegment : roadSegment.drivableLaneSegments)
1292 : : {
1293 : 4 : laneSegment.laneInterval.end = shortenedInterval.end;
1294 : : }
1295 : 3 : }
1296 : :
1297 : 2 : bool calculateRouteParaPointAtDistance(route::FullRoute const &route,
1298 : : route::RouteParaPoint const &origin,
1299 : : physics::Distance const &distance,
1300 : : route::RouteParaPoint &resultingPoint)
1301 : : {
1302 [ + - ]: 2 : RouteIterator routeIterator = getRouteIterator(origin, route);
1303 : :
1304 : 2 : physics::Distance accumulatedDistance(0.);
1305 : :
1306 : 2 : bool routeParaPointFound = false;
1307 : :
1308 [ + + + - : 4 : while (!routeParaPointFound && routeIterator.isValid())
+ + + + ]
1309 : : {
1310 : 2 : physics::ParametricValue originOffset(0.);
1311 [ + - ]: 2 : const physics::Distance segmentLength = calcLength((*routeIterator.roadSegmentIterator));
1312 : :
1313 : 2 : physics::Distance distanceToSegmentBorder = segmentLength;
1314 [ + + ]: 2 : if (routeIterator.roadSegmentIterator->segmentCountFromDestination == origin.segmentCountFromDestination)
1315 : : {
1316 : 1 : originOffset = origin.parametricOffset;
1317 [ + - - + ]: 1 : if (distance < physics::Distance(0))
1318 : : {
1319 [ # # ]: 0 : distanceToSegmentBorder = segmentLength * (origin.parametricOffset);
1320 : : }
1321 : : else
1322 : : {
1323 [ + - + - ]: 1 : distanceToSegmentBorder = segmentLength * (physics::ParametricValue(1.0) - origin.parametricOffset);
1324 : : }
1325 : : }
1326 : : else
1327 : : {
1328 [ + - - + ]: 1 : if (distance < physics::Distance(0))
1329 : : {
1330 : 0 : originOffset = physics::ParametricValue(1.);
1331 : : }
1332 : : }
1333 : :
1334 [ + - + - : 2 : if ((accumulatedDistance + distanceToSegmentBorder) >= std::fabs(distance))
+ + ]
1335 : : {
1336 [ + - ]: 1 : physics::Distance const remainingLength = std::fabs(distance) - accumulatedDistance;
1337 : :
1338 : 1 : resultingPoint.routePlanningCounter = route.routePlanningCounter;
1339 : 1 : resultingPoint.segmentCountFromDestination = routeIterator.roadSegmentIterator->segmentCountFromDestination;
1340 [ + - ]: 1 : physics::ParametricValue delta(remainingLength / segmentLength);
1341 [ + - - + ]: 1 : if (distance < physics::Distance(0))
1342 : : {
1343 [ # # ]: 0 : resultingPoint.parametricOffset = originOffset - delta;
1344 : : }
1345 : : else
1346 : : {
1347 [ + - ]: 1 : resultingPoint.parametricOffset = originOffset + delta;
1348 : : }
1349 : 1 : accumulatedDistance = std::fabs(distance);
1350 : 1 : routeParaPointFound = true;
1351 : : }
1352 : : else
1353 : : {
1354 [ + - ]: 1 : accumulatedDistance += distanceToSegmentBorder;
1355 : : }
1356 : :
1357 [ + - - + ]: 2 : if (distance < physics::Distance(0))
1358 : : {
1359 [ # # ]: 0 : if (routeIterator.roadSegmentIterator != std::begin(routeIterator.route.roadSegments))
1360 : : {
1361 : 0 : routeIterator.roadSegmentIterator--;
1362 : : }
1363 : : else
1364 : : {
1365 : 0 : routeIterator.roadSegmentIterator = std::end(routeIterator.route.roadSegments);
1366 : : }
1367 : : }
1368 : : else
1369 : : {
1370 : 2 : routeIterator.roadSegmentIterator++;
1371 : : }
1372 : : }
1373 : :
1374 : 2 : return routeParaPointFound;
1375 : : }
1376 : :
1377 : 2 : bool getRouteParaPointFromParaPoint(point::ParaPoint const ¶Point,
1378 : : FullRoute const &route,
1379 : : route::RouteParaPoint &routeParaPoint)
1380 : : {
1381 [ + - ]: 2 : auto const waypointResult = route::findWaypoint(paraPoint, route);
1382 : :
1383 [ - + ]: 2 : if (!waypointResult.isValid())
1384 : : {
1385 : 0 : return false;
1386 : : }
1387 : :
1388 : 2 : routeParaPoint.routePlanningCounter = route.routePlanningCounter;
1389 : 2 : routeParaPoint.segmentCountFromDestination = waypointResult.roadSegmentIterator->segmentCountFromDestination;
1390 : : routeParaPoint.parametricOffset
1391 [ + - ]: 2 : = std::fabs(paraPoint.parametricOffset - waypointResult.laneSegmentIterator->laneInterval.start);
1392 : :
1393 : 2 : return true;
1394 : : }
1395 : :
1396 : 2 : route::FullRoute getRouteExpandedTo(route::FullRoute const &route, RouteCreationMode const routeCreationMode)
1397 : : {
1398 : 2 : route::FullRoute expandedRoute;
1399 : 2 : expandedRoute.routeCreationMode = routeCreationMode;
1400 [ + + ]: 6 : for (const auto &roadSegment : route.roadSegments)
1401 : : {
1402 [ + - ]: 4 : if (!roadSegment.drivableLaneSegments.empty())
1403 : : {
1404 [ + - ]: 8 : appendRoadSegmentToRoute(roadSegment.drivableLaneSegments.front().laneInterval,
1405 : 4 : roadSegment.drivableLaneSegments.front().routeLaneOffset,
1406 : : expandedRoute,
1407 : : {});
1408 : : }
1409 : : }
1410 [ + - ]: 2 : planning::updateRoutePlanningCounters(expandedRoute);
1411 : :
1412 [ + - ]: 2 : if (!route.roadSegments.empty())
1413 : : {
1414 [ + - ]: 2 : if (!route.roadSegments.front().drivableLaneSegments.empty())
1415 : : {
1416 : : point::ParaPoint const alignmentParaPoint
1417 [ + - ]: 2 : = getIntervalStart(route.roadSegments.front().drivableLaneSegments.front().laneInterval);
1418 [ + - ]: 2 : alignRouteStartingPoints(alignmentParaPoint, expandedRoute);
1419 : : }
1420 [ + - ]: 2 : if (!route.roadSegments.back().drivableLaneSegments.empty())
1421 : : {
1422 : : point::ParaPoint const alignmentParaPoint
1423 [ + - ]: 2 : = getIntervalEnd(route.roadSegments.back().drivableLaneSegments.front().laneInterval);
1424 [ + - ]: 2 : alignRouteEndingPoints(alignmentParaPoint, expandedRoute);
1425 : : }
1426 : : }
1427 : :
1428 : 2 : return expandedRoute;
1429 : : }
1430 : :
1431 : 4 : FullRoute getRouteSection(FindWaypointResult const ¤tLane,
1432 : : physics::Distance const &distanceFront,
1433 : : physics::Distance const &distanceEnd,
1434 : : FullRoute const &route,
1435 : : RouteSectionCreationMode const routeSectionCreationMode)
1436 : : {
1437 : 4 : FullRoute resultRoute;
1438 [ + - - + : 4 : if (!currentLane.isValid() || (¤tLane.queryRoute != &route))
- + ]
1439 : : {
1440 : 0 : return resultRoute;
1441 : : }
1442 : 4 : resultRoute.fullRouteSegmentCount = route.fullRouteSegmentCount;
1443 : 4 : resultRoute.routePlanningCounter = route.routePlanningCounter;
1444 : :
1445 [ + - ]: 8 : LaneSegment currentLaneSegment = *currentLane.laneSegmentIterator;
1446 : :
1447 [ + - ]: 4 : LaneInterval currentStartInterval;
1448 : 4 : currentStartInterval.laneId = currentLane.laneSegmentIterator->laneInterval.laneId;
1449 : 4 : currentStartInterval.start = currentLane.laneSegmentIterator->laneInterval.start;
1450 : 4 : currentStartInterval.end = currentLane.queryPosition.parametricOffset;
1451 : :
1452 [ + - ]: 4 : physics::Distance accumulatedDistanceFront = calcLength(currentStartInterval);
1453 : :
1454 [ + - + + ]: 4 : if (accumulatedDistanceFront >= distanceFront)
1455 : : {
1456 : : currentLaneSegment.laneInterval
1457 [ + - + - ]: 1 : = shortenIntervalFromBegin(currentLaneSegment.laneInterval, accumulatedDistanceFront - distanceFront);
1458 : 1 : accumulatedDistanceFront = distanceFront;
1459 : : }
1460 : : else
1461 : : {
1462 [ + - ]: 6 : auto currentPredecessors = currentLane.getPredecessorLanes();
1463 : 3 : while ( // required distance not yet reached
1464 [ + - ]: 6 : (accumulatedDistanceFront < distanceFront)
1465 : : // there are still predecessors available
1466 [ + + + + : 6 : && (!currentPredecessors.empty()))
+ + ]
1467 : : {
1468 : : // another road segment will be added
1469 [ + - ]: 6 : route::RoadSegment newRoadSegment;
1470 : 3 : newRoadSegment.boundingSphere = currentPredecessors[0].roadSegmentIterator->boundingSphere;
1471 : : newRoadSegment.segmentCountFromDestination
1472 : 3 : = currentPredecessors[0].roadSegmentIterator->segmentCountFromDestination;
1473 : :
1474 : 6 : std::vector<FindWaypointResult> nextPredecessors;
1475 [ + + ]: 6 : for (auto const &predecessor : currentPredecessors)
1476 : : {
1477 [ + - ]: 3 : newRoadSegment.drivableLaneSegments.push_back(*predecessor.laneSegmentIterator);
1478 [ + - ]: 3 : auto const furtherPredecessors = predecessor.getPredecessorLanes();
1479 : : nextPredecessors.insert(
1480 [ + - ]: 3 : std::end(nextPredecessors), std::begin(furtherPredecessors), std::end(furtherPredecessors));
1481 : : }
1482 : :
1483 [ + - ]: 3 : auto currentSegmentLength = calcLength(newRoadSegment);
1484 [ + - + - : 3 : if (accumulatedDistanceFront + currentSegmentLength > distanceFront)
+ + ]
1485 : : {
1486 [ + - + - : 2 : route::shortenSegmentFromBegin(newRoadSegment, accumulatedDistanceFront + currentSegmentLength - distanceFront);
+ - ]
1487 : 2 : accumulatedDistanceFront = distanceFront;
1488 : : }
1489 : : else
1490 : : {
1491 [ + - ]: 1 : accumulatedDistanceFront += currentSegmentLength;
1492 : : }
1493 : :
1494 [ + - + - ]: 6 : access::getLogger()->trace("ad::map::route::getRouteSection: prepending road segment {}: {} ({})",
1495 : : newRoadSegment,
1496 : : accumulatedDistanceFront,
1497 : : distanceFront);
1498 [ + - ]: 3 : resultRoute.roadSegments.insert(std::begin(resultRoute.roadSegments), newRoadSegment);
1499 : :
1500 : : // prepare for next cycle
1501 : 3 : currentPredecessors.swap(nextPredecessors);
1502 : : }
1503 : : }
1504 : :
1505 [ + - ]: 4 : LaneInterval currentEndInterval;
1506 : 4 : currentEndInterval.laneId = currentLane.laneSegmentIterator->laneInterval.laneId;
1507 : 4 : currentEndInterval.start = currentLane.queryPosition.parametricOffset;
1508 : 4 : currentEndInterval.end = currentLane.laneSegmentIterator->laneInterval.end;
1509 : :
1510 [ + - ]: 4 : physics::Distance accumulatedDistanceEnd = calcLength(currentEndInterval);
1511 [ + - + + ]: 4 : if (accumulatedDistanceEnd >= distanceEnd)
1512 : : {
1513 : : currentLaneSegment.laneInterval
1514 [ + - + - ]: 1 : = shortenIntervalFromEnd(currentLaneSegment.laneInterval, accumulatedDistanceEnd - distanceEnd);
1515 : 1 : accumulatedDistanceEnd = distanceEnd;
1516 : : }
1517 : : else
1518 : : {
1519 : 3 : currentLaneSegment.laneInterval.end = currentEndInterval.end;
1520 : : }
1521 : :
1522 : : // current road segment is added
1523 [ + - ]: 8 : route::RoadSegment currentRoadSegment;
1524 : 4 : currentRoadSegment.boundingSphere = currentLane.roadSegmentIterator->boundingSphere;
1525 : 4 : currentRoadSegment.segmentCountFromDestination = currentLane.roadSegmentIterator->segmentCountFromDestination;
1526 [ + - ]: 4 : currentRoadSegment.drivableLaneSegments.push_back(currentLaneSegment);
1527 [ + - + - ]: 8 : access::getLogger()->trace("ad::map::route::getRouteSection: appending current road segment {}: {}({}) -> {}({})",
1528 : : currentRoadSegment,
1529 : : accumulatedDistanceFront,
1530 : : distanceFront,
1531 : : accumulatedDistanceEnd,
1532 : : distanceEnd);
1533 [ + - ]: 4 : resultRoute.roadSegments.insert(std::end(resultRoute.roadSegments), currentRoadSegment);
1534 : :
1535 [ + - ]: 8 : auto currentSuccessors = currentLane.getSuccessorLanes();
1536 : 7 : while ( // required distance not yet reached
1537 [ + - ]: 11 : (accumulatedDistanceEnd < distanceEnd)
1538 : : // there are still successors available
1539 [ + + + + : 11 : && (!currentSuccessors.empty()))
+ + ]
1540 : : {
1541 : : // another road segment will be added
1542 [ + - ]: 14 : route::RoadSegment newRoadSegment;
1543 : 7 : newRoadSegment.boundingSphere = currentSuccessors[0].roadSegmentIterator->boundingSphere;
1544 : 7 : newRoadSegment.segmentCountFromDestination = currentSuccessors[0].roadSegmentIterator->segmentCountFromDestination;
1545 : :
1546 : 14 : std::vector<FindWaypointResult> nextSuccessors;
1547 [ + + ]: 14 : for (auto const &successor : currentSuccessors)
1548 : : {
1549 [ + - ]: 7 : newRoadSegment.drivableLaneSegments.push_back(*successor.laneSegmentIterator);
1550 [ + - ]: 7 : auto const furtherSuccessors = successor.getSuccessorLanes();
1551 [ + - ]: 7 : nextSuccessors.insert(std::end(nextSuccessors), std::begin(furtherSuccessors), std::end(furtherSuccessors));
1552 : : }
1553 : :
1554 [ + - ]: 7 : auto currentSegmentLength = calcLength(newRoadSegment);
1555 [ + - + - : 7 : if (accumulatedDistanceEnd + currentSegmentLength > distanceEnd)
+ + ]
1556 : : {
1557 [ + - + - : 2 : route::shortenSegmentFromEnd(newRoadSegment, accumulatedDistanceEnd + currentSegmentLength - distanceEnd);
+ - ]
1558 : 2 : accumulatedDistanceEnd = distanceEnd;
1559 : : }
1560 : : else
1561 : : {
1562 [ + - ]: 5 : accumulatedDistanceEnd += currentSegmentLength;
1563 : : }
1564 : :
1565 [ + - + - ]: 14 : access::getLogger()->trace("ad::map::route::getRouteSection: appending road segment {}: {} ({})",
1566 : : newRoadSegment,
1567 : : accumulatedDistanceEnd,
1568 : : distanceEnd);
1569 [ + - ]: 7 : resultRoute.roadSegments.insert(std::end(resultRoute.roadSegments), newRoadSegment);
1570 : :
1571 : : // prepare for next cycle
1572 : 7 : currentSuccessors.swap(nextSuccessors);
1573 : : }
1574 : :
1575 [ + - + - ]: 8 : access::getLogger()->trace("ad::map::route::getRouteSection: result before update lane connections {}", resultRoute);
1576 [ + - ]: 4 : updateLaneConnections(resultRoute);
1577 : :
1578 [ - + ]: 4 : if (routeSectionCreationMode == RouteSectionCreationMode::AllRouteLanes)
1579 : : {
1580 [ # # ]: 0 : resultRoute = getRouteExpandedTo(resultRoute, route.routeCreationMode);
1581 : : }
1582 : :
1583 [ + - + - ]: 8 : access::getLogger()->debug("ad::map::route::getRouteSection({} < {}:{} > {} ) {}",
1584 : : distanceFront,
1585 : 4 : currentLane.laneSegmentIterator->laneInterval.laneId,
1586 : 4 : currentLane.queryPosition.parametricOffset,
1587 : : distanceEnd,
1588 : : resultRoute);
1589 : :
1590 : 4 : return resultRoute;
1591 : : }
1592 : :
1593 : 4 : FullRoute getRouteSection(point::ParaPoint const ¢erPoint,
1594 : : physics::Distance const &distanceFront,
1595 : : physics::Distance const &distanceEnd,
1596 : : FullRoute const &route,
1597 : : RouteSectionCreationMode const routeSectionCreationMode)
1598 : : {
1599 [ + - ]: 8 : return getRouteSection(findWaypoint(centerPoint, route), distanceFront, distanceEnd, route, routeSectionCreationMode);
1600 : : }
1601 : :
1602 : 0 : FullRoute getRouteSection(match::Object const &object,
1603 : : FullRoute const &route,
1604 : : RouteSectionCreationMode const routeSectionCreationMode)
1605 : : {
1606 [ # # ]: 0 : auto const objectCenterPointOnRoute = findCenterWaypoint(object, route);
1607 : : auto const routeAroundVehicle = getRouteSection(objectCenterPointOnRoute,
1608 : 0 : object.enuPosition.dimension.length,
1609 : 0 : object.enuPosition.dimension.length,
1610 : : route,
1611 [ # # ]: 0 : routeSectionCreationMode);
1612 : 0 : return routeAroundVehicle;
1613 : : }
1614 : :
1615 : 5 : FindLaneChangeResult::FindLaneChangeResult(FullRoute const &route)
1616 : : : queryRoute(route)
1617 : 5 : , laneChangeStartRouteIterator(queryRoute.roadSegments.end())
1618 : 5 : , laneChangeEndRouteIterator(queryRoute.roadSegments.end())
1619 : 10 : , laneChangeDirection(LaneChangeDirection::Invalid)
1620 : : {
1621 : 5 : }
1622 : :
1623 : 1 : physics::Distance FindLaneChangeResult::calcZoneLength() const
1624 : : {
1625 : 1 : physics::Distance distance(0.);
1626 [ + - + - ]: 1 : if (isValid())
1627 : : {
1628 [ - + ]: 1 : for (auto roadSegmentIter = laneChangeStartRouteIterator; roadSegmentIter < laneChangeEndRouteIterator;
1629 : 0 : roadSegmentIter++)
1630 : : {
1631 [ # # # # ]: 0 : distance += calcLength(*roadSegmentIter);
1632 : : }
1633 [ + - + - ]: 1 : distance += calcLength(*laneChangeEndRouteIterator);
1634 : : }
1635 : 1 : return distance;
1636 : : }
1637 : :
1638 : 5 : FindLaneChangeResult findFirstLaneChange(match::MapMatchedPosition const ¤tPosition,
1639 : : route::FullRoute const &route)
1640 : : {
1641 : : // this is what we want to return. Two indices defining the start and the end of the lane change
1642 [ + - ]: 5 : FindLaneChangeResult result(route);
1643 : :
1644 : : // first, find the iterator where the first lane change needs to be finished at the latest. If the target lane is
1645 : : // always blocked the user of this function may want to set a stop line at the routeIteratorLaneChangeEnd. To do
1646 : : // this,
1647 : : // - we travel along the successors starting from the current position as long as we can
1648 : : // - if the next route section is not reachable by successors and is not the end of the route, we found the lane
1649 : : // change end and traverse along the route in the other direction to find the lane change start
1650 : : // - if we can travel along the whole route without changing lanes, we return an invalid result (since there is no
1651 : : // lane change on this route segment)
1652 [ + - ]: 5 : FindWaypointResult const findWaypointResult = findWaypoint(currentPosition.lanePoint.paraPoint, route);
1653 : :
1654 [ + + ]: 5 : if (!findWaypointResult.isValid())
1655 : : {
1656 [ + - ]: 1 : if (!route.roadSegments.empty())
1657 : : {
1658 [ + - + - ]: 2 : access::getLogger()->error(
1659 : : "ad::map::route::findFirstLaneChange: Current position is not part of the route {} {}", currentPosition, route);
1660 : : }
1661 : : // returning an invalid result (as if no lane change was found)
1662 : 1 : return result;
1663 : : }
1664 : :
1665 : : // find the potential end of the lane change
1666 : : // if there is no successor, the lane change has to end
1667 : : // and if there is more than one successor, the lane change has to end, too
1668 : 4 : auto laneChangeEndResult = findWaypointResult;
1669 [ + - + + ]: 16 : for (auto successorLanes = findWaypointResult.getSuccessorLanes(); successorLanes.size() == 1u;
1670 [ + - ]: 12 : successorLanes = successorLanes[0].getSuccessorLanes())
1671 : : {
1672 [ + - ]: 12 : laneChangeEndResult = successorLanes[0];
1673 : : }
1674 : :
1675 : : // Find the transition lane, to which we need to change to be able to get to the next road segment.
1676 : : // The closest neighbor lane to laneChangeEndResult having any successors
1677 [ + - ]: 4 : FindWaypointResult rightNeighbor(route);
1678 : 4 : std::size_t rightNeighborDistance = 0u;
1679 [ + - + + : 4 : for (auto neighbor = laneChangeEndResult.getRightLane(); neighbor.isValid(); neighbor = neighbor.getRightLane())
- - - - ]
1680 : : {
1681 : 1 : rightNeighborDistance++;
1682 [ + - + - ]: 1 : if (!neighbor.getSuccessorLanes().empty())
1683 : : {
1684 [ + - ]: 1 : rightNeighbor = neighbor;
1685 : 1 : break;
1686 : : }
1687 : : }
1688 : :
1689 [ + - ]: 4 : FindWaypointResult leftNeighbor(route);
1690 : 4 : std::size_t leftNeighborDistance = 0u;
1691 [ + - + + : 4 : for (auto neighbor = laneChangeEndResult.getLeftLane(); neighbor.isValid(); neighbor = neighbor.getLeftLane())
- - - - ]
1692 : : {
1693 : 1 : leftNeighborDistance++;
1694 [ + - + - ]: 1 : if (!neighbor.getSuccessorLanes().empty())
1695 : : {
1696 [ + - ]: 1 : leftNeighbor = neighbor;
1697 : 1 : break;
1698 : : }
1699 : : }
1700 : :
1701 [ + - ]: 4 : FindWaypointResult transitionEndLane(route);
1702 [ + + - + : 4 : if (leftNeighbor.isValid() && rightNeighbor.isValid())
- + ]
1703 : : {
1704 [ # # ]: 0 : if (leftNeighborDistance < rightNeighborDistance)
1705 : : {
1706 [ # # ]: 0 : transitionEndLane = leftNeighbor;
1707 : 0 : result.laneChangeDirection = LaneChangeDirection::RightToLeft;
1708 : : }
1709 : : else
1710 : : {
1711 [ # # ]: 0 : transitionEndLane = rightNeighbor;
1712 : 0 : result.laneChangeDirection = LaneChangeDirection::LeftToRight;
1713 : : }
1714 : : }
1715 [ + + ]: 4 : else if (leftNeighbor.isValid())
1716 : : {
1717 [ + - ]: 1 : transitionEndLane = leftNeighbor;
1718 : 1 : result.laneChangeDirection = LaneChangeDirection::RightToLeft;
1719 : : }
1720 [ + + ]: 3 : else if (rightNeighbor.isValid())
1721 : : {
1722 [ + - ]: 1 : transitionEndLane = rightNeighbor;
1723 : 1 : result.laneChangeDirection = LaneChangeDirection::LeftToRight;
1724 : : }
1725 : : else
1726 : : {
1727 : : // there is no lane change at all, since no neighbor has any successor
1728 [ + - + - ]: 4 : access::getLogger()->trace("ad::map::route::no lane change required {} {}", currentPosition, route);
1729 : 2 : return result;
1730 : : }
1731 : :
1732 : : // we found an actual lane change
1733 : 2 : result.laneChangeEndRouteIterator = transitionEndLane.roadSegmentIterator;
1734 : 2 : result.laneChangeEndLaneSegmentIterator = transitionEndLane.laneSegmentIterator;
1735 : :
1736 : : // now, traverse back to the beginning (currentPosiion) from the transition lane
1737 : : // if there is no predecessor, the lane change has to begin latest there
1738 : : // and if there is more than one predecessor, the lane change has to begin there, too
1739 : 2 : bool currentPositionReached = false;
1740 : :
1741 [ + + ]: 4 : while (!currentPositionReached)
1742 : : {
1743 : 3 : auto laneChangeBeginResult = transitionEndLane;
1744 [ + - + + ]: 6 : for (auto predecessorLanes = transitionEndLane.getPredecessorLanes(); predecessorLanes.size() == 1u;
1745 [ + - ]: 3 : predecessorLanes = predecessorLanes[0].getPredecessorLanes())
1746 : : {
1747 [ + - ]: 3 : laneChangeBeginResult = predecessorLanes[0];
1748 : : }
1749 : :
1750 [ + - ]: 3 : FindWaypointResult transitionStartLane(route);
1751 [ + + ]: 3 : if (result.laneChangeDirection == LaneChangeDirection::LeftToRight)
1752 : : {
1753 [ + - + - ]: 2 : transitionStartLane = laneChangeBeginResult.getLeftLane();
1754 : : }
1755 : : else
1756 : : {
1757 [ + - + - ]: 1 : transitionStartLane = laneChangeBeginResult.getRightLane();
1758 : : }
1759 [ + + ]: 3 : if (!transitionStartLane.isValid())
1760 : : {
1761 [ + - + - ]: 2 : access::getLogger()->error("ad::map::route::findFirstLaneChange: cannot find valid transition start lane at lane "
1762 : : "change, begin: {} with lane change direction {} and route: {}",
1763 : 1 : laneChangeBeginResult.laneSegmentIterator->laneInterval.laneId,
1764 : : result.laneChangeDirection,
1765 : : route);
1766 : :
1767 : : // returning an invalid result (as if no lane change was found)
1768 : 1 : return result;
1769 : : }
1770 : :
1771 : 2 : result.laneChangeStartRouteIterator = transitionStartLane.roadSegmentIterator;
1772 : 2 : result.laneChangeStartLaneSegmentIterator = transitionStartLane.laneSegmentIterator;
1773 : :
1774 [ + - + - ]: 4 : access::getLogger()->debug("ad::map::route::findFirstLaneChange: found valid lane change {} starting at {} "
1775 : : "laneId[] {} ending at {} laneId[] {} input position {} and route {}",
1776 : : result.laneChangeDirection,
1777 : 2 : *result.laneChangeStartRouteIterator,
1778 : 2 : result.laneChangeStartLaneSegmentIterator->laneInterval.laneId,
1779 : 2 : *result.laneChangeEndRouteIterator,
1780 : 2 : result.laneChangeEndLaneSegmentIterator->laneInterval.laneId,
1781 : : currentPosition,
1782 : : route);
1783 : :
1784 [ + - + + ]: 2 : if (result.laneChangeStartLaneSegmentIterator->laneInterval.laneId == currentPosition.lanePoint.paraPoint.laneId)
1785 : : {
1786 : 1 : currentPositionReached = true;
1787 : : }
1788 : : else
1789 : : {
1790 : 1 : result.laneChangeEndRouteIterator = result.laneChangeStartRouteIterator;
1791 : 1 : result.laneChangeEndLaneSegmentIterator = result.laneChangeStartLaneSegmentIterator;
1792 : 1 : transitionEndLane.laneSegmentIterator = result.laneChangeEndLaneSegmentIterator;
1793 : 1 : transitionEndLane.roadSegmentIterator = result.laneChangeStartRouteIterator;
1794 : : }
1795 : :
1796 : 2 : result.numberOfConnectedLaneChanges++;
1797 : : }
1798 : :
1799 : 1 : return result;
1800 : : }
1801 : :
1802 : 27304 : void addLaneIdUnique(lane::LaneIdList &laneIds, lane::LaneId const laneId)
1803 : : {
1804 [ + - ]: 27304 : auto findResult = std::find(std::begin(laneIds), std::end(laneIds), laneId);
1805 [ + + ]: 27304 : if (findResult == std::end(laneIds))
1806 : : {
1807 [ + - ]: 20544 : laneIds.push_back(laneId);
1808 : : }
1809 : 27304 : }
1810 : :
1811 : 14177 : void addRoutePredecessorRelationToNewSegment(lane::Lane const &lane,
1812 : : route::LaneSegment &laneSegment,
1813 : : route::FullRoute &route,
1814 : : bool routeDirectionPositive)
1815 : : {
1816 [ + + ]: 14177 : if (route.roadSegments.empty())
1817 : : {
1818 : 3687 : return;
1819 : : }
1820 : 20035 : for (auto predecessorLaneContact : lane::getContactLanes(
1821 [ + + + - : 50560 : lane, routeDirectionPositive ? lane::ContactLocation::PREDECESSOR : lane::ContactLocation::SUCCESSOR))
+ + + - ]
1822 : : {
1823 : 20035 : auto predecessorLaneId = predecessorLaneContact.toLane;
1824 : 20035 : auto predecessorLaneSegmentIter = std::find_if(std::begin(route.roadSegments.back().drivableLaneSegments),
1825 : 40070 : std::end(route.roadSegments.back().drivableLaneSegments),
1826 : 26170 : [&predecessorLaneId](route::LaneSegment const &innerLaneSegment) {
1827 : 26170 : return innerLaneSegment.laneInterval.laneId == predecessorLaneId;
1828 [ + - ]: 40070 : });
1829 : :
1830 [ + + ]: 20035 : if (predecessorLaneSegmentIter != std::end(route.roadSegments.back().drivableLaneSegments))
1831 : : {
1832 [ + - ]: 13652 : addLaneIdUnique(predecessorLaneSegmentIter->successors, laneSegment.laneInterval.laneId);
1833 [ + - ]: 13652 : addLaneIdUnique(laneSegment.predecessors, predecessorLaneId);
1834 : 13652 : laneSegment.routeLaneOffset = predecessorLaneSegmentIter->routeLaneOffset;
1835 : : }
1836 : : }
1837 : : }
1838 : :
1839 : 21502 : bool isRightNeighbor(lane::ContactLocation const contactLocation, bool const isRouteDirectionPositive)
1840 : : {
1841 : : bool rightNeighbor;
1842 : : // determine the neighborhood relationship in respect to route direction
1843 [ + + ]: 21502 : if (contactLocation == lane::ContactLocation::RIGHT)
1844 : : {
1845 : 10751 : rightNeighbor = isRouteDirectionPositive;
1846 : : }
1847 : : else // contactLocation == core::ContactLocation::LEFT
1848 : : {
1849 : 10751 : rightNeighbor = !isRouteDirectionPositive;
1850 : : }
1851 : 21502 : return rightNeighbor;
1852 : : }
1853 : :
1854 : 14648 : void updateRouteLaneOffsetRange(RouteLaneOffset const &routeLaneOffset, FullRoute &route)
1855 : : {
1856 [ + + ]: 14648 : if (routeLaneOffset > route.maxLaneOffset)
1857 : : {
1858 : 607 : route.maxLaneOffset = routeLaneOffset;
1859 : : }
1860 [ + + ]: 14648 : if (routeLaneOffset < route.minLaneOffset)
1861 : : {
1862 : 483 : route.minLaneOffset = routeLaneOffset;
1863 : : }
1864 : 14648 : }
1865 : :
1866 : 3897 : void updateRouteLaneOffset(bool const rightNeighbor, RouteLaneOffset &routeLaneOffset, FullRoute &route)
1867 : : {
1868 [ + + ]: 3897 : if (rightNeighbor)
1869 : : {
1870 : 1709 : routeLaneOffset--;
1871 : : }
1872 : : else
1873 : : {
1874 : 2188 : routeLaneOffset++;
1875 : : }
1876 : 3897 : updateRouteLaneOffsetRange(routeLaneOffset, route);
1877 : 3897 : }
1878 : :
1879 : 2 : void appendLaneSegmentToRoute(route::LaneInterval const &laneInterval,
1880 : : route::FullRoute &route,
1881 : : route::SegmentCounter const segmentCountFromDestination)
1882 : : {
1883 [ + - + - ]: 4 : auto lane = lane::getLane(laneInterval.laneId);
1884 : :
1885 [ + - ]: 4 : route::RoadSegment roadSegment;
1886 : 2 : roadSegment.boundingSphere = lane.boundingSphere;
1887 : 2 : roadSegment.segmentCountFromDestination = segmentCountFromDestination;
1888 : :
1889 [ + - ]: 2 : bool const routeDirectionPositive = isRouteDirectionPositive(laneInterval);
1890 : :
1891 [ + - ]: 4 : route::LaneSegment laneSegment;
1892 : 2 : laneSegment.laneInterval = laneInterval;
1893 : :
1894 [ + - ]: 2 : addRoutePredecessorRelationToNewSegment(lane, laneSegment, route, routeDirectionPositive);
1895 : :
1896 [ + - ]: 2 : roadSegment.drivableLaneSegments.push_back(laneSegment);
1897 [ + - ]: 2 : route.roadSegments.push_back(roadSegment);
1898 : 2 : }
1899 : :
1900 : 10751 : void appendRoadSegmentToRoute(route::LaneInterval const &laneInterval,
1901 : : route::RouteLaneOffset const &routeLaneOffset,
1902 : : FullRoute &route,
1903 : : lane::LaneIdSet const &relevantLanes)
1904 : : {
1905 [ + - + - ]: 21502 : auto lane = lane::getLane(laneInterval.laneId);
1906 : :
1907 [ + - ]: 21502 : route::RoadSegment roadSegment;
1908 : 10751 : roadSegment.boundingSphere = lane.boundingSphere;
1909 : : // this information will be post processed by updateRoutePlanningCounters()
1910 : 10751 : roadSegment.segmentCountFromDestination = 0u;
1911 : :
1912 [ + - ]: 10751 : const bool routeDirectionPositive = isRouteDirectionPositive(laneInterval);
1913 : :
1914 [ + - ]: 21502 : route::LaneSegment laneSegment;
1915 : 10751 : laneSegment.laneInterval = laneInterval;
1916 : 10751 : laneSegment.routeLaneOffset = routeLaneOffset;
1917 [ + - ]: 10751 : updateRouteLaneOffsetRange(routeLaneOffset, route);
1918 [ + - ]: 10751 : addRoutePredecessorRelationToNewSegment(lane, laneSegment, route, routeDirectionPositive);
1919 [ + - ]: 10751 : roadSegment.drivableLaneSegments.push_back(laneSegment);
1920 : :
1921 : 10751 : lane::LaneDirection const direction = lane.direction;
1922 [ + + ]: 32253 : for (auto contactLocation : {lane::ContactLocation::LEFT, lane::ContactLocation::RIGHT})
1923 : : {
1924 : 21502 : route::RouteLaneOffset currentRouteLaneOffset = routeLaneOffset;
1925 [ + - ]: 21502 : bool const rightNeighbor = isRightNeighbor(contactLocation, routeDirectionPositive);
1926 [ + - ]: 43004 : lane::ContactLaneList contactLanes = lane::getContactLanes(lane, contactLocation);
1927 : :
1928 : 43004 : lane::LaneIdSet visitedLaneIds;
1929 [ + - ]: 21502 : visitedLaneIds.insert(laneInterval.laneId);
1930 : :
1931 : : // we expect that per map model only one contact lane is possible in one direction
1932 [ + + ]: 35294 : while (contactLanes.size() == 1u)
1933 : : {
1934 : : // in some broken map cases we could end in an infinite loop here, due to broken neighborhood relations
1935 : : // to avoid this issue, add an early return upon detecting the start lane again
1936 : 13797 : lane::LaneId otherLaneId = contactLanes.front().toLane;
1937 : :
1938 [ + - + + ]: 13797 : if (!isLaneRelevantForExpansion(otherLaneId, relevantLanes))
1939 : : {
1940 : : // stop expansion
1941 : 5 : contactLanes.clear();
1942 : 5 : break;
1943 : : }
1944 : :
1945 [ + - ]: 13792 : auto laneNotVisited = visitedLaneIds.insert(otherLaneId);
1946 [ - + ]: 13792 : if (!laneNotVisited.second)
1947 : : {
1948 [ # # # # ]: 0 : access::getLogger()->warn("ad::map::route::appendRoadSegmentToRoute: Broken neighborhood relations detected "
1949 : : "for LaneId {}, Skipping expansion.",
1950 : 0 : laneInterval.laneId);
1951 : 0 : contactLanes.clear();
1952 : 0 : break;
1953 : : }
1954 : :
1955 [ + - ]: 13792 : auto const &otherLane = lane::getLane(otherLaneId);
1956 [ + + ]: 13681 : if (((route.routeCreationMode == RouteCreationMode::SameDrivingDirection) && (direction == otherLane.direction))
1957 [ + + + + ]: 10431 : || ((route.routeCreationMode == RouteCreationMode::AllRoutableLanes) && (lane::isRouteable(otherLane)))
1958 [ + + + + : 27584 : || (route.routeCreationMode == RouteCreationMode::AllNeighborLanes))
+ + ]
1959 : : {
1960 [ + - ]: 3424 : route::LaneSegment newLaneSegment;
1961 : 3424 : newLaneSegment.laneInterval.laneId = otherLaneId;
1962 : 3424 : newLaneSegment.laneInterval.start = laneInterval.start;
1963 : 3424 : newLaneSegment.laneInterval.end = laneInterval.end;
1964 [ + + ]: 3424 : if (direction == otherLane.direction)
1965 : : {
1966 : 3363 : newLaneSegment.laneInterval.wrongWay = laneInterval.wrongWay;
1967 : : }
1968 : : else
1969 : : {
1970 : 61 : newLaneSegment.laneInterval.wrongWay = !laneInterval.wrongWay;
1971 : : }
1972 [ + - ]: 3424 : updateRouteLaneOffset(rightNeighbor, currentRouteLaneOffset, route);
1973 : 3424 : newLaneSegment.routeLaneOffset = currentRouteLaneOffset;
1974 : :
1975 [ + - ]: 3424 : roadSegment.boundingSphere = roadSegment.boundingSphere + otherLane.boundingSphere;
1976 : :
1977 [ + - ]: 3424 : addRoutePredecessorRelationToNewSegment(otherLane, newLaneSegment, route, routeDirectionPositive);
1978 : :
1979 : : // sorting: right lanes are added at front, left lanes at back
1980 [ + + ]: 3424 : if (rightNeighbor)
1981 : : {
1982 : 1497 : roadSegment.drivableLaneSegments.front().rightNeighbor = newLaneSegment.laneInterval.laneId;
1983 : 1497 : newLaneSegment.leftNeighbor = roadSegment.drivableLaneSegments.front().laneInterval.laneId;
1984 [ + - ]: 1497 : roadSegment.drivableLaneSegments.insert(roadSegment.drivableLaneSegments.begin(), newLaneSegment);
1985 : : }
1986 : : else
1987 : : {
1988 : 1927 : roadSegment.drivableLaneSegments.back().leftNeighbor = newLaneSegment.laneInterval.laneId;
1989 : 1927 : newLaneSegment.rightNeighbor = roadSegment.drivableLaneSegments.back().laneInterval.laneId;
1990 [ + - ]: 1927 : roadSegment.drivableLaneSegments.insert(roadSegment.drivableLaneSegments.end(), newLaneSegment);
1991 : : }
1992 : :
1993 : : // go aside recursively
1994 [ + - ]: 3424 : contactLanes = lane::getContactLanes(otherLane, contactLocation);
1995 : : }
1996 : : else
1997 : : {
1998 : 10368 : contactLanes.clear();
1999 : : }
2000 : : }
2001 [ - + ]: 21502 : if (!contactLanes.empty())
2002 : : {
2003 : : throw std::runtime_error(
2004 [ # # ]: 0 : "AdRoadNetworkAdm::fillRoadSegment algorithm is not able to handle multiple left/right contact lanes");
2005 : : }
2006 : : }
2007 [ + - ]: 10751 : route.roadSegments.push_back(roadSegment);
2008 : 10751 : }
2009 : :
2010 : 3 : physics::Distance addOpposingLaneSegmentToRoadSegment(point::ParaPoint const &startpoint,
2011 : : physics::Distance const &distance,
2012 : : route::RoadSegment &roadSegment,
2013 : : route::FullRoute &route)
2014 : : {
2015 [ - + ]: 3 : if (roadSegment.drivableLaneSegments.empty())
2016 : : {
2017 : 0 : return physics::Distance(-1.);
2018 : : }
2019 : :
2020 [ + - ]: 3 : route::LaneInterval laneInterval;
2021 : 3 : laneInterval.laneId = startpoint.laneId;
2022 : 3 : laneInterval.start = startpoint.parametricOffset;
2023 : 3 : laneInterval.end = roadSegment.drivableLaneSegments[0].laneInterval.end;
2024 : :
2025 : : // the lane segment we want to add is opposing the current road segment
2026 : : // for right-hand traffic this means, the lane segment is left of the left-most lane segment
2027 : : // for left-hand traffic this is the segment right of the right-most lane segment
2028 : 3 : LaneSegmentList::iterator neighborIterator;
2029 [ + - + - ]: 3 : if (!access::isLeftHandedTraffic())
2030 : : {
2031 : : // sorting: right lanes are added at front, left lanes at back
2032 : 3 : neighborIterator = roadSegment.drivableLaneSegments.end() - 1;
2033 : : }
2034 : : else
2035 : : {
2036 : 0 : neighborIterator = roadSegment.drivableLaneSegments.begin();
2037 : : }
2038 : 3 : laneInterval.end = neighborIterator->laneInterval.end;
2039 : :
2040 : : // let's ensure that the lane segment is neighbor of the provided road segment
2041 [ + - ]: 3 : auto const neighborhood = getDirectNeighborhoodRelation(laneInterval.laneId, neighborIterator->laneInterval.laneId);
2042 [ + - + + ]: 3 : if ((neighborhood != lane::ContactLocation::LEFT) && (neighborhood != lane::ContactLocation::RIGHT))
2043 : : {
2044 : 2 : return physics::Distance(-1.);
2045 : : }
2046 : :
2047 : : // determine wrong way flag
2048 [ + - ]: 1 : if (lane::isLaneDirectionPositive(neighborIterator->laneInterval.laneId)
2049 [ + - - + ]: 1 : == lane::isLaneDirectionPositive(laneInterval.laneId))
2050 : : {
2051 : 0 : laneInterval.wrongWay = neighborIterator->laneInterval.wrongWay;
2052 : : }
2053 : : else
2054 : : {
2055 : 1 : laneInterval.wrongWay = !neighborIterator->laneInterval.wrongWay;
2056 : : }
2057 : :
2058 [ + - ]: 1 : laneInterval = route::restrictIntervalFromBegin(laneInterval, distance);
2059 : :
2060 [ + - ]: 2 : route::LaneSegment laneSegment;
2061 : 1 : laneSegment.laneInterval = laneInterval;
2062 : 1 : laneSegment.routeLaneOffset = neighborIterator->routeLaneOffset;
2063 : :
2064 [ + - + - ]: 1 : if (!access::isLeftHandedTraffic())
2065 : : {
2066 : 1 : laneSegment.rightNeighbor = neighborIterator->laneInterval.laneId;
2067 : 1 : neighborIterator->leftNeighbor = laneInterval.laneId;
2068 [ + - ]: 1 : updateRouteLaneOffset(false, laneSegment.routeLaneOffset, route);
2069 [ + - ]: 1 : roadSegment.drivableLaneSegments.push_back(laneSegment);
2070 : : }
2071 : : else
2072 : : {
2073 : 0 : laneSegment.leftNeighbor = neighborIterator->laneInterval.laneId;
2074 : 0 : neighborIterator->rightNeighbor = laneInterval.laneId;
2075 [ # # ]: 0 : updateRouteLaneOffset(true, laneSegment.routeLaneOffset, route);
2076 [ # # ]: 0 : roadSegment.drivableLaneSegments.insert(roadSegment.drivableLaneSegments.begin(), laneSegment);
2077 : : }
2078 : :
2079 [ + - ]: 1 : return route::calcLength(laneInterval);
2080 : : }
2081 : :
2082 : 3 : bool addOpposingLaneToRoute(point::ParaPoint const &pointOnOppositeLane,
2083 : : physics::Distance const &distanceOnWrongLane,
2084 : : route::FullRoute &route,
2085 : : physics::Distance &coveredDistance)
2086 : : {
2087 : 3 : coveredDistance = physics::Distance(0.);
2088 : 3 : uint32_t segmentCounter = 0;
2089 : 3 : point::ParaPoint startPoint = pointOnOppositeLane;
2090 : :
2091 [ - + ]: 3 : if (route.roadSegments.empty())
2092 : : {
2093 : 0 : return false;
2094 : : }
2095 : :
2096 [ + - ]: 3 : auto startWayPoint = findWaypoint(startPoint, route);
2097 [ + + ]: 3 : if (startWayPoint.isValid())
2098 : : {
2099 : 2 : if (std::find_if(route.roadSegments[0].drivableLaneSegments.begin(),
2100 : 2 : route.roadSegments[0].drivableLaneSegments.end(),
2101 [ + - ]: 7 : [&startPoint](const route::LaneSegment &l) { return l.laneInterval.laneId == startPoint.laneId; })
2102 [ + + ]: 4 : != route.roadSegments[0].drivableLaneSegments.end())
2103 : : {
2104 : : // point is already on route
2105 : 1 : return false;
2106 : : }
2107 : : }
2108 : :
2109 [ + - + - ]: 2 : while (coveredDistance < distanceOnWrongLane)
2110 : : {
2111 : : auto segmentDistance = route::addOpposingLaneSegmentToRoadSegment(
2112 [ + - ]: 2 : startPoint, distanceOnWrongLane, route.roadSegments[segmentCounter], route);
2113 : :
2114 [ + - + + ]: 2 : if (segmentDistance < physics::Distance(0.))
2115 : : {
2116 : 1 : return false;
2117 : : }
2118 : :
2119 [ + - ]: 1 : coveredDistance += segmentDistance;
2120 : :
2121 [ + - + - ]: 1 : if (coveredDistance < distanceOnWrongLane)
2122 : : {
2123 : 1 : lane::ContactLaneList successors;
2124 [ + - - + ]: 1 : if (route::isRouteDirectionNegative(route.roadSegments[segmentCounter].drivableLaneSegments[0].laneInterval))
2125 : : {
2126 : : // get predecessor, if there are multiple its an intersection so stop
2127 [ # # # # ]: 0 : successors = lane::getContactLanes(lane::getLane(startPoint.laneId), lane::ContactLocation::PREDECESSOR);
2128 : : }
2129 : : else
2130 : : {
2131 : : // get successor, if there are multiple its an intersection so stop
2132 [ + - + - ]: 1 : successors = lane::getContactLanes(lane::getLane(startPoint.laneId), lane::ContactLocation::SUCCESSOR);
2133 : : }
2134 : :
2135 : 1 : segmentCounter++;
2136 : :
2137 [ + - - + : 1 : if ((successors.size() == 0) || (successors.size() > 1) || (segmentCounter == route.roadSegments.size()))
- - + - ]
2138 : : {
2139 : 1 : break;
2140 : : }
2141 : :
2142 : 0 : startPoint.laneId = successors[0].toLane;
2143 : 0 : startPoint.parametricOffset = route.roadSegments[segmentCounter].drivableLaneSegments[0].laneInterval.start;
2144 : :
2145 [ # # # # ]: 0 : if (intersection::Intersection::isLanePartOfAnIntersection(startPoint.laneId))
2146 : : {
2147 : 0 : break;
2148 : : }
2149 : : }
2150 : : }
2151 : :
2152 : 1 : return true;
2153 : : }
2154 : :
2155 : 1 : route::FullRoute getRouteExpandedToOppositeLanes(route::FullRoute const &route)
2156 : : {
2157 : 1 : return getRouteExpandedTo(route, RouteCreationMode::AllRoutableLanes);
2158 : : }
2159 : :
2160 : 1 : route::FullRoute getRouteExpandedToAllNeighborLanes(route::FullRoute const &route)
2161 : : {
2162 : 1 : return getRouteExpandedTo(route, RouteCreationMode::AllNeighborLanes);
2163 : : }
2164 : :
2165 : 1 : bool calculateBypassingRoute(route::FullRoute const &route, route::FullRoute &bypassingRoute)
2166 : : {
2167 : 1 : bypassingRoute = route::FullRoute();
2168 : :
2169 [ + + ]: 3 : for (const auto &segment : route.roadSegments)
2170 : : {
2171 [ - + ]: 2 : if (segment.drivableLaneSegments.empty())
2172 : : {
2173 : : // this should not happen
2174 : : // better avoid to passing
2175 : 0 : return false;
2176 : : }
2177 : :
2178 : : // select the most outer relevant lane segment
2179 : : // that is for left-hand traffic the right most segment
2180 : : // that is for right-hand traffic the left most segment
2181 [ + - ]: 2 : LaneSegment laneSegment;
2182 [ + - - + ]: 2 : if (access::isLeftHandedTraffic())
2183 : : {
2184 [ # # ]: 0 : laneSegment = segment.drivableLaneSegments.front();
2185 : : }
2186 : : else
2187 : : {
2188 [ + - ]: 2 : laneSegment = segment.drivableLaneSegments.back();
2189 : : }
2190 : :
2191 : : // depending on the lane orientation and the driving direction (left-hand vs. right-hand)
2192 : : // the bypassingRoute has to use the left or right neighbor segments
2193 [ + - ]: 2 : bool useLeftNeighbor = isRouteDirectionPositive(laneSegment.laneInterval);
2194 [ + - ]: 2 : useLeftNeighbor = useLeftNeighbor ^ access::isLeftHandedTraffic();
2195 : :
2196 [ + - + - ]: 2 : auto lane = lane::getLane(laneSegment.laneInterval.laneId);
2197 : 2 : lane::ContactLaneList contactLanes;
2198 [ - + ]: 2 : if (useLeftNeighbor)
2199 : : {
2200 [ # # ]: 0 : contactLanes = lane::getContactLanes(lane, lane::ContactLocation::LEFT);
2201 : : }
2202 : : else
2203 : : {
2204 [ + - ]: 2 : contactLanes = lane::getContactLanes(lane, lane::ContactLocation::RIGHT);
2205 : : }
2206 : :
2207 [ - + ]: 2 : if (contactLanes.empty())
2208 : : {
2209 : : // there are no neighbors, thus no bypass option
2210 : 0 : return false;
2211 : : }
2212 : :
2213 : 2 : auto neighborLaneId = contactLanes.front().toLane;
2214 [ + - + - ]: 2 : auto neighborLane = lane::getLane(neighborLaneId);
2215 [ - + ]: 2 : if (lane::isLanePartOfAnIntersection(neighborLane))
2216 : : {
2217 : 0 : return false;
2218 : : }
2219 : :
2220 : 2 : route::LaneInterval neighborLaneInterval = laneSegment.laneInterval;
2221 : 2 : neighborLaneInterval.laneId = neighborLaneId;
2222 [ + - ]: 2 : if (lane.direction != neighborLane.direction)
2223 : : {
2224 : 2 : neighborLaneInterval.wrongWay = !neighborLaneInterval.wrongWay;
2225 : : }
2226 [ + - ]: 2 : route::appendLaneSegmentToRoute(neighborLaneInterval, bypassingRoute);
2227 : : }
2228 : :
2229 [ + + ]: 3 : for (size_t i = 0; i < bypassingRoute.roadSegments.size(); ++i)
2230 : : {
2231 : 2 : bypassingRoute.roadSegments[i].segmentCountFromDestination = bypassingRoute.roadSegments.size() - i;
2232 : : }
2233 : :
2234 : 1 : return true;
2235 : : }
2236 : :
2237 : : // let's keep this for the moment like this with the internal inline function
2238 : : // we will have to rework the border calculation in each case
2239 : : // - add TRange instead of TParam...
2240 : : // - consider the laneIntervals directly instead of the whole segment
2241 : : inline LaneInterval
2242 : 6 : cutLaneIntervalAtEndByRoadSegmentParametricOffset(LaneInterval const &interval,
2243 : : physics::ParametricValue const segmentParametricOffset)
2244 : : {
2245 : 6 : physics::ParametricValue laneParametricOffset = segmentParametricOffset;
2246 [ + - - + ]: 6 : if (isRouteDirectionNegative(interval))
2247 : : {
2248 [ # # ]: 0 : laneParametricOffset = physics::ParametricValue(1.) - segmentParametricOffset;
2249 : : }
2250 [ + - ]: 12 : return cutIntervalAtEnd(interval, laneParametricOffset);
2251 : : }
2252 : :
2253 : : template <typename BorderType>
2254 : 3 : void getBorderOfRoadSegment(RoadSegment const &roadSegment,
2255 : : BorderType &border,
2256 : : physics::ParametricValue const parametricOffset)
2257 : : {
2258 [ + - ]: 3 : if (!roadSegment.drivableLaneSegments.empty())
2259 : : {
2260 [ + - ]: 3 : getRightProjectedEdge(cutLaneIntervalAtEndByRoadSegmentParametricOffset(
2261 : 3 : roadSegment.drivableLaneSegments.front().laneInterval, parametricOffset),
2262 : 3 : border.right);
2263 [ + - ]: 3 : getLeftProjectedEdge(cutLaneIntervalAtEndByRoadSegmentParametricOffset(
2264 : 3 : roadSegment.drivableLaneSegments.back().laneInterval, parametricOffset),
2265 : 3 : border.left);
2266 : : }
2267 : 3 : }
2268 : :
2269 : 1 : lane::ECEFBorder getECEFBorderOfRoadSegment(RoadSegment const &roadSegment,
2270 : : physics::ParametricValue const parametricOffset)
2271 : : {
2272 : 1 : lane::ECEFBorder result;
2273 [ + - ]: 1 : getBorderOfRoadSegment(roadSegment, result, parametricOffset);
2274 : 1 : return result;
2275 : : }
2276 : :
2277 : 1 : lane::ENUBorder getENUBorderOfRoadSegment(RoadSegment const &roadSegment,
2278 : : physics::ParametricValue const parametricOffset)
2279 : : {
2280 : 1 : lane::ENUBorder result;
2281 [ + - ]: 1 : getBorderOfRoadSegment(roadSegment, result, parametricOffset);
2282 : 1 : return result;
2283 : : }
2284 : :
2285 : 1 : lane::GeoBorder getGeoBorderOfRoadSegment(RoadSegment const &roadSegment,
2286 : : physics::ParametricValue const parametricOffset)
2287 : : {
2288 : 1 : lane::GeoBorder result;
2289 [ + - ]: 1 : getBorderOfRoadSegment(roadSegment, result, parametricOffset);
2290 : 1 : return result;
2291 : : }
2292 : :
2293 : 0 : lane::ENUBorderList getENUBorderOfRoute(FullRoute const &route)
2294 : : {
2295 : 0 : lane::ENUBorderList enuBorderList;
2296 [ # # ]: 0 : enuBorderList.reserve(route.roadSegments.size());
2297 [ # # ]: 0 : for (auto const &roadSegment : route.roadSegments)
2298 : : {
2299 [ # # # # ]: 0 : enuBorderList.push_back(getENUBorderOfRoadSegment(roadSegment));
2300 : : }
2301 : 0 : return enuBorderList;
2302 : : }
2303 : :
2304 : 0 : lane::ECEFBorderList getECEFBorderOfRoute(FullRoute const &route)
2305 : : {
2306 : 0 : lane::ECEFBorderList ecefBorderList;
2307 [ # # ]: 0 : ecefBorderList.reserve(route.roadSegments.size());
2308 [ # # ]: 0 : for (auto const &roadSegment : route.roadSegments)
2309 : : {
2310 [ # # # # ]: 0 : ecefBorderList.push_back(getECEFBorderOfRoadSegment(roadSegment));
2311 : : }
2312 : 0 : return ecefBorderList;
2313 : : }
2314 : :
2315 : 0 : lane::GeoBorderList getGeoBorderOfRoute(FullRoute const &route)
2316 : : {
2317 : 0 : lane::GeoBorderList geoBorderList;
2318 [ # # ]: 0 : geoBorderList.reserve(route.roadSegments.size());
2319 [ # # ]: 0 : for (auto const &roadSegment : route.roadSegments)
2320 : : {
2321 [ # # # # ]: 0 : geoBorderList.push_back(getGeoBorderOfRoadSegment(roadSegment));
2322 : : }
2323 : 0 : return geoBorderList;
2324 : : }
2325 : :
2326 : 0 : point::ENUHeading getENUHeadingOfRoute(match::Object const &object, FullRoute const &route)
2327 : : {
2328 [ # # ]: 0 : FindWaypointResult objectCenterWaypoint = findCenterWaypoint(object, route);
2329 [ # # ]: 0 : if (!objectCenterWaypoint.isValid())
2330 : : {
2331 [ # # # # ]: 0 : access::getLogger()->error("ad::map::route::getENUHeadingOfRoute: object not found in route {} {}", object, route);
2332 [ # # ]: 0 : throw std::runtime_error("route::getENUHeadingOfRoute>> object not found in route");
2333 : : }
2334 : : // ensure the object center point is in
2335 : : auto const routeSectionAroundObject = getRouteSection(objectCenterWaypoint,
2336 : 0 : object.enuPosition.dimension.length,
2337 : 0 : object.enuPosition.dimension.length,
2338 : : route,
2339 [ # # ]: 0 : RouteSectionCreationMode::AllRouteLanes);
2340 [ # # ]: 0 : auto const routeENUBorders = getENUBorderOfRoute(routeSectionAroundObject);
2341 [ # # ]: 0 : return lane::getENUHeading(routeENUBorders, object.enuPosition.centerPoint);
2342 : : }
2343 : :
2344 : 0 : bool isObjectHeadingInRouteDirection(match::Object const &object, FullRoute const &route)
2345 : : {
2346 [ # # ]: 0 : auto const enuHeadingOfRoute = getENUHeadingOfRoute(object, route);
2347 : : // enforce normalization of angle difference
2348 : : const auto headingDifference
2349 [ # # # # ]: 0 : = point::createENUHeading(static_cast<double>(std::fabs(enuHeadingOfRoute - object.enuPosition.heading)));
2350 [ # # ]: 0 : if (static_cast<double>(std::fabs(headingDifference)) > M_PI / 2.)
2351 : : {
2352 : 0 : return false;
2353 : : }
2354 : 0 : return true;
2355 : : }
2356 : :
2357 : : } // namespace route
2358 : : } // namespace map
2359 : : } // namespace ad
|
