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/Planning.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include "RouteOperationPrivate.hpp"
13 : : #include "ad/map/access/Logging.hpp"
14 : : #include "ad/map/access/Operation.hpp"
15 : : #include "ad/map/lane/BorderOperation.hpp"
16 : : #include "ad/map/lane/LaneOperation.hpp"
17 : : #include "ad/map/match/AdMapMatching.hpp"
18 : : #include "ad/map/match/MapMatchedOperation.hpp"
19 : : #include "ad/map/route/RouteAStar.hpp"
20 : : #include "ad/map/route/RouteOperation.hpp"
21 : : #include "ad/map/route/RoutePrediction.hpp"
22 : :
23 : : namespace ad {
24 : : namespace map {
25 : : namespace route {
26 : : namespace planning {
27 : :
28 : 2873 : void updateRoutePlanningCounters(route::FullRoute &route)
29 : : {
30 : : static RoutePlanningCounter routePlanningCounter = 0u;
31 : :
32 : : // post process the route counters
33 : 2873 : routePlanningCounter++;
34 : 2873 : route.routePlanningCounter = routePlanningCounter;
35 : 2873 : route.fullRouteSegmentCount = route.roadSegments.size();
36 [ + + ]: 13624 : for (size_t i = 0; i < route.roadSegments.size(); ++i)
37 : : {
38 : 10751 : route.roadSegments[i].segmentCountFromDestination = route.fullRouteSegmentCount - i;
39 : : }
40 : 2873 : }
41 : :
42 : 31 : RoutingParaPoint createRoutingPoint(lane::LaneId const &laneId,
43 : : physics::ParametricValue const ¶metricOffset,
44 : : RoutingDirection const &routingDirection)
45 : : {
46 : 31 : RoutingParaPoint routingPoint;
47 : 31 : routingPoint.point = point::createParaPoint(laneId, parametricOffset);
48 : 31 : routingPoint.direction = routingDirection;
49 : 31 : return routingPoint;
50 : : }
51 : :
52 : 5431 : RoutingParaPoint createRoutingPoint(point::ParaPoint const ¶Point, RoutingDirection const &routingDirection)
53 : : {
54 : 5431 : RoutingParaPoint routingPoint;
55 : 5431 : routingPoint.point = paraPoint;
56 : 5431 : routingPoint.direction = routingDirection;
57 : 5431 : return routingPoint;
58 : : }
59 : :
60 : 42 : RoutingParaPoint createRoutingPoint(match::LaneOccupiedRegion const &occupiedRegion,
61 : : RoutingDirection const &routingDirection)
62 : : {
63 [ + - ]: 42 : point::ParaPoint paraPoint;
64 : 42 : paraPoint.laneId = occupiedRegion.laneId;
65 : : // the current lane direction has no impact if the routing direction is explicitly set
66 : : // since routing will select neighbor lanes that might have different direction to be able to expand
67 [ + + ]: 42 : if (routingDirection == RoutingDirection::POSITIVE)
68 : : {
69 : 13 : paraPoint.parametricOffset = occupiedRegion.longitudinalRange.minimum;
70 : : }
71 [ + + ]: 29 : else if (routingDirection == RoutingDirection::NEGATIVE)
72 : : {
73 : 5 : paraPoint.parametricOffset = occupiedRegion.longitudinalRange.maximum;
74 : : }
75 : : else
76 : : {
77 : : // otherwise we don't know the actual routing direction and take the lane direction
78 : : // into account
79 [ + - ]: 24 : bool const laneDirectionPositive = lane::isLaneDirectionPositive(paraPoint.laneId);
80 : 24 : bool const routingDirectionPositive = (routingDirection != RoutingDirection::NEGATIVE);
81 [ + + ]: 24 : if (laneDirectionPositive ^ routingDirectionPositive)
82 : : {
83 : : // both different
84 : 4 : paraPoint.parametricOffset = occupiedRegion.longitudinalRange.maximum;
85 : : }
86 : : else
87 : : {
88 : : // both positive or both negative
89 : 20 : paraPoint.parametricOffset = occupiedRegion.longitudinalRange.minimum;
90 : : }
91 : : }
92 : :
93 [ + - ]: 84 : return createRoutingPoint(paraPoint, routingDirection);
94 : : }
95 : :
96 : 22 : RoutingDirection getNominalRoutingDirection(point::ParaPoint const ¶Point, point::ENUHeading const &heading)
97 : : {
98 : : RoutingDirection routingDirection;
99 [ + + ]: 22 : if (lane::isHeadingInLaneDirection(paraPoint, heading))
100 : : {
101 [ + + ]: 19 : if (lane::isLaneDirectionPositive(paraPoint.laneId))
102 : : {
103 : 14 : routingDirection = RoutingDirection::POSITIVE;
104 : : }
105 : : else
106 : : {
107 : 5 : routingDirection = RoutingDirection::NEGATIVE;
108 : : }
109 : : }
110 : : else
111 : : {
112 [ + + ]: 3 : if (lane::isLaneDirectionPositive(paraPoint.laneId))
113 : : {
114 : 2 : routingDirection = RoutingDirection::NEGATIVE;
115 : : }
116 : : else
117 : : {
118 : 1 : routingDirection = RoutingDirection::POSITIVE;
119 : : }
120 : : }
121 : 22 : return routingDirection;
122 : : }
123 : :
124 : 5 : RoutingParaPoint createRoutingPoint(point::ParaPoint const ¶Point, point::ENUHeading const &heading)
125 : : {
126 [ + - ]: 10 : return createRoutingPoint(paraPoint, getNominalRoutingDirection(paraPoint, heading));
127 : : }
128 : :
129 : 17 : RoutingParaPoint createRoutingPoint(match::LaneOccupiedRegion const &occupiedRegion, point::ENUHeading const &heading)
130 : : {
131 : : return createRoutingPoint(occupiedRegion,
132 [ + - + - ]: 34 : getNominalRoutingDirection(match::getCenterParaPoint(occupiedRegion), heading));
133 : : }
134 : :
135 : 10 : void addParaPointToRouteDestList(point::ParaPoint const ¶Point, std::vector<RoutingParaPoint> &routingDestList)
136 : : {
137 [ + + ]: 10 : if (routingDestList.empty())
138 : : {
139 [ + - + - ]: 6 : routingDestList.push_back(createRoutingPoint(paraPoint));
140 : 6 : return;
141 : : }
142 : :
143 [ + - + - ]: 4 : if (routingDestList.back().point.laneId != paraPoint.laneId)
144 : : {
145 [ + - + - ]: 4 : routingDestList.push_back(createRoutingPoint(paraPoint));
146 : 4 : return;
147 : : }
148 : :
149 : : // the last point in the destination list is on the same lane as the new paraPoint
150 : : // in this case, we need to ensure, that the order of the points is aligned to the
151 : : // driving direction. If this is the case, we *add* the new paraPoint, otherwise, we
152 : : // *replace* the last point with the new paraPoint.
153 : :
154 [ # # # # ]: 0 : auto lane = lane::getLane(paraPoint.laneId);
155 : :
156 : : // positive lane direction means increasing TParam
157 [ # # ]: 0 : if (lane.direction == lane::LaneDirection::POSITIVE)
158 : : {
159 [ # # # # ]: 0 : if (routingDestList.back().point.parametricOffset < paraPoint.parametricOffset)
160 : : {
161 [ # # # # ]: 0 : routingDestList.push_back(createRoutingPoint(paraPoint));
162 : 0 : return;
163 : : }
164 : : }
165 : :
166 : : // negative lane direction means decreasing TParam
167 [ # # ]: 0 : if (lane.direction == lane::LaneDirection::NEGATIVE)
168 : : {
169 [ # # # # ]: 0 : if (routingDestList.back().point.parametricOffset > paraPoint.parametricOffset)
170 : : {
171 [ # # # # ]: 0 : routingDestList.push_back(createRoutingPoint(paraPoint));
172 : 0 : return;
173 : : }
174 : : }
175 : :
176 : : // in all other cases, we replace the last point with the new paraPoint
177 [ # # ]: 0 : routingDestList.back() = createRoutingPoint(paraPoint);
178 : 0 : return;
179 : : }
180 : :
181 : 2864 : FullRoute createFullRoute(const planning::Route::RawRoute &rawRoute,
182 : : RouteCreationMode const routeCreationMode,
183 : : lane::LaneIdSet const &relevantLanes)
184 : : {
185 : 2864 : FullRoute resultRoute;
186 : 2864 : resultRoute.routeCreationMode = routeCreationMode;
187 : :
188 : 2864 : RouteLaneOffset currentLaneOffset = 0;
189 [ + + ]: 13582 : for (size_t i = 0u; i < rawRoute.paraPointList.size();)
190 : : {
191 : : // first create a plain route containing no neighboring lanes
192 : 10718 : const point::ParaPoint &intervalStartPoint = rawRoute.paraPointList[i];
193 [ + - ]: 10718 : LaneInterval newInterval;
194 : 10718 : newInterval.laneId = intervalStartPoint.laneId;
195 : 10718 : newInterval.start = intervalStartPoint.parametricOffset;
196 : 10718 : newInterval.end = newInterval.start;
197 : :
198 : : // search the end: the last point of the last neighbor
199 : 10718 : int32_t laneMovesRight = 0;
200 : 10718 : auto nextIntervalContact = lane::ContactLocation::INVALID;
201 [ + + ]: 21914 : for (++i; i < rawRoute.paraPointList.size(); ++i)
202 : : {
203 [ + - ]: 19054 : nextIntervalContact = getDirectNeighborhoodRelation(newInterval.laneId, rawRoute.paraPointList[i].laneId);
204 [ + + ]: 19054 : if ((nextIntervalContact == lane::ContactLocation::OVERLAP)
205 [ + + ]: 8332 : || (nextIntervalContact == lane::ContactLocation::LEFT)
206 [ + + ]: 8057 : || (nextIntervalContact == lane::ContactLocation::RIGHT))
207 : : {
208 : 11196 : newInterval.laneId = rawRoute.paraPointList[i].laneId;
209 : 11196 : newInterval.end = rawRoute.paraPointList[i].parametricOffset;
210 [ + + ]: 11196 : if (nextIntervalContact == lane::ContactLocation::RIGHT)
211 : : {
212 : 199 : laneMovesRight++;
213 : : }
214 [ + + ]: 10997 : else if (nextIntervalContact == lane::ContactLocation::LEFT)
215 : : {
216 : 275 : laneMovesRight--;
217 : : }
218 : : }
219 : : else
220 : : {
221 : : break;
222 : : }
223 : : }
224 : :
225 : : // determine wrong way flag
226 [ + - + - ]: 21436 : auto newLane = lane::getLane(newInterval.laneId);
227 [ + + - + ]: 10718 : if ((newLane.direction != lane::LaneDirection::POSITIVE) && (newLane.direction != lane::LaneDirection::NEGATIVE))
228 : : {
229 : 0 : newInterval.wrongWay = false;
230 : : }
231 : : else
232 : : {
233 : 10718 : auto physicalRouteDirection = lane::LaneDirection::NONE;
234 [ + - + + ]: 10718 : if (newInterval.start < newInterval.end)
235 : : {
236 : 5959 : physicalRouteDirection = lane::LaneDirection::POSITIVE;
237 : : }
238 [ + - + + ]: 4759 : else if (newInterval.start > newInterval.end)
239 : : {
240 : 4687 : physicalRouteDirection = lane::LaneDirection::NEGATIVE;
241 : : }
242 : : else
243 : : {
244 : : // decide by the contact to the next route point
245 [ - + ]: 72 : if (nextIntervalContact == lane::ContactLocation::SUCCESSOR)
246 : : {
247 : 0 : physicalRouteDirection = lane::LaneDirection::POSITIVE;
248 : : }
249 [ - + ]: 72 : else if (nextIntervalContact == lane::ContactLocation::PREDECESSOR)
250 : : {
251 : 0 : physicalRouteDirection = lane::LaneDirection::NEGATIVE;
252 : : }
253 : : // reached the end of the route
254 : : // route only consists of this single degenerated segment
255 [ + + ]: 72 : else if (resultRoute.roadSegments.empty())
256 : : {
257 : : // we actually cannot decide, the route is directionless
258 : : }
259 : : // reached the end of the route
260 : : // since this is degenerated, we know which physical end of the lane we reached
261 [ + - + + ]: 69 : else if (intervalStartPoint.parametricOffset == physics::ParametricValue(0.))
262 : : {
263 : : // entered the lane from the beginning
264 : 64 : physicalRouteDirection = lane::LaneDirection::POSITIVE;
265 : : }
266 [ + - + - ]: 5 : else if (intervalStartPoint.parametricOffset == physics::ParametricValue(1.))
267 : : {
268 : : // entered the lane from the end
269 : 5 : physicalRouteDirection = lane::LaneDirection::NEGATIVE;
270 : : }
271 : : else
272 : : {
273 : : // this should not happen, otherwise we jumped somehow in the middle
274 : : // => directionless
275 : : }
276 : : }
277 : :
278 [ + + ]: 10718 : if (physicalRouteDirection == lane::LaneDirection::NONE)
279 : : {
280 : : // decision is made based on the lane direction difference between
281 : : // interval start lane and this end lane
282 : : // Since in case we changed lanes sidewards the newInterval might
283 : : // have a different routing direction than the intervalStartPoint
284 [ + - + - ]: 3 : auto intervalStartLane = lane::getLane(intervalStartPoint.laneId);
285 : 3 : newInterval.wrongWay = intervalStartLane.direction != newLane.direction;
286 : : }
287 : : else
288 : : {
289 : 10715 : newInterval.wrongWay = physicalRouteDirection != newLane.direction;
290 : : }
291 : : }
292 : :
293 : : // update route lane offset (has to be done after setting wrongWay flag!)
294 [ + + ]: 10718 : if (laneMovesRight != 0)
295 : : {
296 [ + - ]: 470 : bool const rightNeighbor = isRouteDirectionPositive(newInterval) ^ (laneMovesRight < 0);
297 [ + + ]: 942 : for (size_t laneMoves = static_cast<size_t>(std::abs(laneMovesRight)); laneMoves > 0u; laneMoves--)
298 : : {
299 [ + - ]: 472 : updateRouteLaneOffset(rightNeighbor, currentLaneOffset, resultRoute);
300 : : }
301 : : }
302 : :
303 : : // add the new interval
304 [ + - ]: 10718 : appendRoadSegmentToRoute(newInterval, currentLaneOffset, resultRoute, relevantLanes);
305 : : }
306 : 2864 : resultRoute.destinationLaneOffset = currentLaneOffset;
307 : :
308 [ + - ]: 2864 : updateRoutePlanningCounters(resultRoute);
309 : :
310 [ + + ]: 2864 : if (!rawRoute.paraPointList.empty())
311 : : {
312 : : // ensure the route starting points are aligned
313 [ + - ]: 2860 : alignRouteStartingPoints(rawRoute.paraPointList.front(), resultRoute);
314 [ + - ]: 2860 : alignRouteEndingPoints(rawRoute.paraPointList.back(), resultRoute);
315 : : }
316 : :
317 [ + - + - ]: 5728 : access::getLogger()->trace("createFullRoute result {}", resultRoute);
318 : 5728 : return resultRoute;
319 : : }
320 : :
321 : 2702 : FullRoute planRoute(const RoutingParaPoint &routingStart,
322 : : const RoutingParaPoint &routingDest,
323 : : RouteCreationMode const routeCreationMode)
324 : : {
325 [ + - ]: 5404 : RouteAstar routePlanning(routingStart, routingDest, Route::Type::SHORTEST);
326 : 2702 : planning::Route::RawRoute rawRoute;
327 [ + - + + ]: 2702 : if (routePlanning.calculate())
328 : : {
329 [ + - + - ]: 2698 : rawRoute = routePlanning.getRawRoute();
330 : : }
331 [ + - ]: 5404 : return createFullRoute(rawRoute, routeCreationMode, {});
332 : : }
333 : :
334 : 1 : FullRoute planRoute(const match::MapMatchedPositionConfidenceList &mapMatchingResults,
335 : : const RoutingParaPoint &routingStart,
336 : : RouteCreationMode const routeCreationMode)
337 : : {
338 : 1 : FullRoute resultRoute;
339 : 1 : physics::Distance resultDistance = std::numeric_limits<physics::Distance>::max();
340 : :
341 [ + + ]: 3 : for (const auto &mapMatchingResult : mapMatchingResults)
342 : : {
343 : : FullRoute route
344 [ + - + - ]: 4 : = planRoute(routingStart, createRoutingPoint(mapMatchingResult.lanePoint.paraPoint), routeCreationMode);
345 : :
346 [ + - ]: 2 : if (route.roadSegments.size() > 0)
347 : : {
348 [ + - ]: 2 : physics::Distance const routeDistance = calcLength(route);
349 : :
350 [ + - + + ]: 2 : if (routeDistance < resultDistance)
351 : : {
352 : 1 : resultDistance = routeDistance;
353 [ + - ]: 1 : resultRoute = route;
354 : : }
355 : : }
356 : : }
357 : 2 : return resultRoute;
358 : : }
359 : :
360 : : template <typename T>
361 : 1 : FullRoute planRoutePoint(const RoutingParaPoint &routingStart, const T &dest, RouteCreationMode const routeCreationMode)
362 : : {
363 [ + - ]: 2 : match::AdMapMatching mapMatching;
364 [ + - ]: 2 : auto mapMatchingResults = mapMatching.getMapMatchedPositions(dest, physics::Distance(1.), physics::Probability(.05));
365 : :
366 [ + - ]: 2 : return planRoute(mapMatchingResults, routingStart, routeCreationMode);
367 : : }
368 : :
369 : : FullRoute
370 : 1 : planRoute(const RoutingParaPoint &routingStart, const point::GeoPoint &dest, RouteCreationMode const routeCreationMode)
371 : : {
372 : 1 : return planRoutePoint(routingStart, dest, routeCreationMode);
373 : : }
374 : :
375 : : FullRoute
376 : 0 : planRoute(const RoutingParaPoint &routingStart, const point::ENUPoint &dest, RouteCreationMode const routeCreationMode)
377 : : {
378 : 0 : return planRoutePoint(routingStart, dest, routeCreationMode);
379 : : }
380 : :
381 : : template <typename T>
382 : : FullRoute
383 : 6 : planRouteVector(const RoutingParaPoint &start, const std::vector<T> &dest, RouteCreationMode const routeCreationMode)
384 : : {
385 [ + - ]: 12 : match::AdMapMatching mapMatching;
386 : 12 : std::vector<RoutingParaPoint> routingDestList;
387 [ + + ]: 16 : for (auto destIter = dest.begin(); destIter != dest.end(); destIter++)
388 : : {
389 [ + - ]: 20 : auto mapMatchingResults
390 : 10 : = mapMatching.getMapMatchedPositions(*destIter, physics::Distance(1.), physics::Probability(.05));
391 : :
392 [ + + ]: 10 : if ((destIter + 1) == dest.end())
393 : : {
394 [ + - ]: 6 : if (mapMatchingResults.size() >= 1u)
395 : : {
396 [ + - ]: 6 : addParaPointToRouteDestList(mapMatchingResults[0].lanePoint.paraPoint, routingDestList);
397 : : }
398 : : else
399 : : {
400 [ # # # # ]: 0 : access::getLogger()->error("planRoute failed to locate final destination in the map {}", *destIter);
401 : 0 : return FullRoute();
402 : : }
403 : : }
404 [ + - ]: 4 : else if (mapMatchingResults.size() == 1u)
405 : : {
406 [ + - ]: 4 : addParaPointToRouteDestList(mapMatchingResults[0].lanePoint.paraPoint, routingDestList);
407 : : }
408 [ # # ]: 0 : else if (mapMatchingResults.size() == 0u)
409 : : {
410 [ # # # # ]: 0 : access::getLogger()->trace("planRoute ignoring intermediate point not located in the map {}", *destIter);
411 : : }
412 : : else
413 : : {
414 [ # # # # ]: 0 : access::getLogger()->trace("planRoute ignoring ambiguous intermediate point {}", *destIter);
415 : : }
416 : : }
417 [ + - ]: 6 : return planRoute(start, routingDestList, routeCreationMode);
418 : : }
419 : :
420 : 6 : FullRoute planRoute(const RoutingParaPoint &start,
421 : : const std::vector<point::GeoPoint> &dest,
422 : : RouteCreationMode const routeCreationMode)
423 : : {
424 : 6 : return planRouteVector(start, dest, routeCreationMode);
425 : : }
426 : :
427 : 0 : FullRoute planRoute(const RoutingParaPoint &start,
428 : : const std::vector<point::ENUPoint> &dest,
429 : : RouteCreationMode const routeCreationMode)
430 : : {
431 : 0 : return planRouteVector(start, dest, routeCreationMode);
432 : : }
433 : :
434 : 6 : FullRoute planRoute(const RoutingParaPoint &start,
435 : : std::vector<RoutingParaPoint> const &dest,
436 : : RouteCreationMode const routeCreationMode)
437 : : {
438 : 6 : auto routingStart = start;
439 : 12 : planning::Route::RawRoute mergedRawRoute;
440 [ + + ]: 13 : for (auto &routingDest : dest)
441 : : {
442 [ + - ]: 9 : RouteAstar routePlanning(routingStart, routingDest, Route::Type::SHORTEST);
443 [ + - + + ]: 9 : if (routePlanning.calculate())
444 : : {
445 [ + - + - ]: 7 : auto rawRoute = routePlanning.getRawRoute();
446 : : mergedRawRoute.paraPointList.insert(
447 [ + - ]: 7 : mergedRawRoute.paraPointList.end(), rawRoute.paraPointList.begin(), rawRoute.paraPointList.end());
448 [ + - ]: 7 : mergedRawRoute.routeDistance += rawRoute.routeDistance;
449 [ + - ]: 7 : mergedRawRoute.routeDuration += rawRoute.routeDuration;
450 : 7 : routingStart = routePlanning.getRoutingDest();
451 : : }
452 : : else
453 : : {
454 [ + - + - ]: 4 : access::getLogger()->error(
455 : 2 : "planRoute failed to calculate route between {} and {}", routingStart.point, routingDest.point);
456 : 2 : return FullRoute();
457 : : }
458 : : }
459 [ + - ]: 4 : return createFullRoute(mergedRawRoute, routeCreationMode, {});
460 : : }
461 : :
462 : 0 : std::ostream &operator<<(std::ostream &os, CompareRouteResult const &value)
463 : : {
464 [ # # # # : 0 : switch (value)
# ]
465 : : {
466 : 0 : case CompareRouteResult::Equal:
467 : 0 : return os << "CompareRouteResult::Equal";
468 : 0 : case CompareRouteResult::Shorter:
469 : 0 : return os << "CompareRouteResult::Shorter";
470 : 0 : case CompareRouteResult::Longer:
471 : 0 : return os << "CompareRouteResult::Longer";
472 : 0 : case CompareRouteResult::Differ:
473 : 0 : return os << "CompareRouteResult::Differ";
474 : 0 : default:
475 : 0 : return os << "CompareRouteResult::out-of-range";
476 : : }
477 : : }
478 : :
479 : : enum class CheckMode
480 : : {
481 : : Start,
482 : : MiddleAfterStart,
483 : : Middle,
484 : : EndAfterStart,
485 : : End,
486 : : };
487 : :
488 : : CompareRouteResult
489 : 6046 : compareRoadSegmentOnIntervalLevelOrdered(RoadSegment const &left, RoadSegment const &right, CheckMode checkMode)
490 : : {
491 : 6046 : CompareRouteResult equalResult = CompareRouteResult::Equal;
492 [ + + ]: 6046 : if (left.drivableLaneSegments.size() == right.drivableLaneSegments.size())
493 : : {
494 [ + + ]: 15868 : for (std::size_t j = 0u; j < left.drivableLaneSegments.size(); j++)
495 : : {
496 [ + + ]: 10451 : if (left.drivableLaneSegments[j].laneInterval.laneId != right.drivableLaneSegments[j].laneInterval.laneId)
497 : : {
498 : 174 : return CompareRouteResult::Differ;
499 : : }
500 : :
501 : : // don't re-check for start equality directly after start
502 [ + + + + ]: 10277 : if ((checkMode != CheckMode::MiddleAfterStart) && (checkMode != CheckMode::EndAfterStart))
503 : : {
504 : : bool const startEqual
505 : 8944 : = left.drivableLaneSegments[j].laneInterval.start == right.drivableLaneSegments[j].laneInterval.start;
506 : :
507 [ + + ]: 8944 : if (!startEqual)
508 : : {
509 [ - + ]: 1 : if (checkMode != CheckMode::Start)
510 : : {
511 : 0 : return CompareRouteResult::Differ;
512 : : }
513 [ + - ]: 1 : else if (isBeforeInterval(left.drivableLaneSegments[j].laneInterval,
514 : 1 : right.drivableLaneSegments[j].laneInterval.start))
515 : : {
516 : 1 : equalResult = CompareRouteResult::Shorter;
517 : : }
518 : : else
519 : : {
520 : 0 : equalResult = CompareRouteResult::Longer;
521 : : }
522 : : }
523 : : }
524 : :
525 : : // don't check for end equality while searching for start
526 [ + + ]: 10277 : if (checkMode != CheckMode::Start)
527 : : {
528 : : bool const endEqual
529 : 8944 : = left.drivableLaneSegments[j].laneInterval.end == right.drivableLaneSegments[j].laneInterval.end;
530 [ + + ]: 8944 : if (!endEqual)
531 : : {
532 [ + + - + ]: 47 : if ((checkMode != CheckMode::End) && (checkMode != CheckMode::EndAfterStart))
533 : : {
534 : 0 : return CompareRouteResult::Differ;
535 : : }
536 [ + + ]: 47 : else if (isAfterInterval(left.drivableLaneSegments[j].laneInterval,
537 : 47 : right.drivableLaneSegments[j].laneInterval.end))
538 : : {
539 : 21 : equalResult = CompareRouteResult::Shorter;
540 : : }
541 : : else
542 : : {
543 : 26 : equalResult = CompareRouteResult::Longer;
544 : : }
545 : : }
546 : : }
547 : : }
548 : : }
549 : : else
550 : : {
551 : 455 : return CompareRouteResult::Differ;
552 : : }
553 : 5417 : return equalResult;
554 : : }
555 : :
556 : 686 : CompareRouteResult compareRoutesOnIntervalLevel(FullRoute const &left, FullRoute const &right)
557 : : {
558 : : FullRoute const *smallRoute;
559 : : FullRoute const *largeRoute;
560 : : CompareRouteResult equalResult;
561 [ + + ]: 686 : if (left.roadSegments.size() < right.roadSegments.size())
562 : : {
563 : 230 : smallRoute = &left;
564 : 230 : largeRoute = &right;
565 : 230 : equalResult = CompareRouteResult::Shorter;
566 : : }
567 [ + + ]: 456 : else if (left.roadSegments.size() > right.roadSegments.size())
568 : : {
569 : 287 : smallRoute = &right;
570 : 287 : largeRoute = &left;
571 : 287 : equalResult = CompareRouteResult::Longer;
572 : : }
573 : : else
574 : : {
575 : 169 : smallRoute = &left;
576 : 169 : largeRoute = &right;
577 : 169 : equalResult = CompareRouteResult::Equal;
578 : : }
579 : :
580 : : // handle empty routes
581 [ - + ]: 686 : if (smallRoute->roadSegments.size() == 0u)
582 : : {
583 [ # # ]: 0 : if (largeRoute->roadSegments.size() == 0u)
584 : : {
585 : 0 : return CompareRouteResult::Equal;
586 : : }
587 : : else
588 : : {
589 : 0 : return CompareRouteResult::Differ;
590 : : }
591 : : }
592 : :
593 : 686 : std::size_t largeRouteIndexOffset = 0u;
594 : 686 : std::size_t const maxIndexOffset = largeRoute->roadSegments.size() - smallRoute->roadSegments.size();
595 : 686 : std::size_t smallRouteIndex = 0u;
596 : 686 : auto checkMode{CheckMode::Start};
597 : : // search for comparison begin
598 [ + + ]: 701 : for (; largeRouteIndexOffset <= maxIndexOffset; largeRouteIndexOffset++)
599 : : {
600 : 686 : auto const compareSegmentResult = compareRoadSegmentOnIntervalLevelOrdered(
601 : 686 : smallRoute->roadSegments[smallRouteIndex], largeRoute->roadSegments[largeRouteIndexOffset], checkMode);
602 [ + + ]: 686 : if (compareSegmentResult != CompareRouteResult::Differ)
603 : : {
604 [ + + ]: 671 : if (equalResult == CompareRouteResult::Equal)
605 : : {
606 : 154 : equalResult = compareSegmentResult;
607 : : }
608 [ - + ]: 517 : else if (compareSegmentResult == CompareRouteResult::Longer)
609 : : {
610 : : // small route is longer!
611 : 0 : return CompareRouteResult::Differ;
612 : : }
613 : 671 : break;
614 : : }
615 : : }
616 : : // no identical start segment found
617 [ + + ]: 686 : if (largeRouteIndexOffset > maxIndexOffset)
618 : : {
619 : 15 : return CompareRouteResult::Differ;
620 : : }
621 : :
622 : : // search for equality over the whole shorter route
623 : :
624 : : // since we checked the start for the current segment already, we check with MiddleAfterStart mode first
625 : 671 : checkMode = CheckMode::MiddleAfterStart;
626 [ + + ]: 5417 : for (; smallRouteIndex < smallRoute->roadSegments.size(); smallRouteIndex++)
627 : : {
628 : 5360 : std::size_t const largeRouteIndex = smallRouteIndex + largeRouteIndexOffset;
629 [ + + ]: 5360 : if (smallRouteIndex + 1u == smallRoute->roadSegments.size())
630 : : {
631 [ + + ]: 57 : if (checkMode == CheckMode::MiddleAfterStart)
632 : : {
633 : 12 : checkMode = CheckMode::EndAfterStart;
634 : : }
635 : : else
636 : : {
637 : 45 : checkMode = CheckMode::End;
638 : : }
639 : : }
640 : :
641 : 5360 : auto const compareSegmentResult = compareRoadSegmentOnIntervalLevelOrdered(
642 : 5360 : smallRoute->roadSegments[smallRouteIndex], largeRoute->roadSegments[largeRouteIndex], checkMode);
643 : :
644 [ + + ]: 5360 : if (compareSegmentResult != CompareRouteResult::Equal)
645 : : {
646 [ + + + + ]: 639 : if ((checkMode != CheckMode::End) && (checkMode != CheckMode::EndAfterStart))
647 : : {
648 : 614 : return CompareRouteResult::Differ;
649 : : }
650 : :
651 : : // last segment allowed to be shorter
652 [ + + ]: 25 : if (equalResult == CompareRouteResult::Equal)
653 : : {
654 : 23 : equalResult = compareSegmentResult;
655 : : }
656 [ - + ]: 2 : else if (compareSegmentResult == CompareRouteResult::Longer)
657 : : {
658 : : // small route is longer!
659 : 0 : return CompareRouteResult::Differ;
660 : : }
661 : : }
662 : : // next rounds we use Middle
663 : 4746 : checkMode = CheckMode::Middle;
664 : : }
665 : :
666 : 57 : return equalResult;
667 : : }
668 : :
669 : 53 : std::vector<FullRoute> filterDuplicatedRoutes(const std::vector<FullRoute> fullRoutes,
670 : : FilterDuplicatesMode const filterMode)
671 : : {
672 : 53 : std::vector<FullRoute> filteredRoutes;
673 [ + + ]: 53 : if (filterMode == FilterDuplicatesMode::Off)
674 : : {
675 [ + - ]: 16 : filteredRoutes = fullRoutes;
676 : : }
677 : : else
678 : : {
679 [ + + ]: 189 : for (auto const &route : fullRoutes)
680 : : {
681 : 152 : bool addRoute = true;
682 [ + + ]: 781 : for (auto &filteredRoute : filteredRoutes)
683 : : {
684 : : // - if both are the same the new one is not added
685 : : // - if one of the routes is a real sub-route of the other the filteringMode decides which to take
686 : : // - otherwise, both are disjunct and the new one might be added to the filtered list
687 [ + - ]: 686 : auto comparisonResult = compareRoutesOnIntervalLevel(route, filteredRoute);
688 [ - + - - ]: 686 : if (((filterMode == FilterDuplicatesMode::OnlyEqual) && (comparisonResult == CompareRouteResult::Equal))
689 [ + - + + ]: 686 : || ((filterMode != FilterDuplicatesMode::OnlyEqual) && (comparisonResult != CompareRouteResult::Differ)))
690 : : {
691 : 57 : addRoute = false;
692 [ + + ]: 57 : if ((filterMode == FilterDuplicatesMode::SubRoutesPreferShorterOnes)
693 [ - + ]: 3 : && (comparisonResult == CompareRouteResult::Shorter))
694 : : {
695 : : // the new route is shorter, and so overwrites the current filtered one
696 [ # # # # ]: 0 : access::getLogger()->trace(
697 : : "filterDuplicatedRoutes: replacing route by shorter one {} -> {}", filteredRoute, route);
698 [ # # ]: 0 : filteredRoute = route;
699 : : }
700 [ + + ]: 57 : else if ((filterMode == FilterDuplicatesMode::SubRoutesPreferLongerOnes)
701 [ + + ]: 54 : && (comparisonResult == CompareRouteResult::Longer))
702 : : {
703 : : // the new route is longer, and so overwrites the current filtered one
704 [ + - + - ]: 34 : access::getLogger()->trace(
705 : : "filterDuplicatedRoutes: replacing route by longer one {} -> {}", filteredRoute, route);
706 [ + - ]: 17 : filteredRoute = route;
707 : : }
708 : : else
709 : : {
710 : : // the new route is either identical or doesn't match the desired filter mode
711 : : // it is just skipped
712 [ + - + - ]: 80 : access::getLogger()->trace("filterDuplicatedRoutes: skipping {} route {}", comparisonResult, route);
713 : : }
714 : : // stop the inner loop on filteredRoutes
715 : 57 : break;
716 : : }
717 : : }
718 : : // new route differs to all filtered ones, so we have to add it
719 [ + + ]: 152 : if (addRoute)
720 : : {
721 [ + - + - ]: 190 : access::getLogger()->trace("filterDuplicatedRoutes: adding route {}", route);
722 [ + - ]: 95 : filteredRoutes.push_back(route);
723 : : }
724 : : }
725 : : }
726 : 53 : return filteredRoutes;
727 : : }
728 : :
729 : 39 : FullRouteList predictRoutesInternal(const RoutingParaPoint &start,
730 : : physics::Distance const &predictionDistance,
731 : : physics::Duration const &predictionDuration,
732 : : RouteCreationMode const routeCreationMode,
733 : : FilterDuplicatesMode const filterMode,
734 : : ::ad::map::lane::LaneIdSet const &relevantLanes,
735 : : Route::Type const routingTyp)
736 : : {
737 : 78 : FullRouteList resultRoutes;
738 [ + - ]: 39 : RoutePrediction routePrediction(start, predictionDistance, predictionDuration, routingTyp);
739 [ + - ]: 39 : routePrediction.setRelevantLanes(relevantLanes);
740 [ + - + - ]: 39 : if (routePrediction.calculate())
741 : : {
742 [ + + ]: 191 : for (auto &rawRoute : routePrediction.getRawRoutes())
743 : : {
744 [ + - + - ]: 152 : resultRoutes.push_back(createFullRoute(rawRoute, routeCreationMode, relevantLanes));
745 : : }
746 : : }
747 [ + - + - ]: 78 : return filterDuplicatedRoutes(resultRoutes, filterMode);
748 : : }
749 : :
750 : 37 : FullRouteList predictRoutes(const RoutingParaPoint &start,
751 : : physics::Distance const &predictionDistance,
752 : : physics::Duration const &predictionDuration,
753 : : RouteCreationMode const routeCreationMode,
754 : : FilterDuplicatesMode const filterMode,
755 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
756 : : {
757 : : return predictRoutesInternal(
758 : 37 : start, predictionDistance, predictionDuration, routeCreationMode, filterMode, relevantLanes, Route::Type::SHORTEST);
759 : : }
760 : :
761 : 2 : std::vector<route::FullRoute> predictRoutesDirectionless(const point::ParaPoint &start,
762 : : physics::Distance const &predictionDistance,
763 : : physics::Duration const &predictionDuration,
764 : : RouteCreationMode const routeCreationMode,
765 : : FilterDuplicatesMode const filterMode,
766 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
767 : : {
768 [ + - ]: 4 : return predictRoutesInternal(createRoutingParaPoint(start.laneId, start.parametricOffset),
769 : : predictionDistance,
770 : : predictionDuration,
771 : : routeCreationMode,
772 : : filterMode,
773 : : relevantLanes,
774 [ + - ]: 4 : Route::Type::SHORTEST_IGNORE_DIRECTION);
775 : : }
776 : :
777 : 14 : FullRouteList predictRoutes(const match::MapMatchedObjectBoundingBox &startObject,
778 : : physics::Distance const &predictionDistance,
779 : : physics::Duration const &predictionDuration,
780 : : RouteCreationMode const routeCreationMode,
781 : : FilterDuplicatesMode const filterMode,
782 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
783 : : {
784 : 14 : FullRouteList resultRoutes;
785 [ + - ]: 14 : auto const enuHeading = match::getObjectENUHeading(startObject);
786 [ + + ]: 30 : for (auto const &startMatchingResult : startObject.laneOccupiedRegions)
787 : : {
788 [ + - ]: 16 : auto const routingStart = createRoutingPoint(startMatchingResult, enuHeading);
789 : : auto const routes = predictRoutes(routingStart,
790 : : predictionDistance,
791 : : predictionDuration,
792 : : routeCreationMode,
793 : : FilterDuplicatesMode::Off,
794 [ + - ]: 16 : relevantLanes);
795 [ + - ]: 16 : resultRoutes.insert(resultRoutes.end(), routes.begin(), routes.end());
796 : : }
797 [ + - + - ]: 28 : return filterDuplicatedRoutes(resultRoutes, filterMode);
798 : : }
799 : :
800 : 1 : FullRouteList predictRoutesOnDuration(const RoutingParaPoint &start,
801 : : physics::Duration const &predictionDuration,
802 : : RouteCreationMode const routeCreationMode,
803 : : FilterDuplicatesMode const filterMode,
804 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
805 : : {
806 : : return predictRoutes(
807 [ + - ]: 2 : start, physics::Distance::getMax(), predictionDuration, routeCreationMode, filterMode, relevantLanes);
808 : : }
809 : :
810 : 1 : FullRouteList predictRoutesOnDuration(const match::MapMatchedObjectBoundingBox &startObject,
811 : : physics::Duration const &predictionDuration,
812 : : RouteCreationMode const routeCreationMode,
813 : : FilterDuplicatesMode const filterMode,
814 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
815 : : {
816 : : return predictRoutes(
817 [ + - ]: 2 : startObject, physics::Distance::getMax(), predictionDuration, routeCreationMode, filterMode, relevantLanes);
818 : : }
819 : :
820 : 19 : FullRouteList predictRoutesOnDistance(const RoutingParaPoint &start,
821 : : physics::Distance const &predictionDistance,
822 : : RouteCreationMode const routeCreationMode,
823 : : FilterDuplicatesMode const filterMode,
824 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
825 : : {
826 : : return predictRoutes(
827 [ + - ]: 38 : start, predictionDistance, physics::Duration::getMax(), routeCreationMode, filterMode, relevantLanes);
828 : : }
829 : :
830 : 1 : FullRouteList predictRoutesOnDistance(const match::MapMatchedObjectBoundingBox &startObject,
831 : : physics::Distance const &predictionDistance,
832 : : RouteCreationMode const routeCreationMode,
833 : : FilterDuplicatesMode const filterMode,
834 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
835 : : {
836 : : return predictRoutes(
837 [ + - ]: 2 : startObject, predictionDistance, physics::Duration::getMax(), routeCreationMode, filterMode, relevantLanes);
838 : : }
839 : :
840 : 38 : bool doRoadSegmentsHaveCommonLanes(route::RoadSegment const &roadSegmentA, route::RoadSegment const &roadSegmentB)
841 : : {
842 : 91 : for (route::LaneSegmentList::const_iterator laneSegmentIterA = roadSegmentA.drivableLaneSegments.begin();
843 [ + + ]: 91 : laneSegmentIterA != roadSegmentA.drivableLaneSegments.end();
844 : 53 : ++laneSegmentIterA)
845 : : {
846 : 121 : for (route::LaneSegmentList::const_iterator laneSegmentIterB = roadSegmentB.drivableLaneSegments.begin();
847 [ + + ]: 121 : laneSegmentIterB != roadSegmentB.drivableLaneSegments.end();
848 : 67 : ++laneSegmentIterB)
849 : : {
850 [ + - + + ]: 68 : if (laneSegmentIterA->laneInterval.laneId == laneSegmentIterB->laneInterval.laneId)
851 : : {
852 : : // found a common lane
853 : 1 : return true;
854 : : }
855 : : }
856 : : }
857 : 37 : return false;
858 : : }
859 : :
860 : : route::RoadSegmentList::iterator
861 : 19 : findRoadSegmentWithCommonLanes(route::RoadSegment const &roadSegment,
862 : : route::RoadSegmentList::iterator roadSegmentSearchIterStart,
863 : : route::RoadSegmentList::iterator roadSegmentSearchIterEnd)
864 : : {
865 [ + + ]: 56 : for (auto iter = roadSegmentSearchIterStart; iter != roadSegmentSearchIterEnd; iter++)
866 : : {
867 [ + - + + ]: 38 : if (doRoadSegmentsHaveCommonLanes(roadSegment, *iter))
868 : : {
869 : 1 : return iter;
870 : : }
871 : : }
872 : 18 : return roadSegmentSearchIterEnd;
873 : : }
874 : :
875 : : ConnectingRoute
876 : 1 : calculateConnectingRouteCreateMergingRouteUpToPositions(route::FullRoute &objectARoute,
877 : : route::RoadSegmentList::iterator roadSegmentIterA,
878 : : route::FullRoute &objectBRoute,
879 : : route::RoadSegmentList::iterator roadSegmentIterB)
880 : : {
881 : 1 : ConnectingRoute resultRoute;
882 : 1 : resultRoute.type = ConnectingRouteType::Merging;
883 : : // cut the route at the beginning of the current road segments
884 : 3 : for (route::LaneSegmentList::iterator laneSegmentIterA = roadSegmentIterA->drivableLaneSegments.begin();
885 [ + + ]: 3 : laneSegmentIterA != roadSegmentIterA->drivableLaneSegments.end();
886 : 2 : ++laneSegmentIterA)
887 : : {
888 : 2 : laneSegmentIterA->laneInterval.end = laneSegmentIterA->laneInterval.start;
889 : 2 : laneSegmentIterA->successors.clear();
890 : : }
891 : 1 : roadSegmentIterA++;
892 [ + - ]: 1 : roadSegmentIterA = objectARoute.roadSegments.erase(roadSegmentIterA, objectARoute.roadSegments.end());
893 [ + - ]: 1 : resultRoute.routeA = objectARoute;
894 : :
895 : 3 : for (route::LaneSegmentList::iterator laneSegmentIterB = roadSegmentIterB->drivableLaneSegments.begin();
896 [ + + ]: 3 : laneSegmentIterB != roadSegmentIterB->drivableLaneSegments.end();
897 : 2 : ++laneSegmentIterB)
898 : : {
899 : 2 : laneSegmentIterB->laneInterval.end = laneSegmentIterB->laneInterval.start;
900 : 2 : laneSegmentIterB->successors.clear();
901 : : }
902 : 1 : roadSegmentIterB++;
903 [ + - ]: 1 : roadSegmentIterB = objectBRoute.roadSegments.erase(roadSegmentIterB, objectBRoute.roadSegments.end());
904 [ + - ]: 1 : resultRoute.routeB = objectBRoute;
905 : 1 : return resultRoute;
906 : : }
907 : :
908 : 5 : ConnectingRoute calculateConnectingRouteCheckForMergingRoute(route::FullRoute &objectARoute,
909 : : route::FullRoute &objectBRoute)
910 : : {
911 : 5 : auto roadSegmentIterA = objectARoute.roadSegments.begin();
912 : 5 : auto roadSegmentIterB = objectBRoute.roadSegments.begin();
913 : 5 : physics::Distance routeADistanceCovered{0.};
914 : 5 : physics::Distance routeBDistanceCovered{0.};
915 : :
916 : 6 : do
917 : : {
918 : : // we want to find the shortest connecting route
919 : : // therefore, increment the roadSegmentIter of the route with less distance covered
920 [ + - + + ]: 11 : if (routeADistanceCovered <= routeBDistanceCovered)
921 : : {
922 [ + - ]: 8 : if (roadSegmentIterA != objectARoute.roadSegments.end())
923 : : {
924 [ + - + - ]: 8 : routeADistanceCovered += calcLength(*roadSegmentIterA);
925 : :
926 : : // fix current segment A and search rest of B
927 : : auto findResultB
928 [ + - ]: 8 : = findRoadSegmentWithCommonLanes(*roadSegmentIterA, roadSegmentIterB, objectBRoute.roadSegments.end());
929 [ - + ]: 8 : if (findResultB != objectBRoute.roadSegments.end())
930 : : {
931 : : return calculateConnectingRouteCreateMergingRouteUpToPositions(
932 [ # # ]: 0 : objectARoute, roadSegmentIterA, objectBRoute, findResultB);
933 : : }
934 : :
935 : 8 : roadSegmentIterA++;
936 : : }
937 : : }
938 [ + - + - ]: 11 : if (routeBDistanceCovered <= routeADistanceCovered)
939 : : {
940 [ + - ]: 11 : if (roadSegmentIterB != objectBRoute.roadSegments.end())
941 : : {
942 [ + - + - ]: 11 : routeBDistanceCovered += calcLength(*roadSegmentIterB);
943 : :
944 : : // fix current segment B and search rest of A
945 : : auto findResultA
946 [ + - ]: 11 : = findRoadSegmentWithCommonLanes(*roadSegmentIterB, roadSegmentIterA, objectARoute.roadSegments.end());
947 [ + + ]: 11 : if (findResultA != objectARoute.roadSegments.end())
948 : : {
949 : : return calculateConnectingRouteCreateMergingRouteUpToPositions(
950 [ + - ]: 1 : objectARoute, findResultA, objectBRoute, roadSegmentIterB);
951 : : }
952 : :
953 : 10 : roadSegmentIterB++;
954 : : }
955 : : }
956 : 10 : } while ((roadSegmentIterA != objectARoute.roadSegments.end())
957 [ + + + + : 10 : && (roadSegmentIterB != objectBRoute.roadSegments.end()));
+ + ]
958 : :
959 : 8 : ConnectingRoute resultRoute;
960 : 4 : resultRoute.type = ConnectingRouteType::Invalid;
961 : 4 : return resultRoute;
962 : : }
963 : :
964 : 16 : double createHeadingRating(point::ENUHeading const &routeHeading,
965 : : point::ENUHeading const &objectHeading,
966 : : double objectHeadingOffset = 0.)
967 : : {
968 : 16 : auto routeHeadingD = static_cast<double>(routeHeading);
969 : 16 : auto objectHeadingD = static_cast<double>(objectHeading) + objectHeadingOffset;
970 : :
971 : : auto const headingDifference
972 [ + - ]: 16 : = std::fabs(physics::normalizeAngleSigned(physics::Angle(routeHeadingD - objectHeadingD)));
973 : :
974 [ + - + + ]: 16 : if (headingDifference > physics::cPI_2)
975 : : {
976 : 8 : return 0.;
977 : : }
978 [ + - + - ]: 8 : return 1. - 2. * headingDifference / physics::cPI;
979 : : }
980 : :
981 : : struct ConnectingRouteCandidate
982 : : {
983 : 4 : ConnectingRouteCandidate(const match::Object &iObjectA,
984 : : const match::Object &iObjectB,
985 : : Route::RawRoute const &iRawRoute,
986 : : match::LaneOccupiedRegion const &iRegionA,
987 : : match::LaneOccupiedRegion const &iRegionB,
988 : : lane::LaneIdSet const &iRelevantLanes)
989 : 4 : : objectA(iObjectA)
990 : : , objectB(iObjectB)
991 : : , rawRoute(iRawRoute)
992 : : , regionA(iRegionA)
993 : : , regionB(iRegionB)
994 : 4 : , relevantLanes(iRelevantLanes)
995 : : {
996 : 4 : }
997 : :
998 : : ConnectingRouteCandidate(ConnectingRouteCandidate const &other) = delete;
999 : 4 : ConnectingRouteCandidate(ConnectingRouteCandidate &&other) = default;
1000 : :
1001 : : const match::Object &objectA;
1002 : : const match::Object &objectB;
1003 : : Route::RawRoute rawRoute;
1004 : : match::LaneOccupiedRegion regionA;
1005 : : match::LaneOccupiedRegion regionB;
1006 : : lane::LaneIdSet const &relevantLanes;
1007 : :
1008 : 4 : void createFullRouteAndRating()
1009 : : {
1010 : 4 : fullRoute = calculateFullRoute(regionA, regionB, true);
1011 : 4 : fullRouteLength = route::calcLength(fullRoute);
1012 : :
1013 : 4 : headingRatingAlongRouteA = createHeadingRating(enuHeadingRouteA, objectA.enuPosition.heading);
1014 : 4 : headingRatingAlongRouteB = createHeadingRating(enuHeadingRouteB, objectB.enuPosition.heading);
1015 : 4 : headingRatingOppositeRouteA = createHeadingRating(enuHeadingRouteA, objectA.enuPosition.heading, M_PI);
1016 : 4 : headingRatingOppositeRouteB = createHeadingRating(enuHeadingRouteB, objectB.enuPosition.heading, M_PI);
1017 : 4 : }
1018 : :
1019 : : FullRoute calculateFullRoute(match::LaneOccupiedRegion const ®ionDropStart,
1020 : : match::LaneOccupiedRegion const ®ionDropEnd,
1021 : : bool updateHeading);
1022 : :
1023 : 2 : FullRoute getInvertedFullRoute()
1024 : : {
1025 : 2 : std::reverse(rawRoute.paraPointList.begin(), rawRoute.paraPointList.end());
1026 : 2 : return calculateFullRoute(regionB, regionA, false);
1027 : : }
1028 : :
1029 : : FullRoute fullRoute;
1030 : : physics::Distance fullRouteLength{};
1031 : : point::ENUHeading enuHeadingRouteA;
1032 : : point::ENUHeading enuHeadingRouteB;
1033 : :
1034 : : double headingRatingAlongRouteA{0.};
1035 : : double headingRatingAlongRouteB{0.};
1036 : : double headingRatingOppositeRouteA{0.};
1037 : : double headingRatingOppositeRouteB{0.};
1038 : :
1039 : 4 : bool isDegenerated()
1040 : : {
1041 [ + - ]: 4 : return fullRouteLength == physics::Distance(0.);
1042 : : }
1043 : :
1044 : 7 : bool objectADrivesAlongRoute()
1045 : : {
1046 : 7 : return headingRatingAlongRouteA > 0.;
1047 : : }
1048 : :
1049 : 4 : bool objectBDrivesOppositeToRoute()
1050 : : {
1051 : 4 : return headingRatingOppositeRouteB > 0.;
1052 : : }
1053 : :
1054 : 1 : void setAFollowsB()
1055 : : {
1056 [ + - ]: 1 : if (!isResultValid())
1057 : : {
1058 : 1 : resultRoute.type = ConnectingRouteType::Following;
1059 : 1 : resultRoute.routeA = fullRoute;
1060 : 1 : resultRoute.routeB = route::FullRoute();
1061 : 1 : headingFeasibility = headingRatingAlongRouteA * headingRatingAlongRouteB;
1062 : : }
1063 : 1 : }
1064 : :
1065 : 1 : void setBFollowsA()
1066 : : {
1067 [ + - ]: 1 : if (!isResultValid())
1068 : : {
1069 : 1 : resultRoute.type = ConnectingRouteType::Following;
1070 : 1 : resultRoute.routeA = route::FullRoute();
1071 : 1 : resultRoute.routeB = getInvertedFullRoute();
1072 : 1 : headingFeasibility = headingRatingOppositeRouteA * headingRatingOppositeRouteB;
1073 : : }
1074 : 1 : }
1075 : :
1076 : 1 : void setOpposing()
1077 : : {
1078 [ + - ]: 1 : if (!isResultValid())
1079 : : {
1080 : 1 : resultRoute.type = ConnectingRouteType::Opposing;
1081 : 1 : resultRoute.routeA = fullRoute;
1082 : 1 : resultRoute.routeB = getInvertedFullRoute();
1083 : 1 : headingFeasibility = headingRatingAlongRouteA * headingRatingOppositeRouteB;
1084 : : }
1085 : 1 : }
1086 : :
1087 : 0 : void setInvertedOpposing()
1088 : : {
1089 [ # # ]: 0 : if (!isResultValid())
1090 : : {
1091 : 0 : resultRoute.type = ConnectingRouteType::Opposing;
1092 : 0 : resultRoute.routeA = getInvertedFullRoute();
1093 : 0 : resultRoute.routeB = fullRoute;
1094 : 0 : headingFeasibility = headingRatingOppositeRouteA * headingRatingAlongRouteB;
1095 : : }
1096 : 0 : }
1097 : :
1098 : 4 : bool isResultValid() const
1099 : : {
1100 : 4 : return resultRoute.type != ConnectingRouteType::Invalid;
1101 : : }
1102 : :
1103 : 4 : bool isResultFeasible() const
1104 : : {
1105 [ + + ]: 4 : if (resultRoute.type == ConnectingRouteType::Invalid)
1106 : : {
1107 : 1 : return false;
1108 : : }
1109 : :
1110 : : // both 45° error -> headingFeasibility = 0.25
1111 : 3 : return headingFeasibility > 0.2;
1112 : : }
1113 : :
1114 : : ConnectingRoute resultRoute;
1115 : : double headingFeasibility{0.};
1116 : : };
1117 : :
1118 : 0 : std::ostream &operator<<(std::ostream &os, ConnectingRouteCandidate const &value)
1119 : : {
1120 : : os << "ConnectingRouteCandidate("
1121 : 0 : << " A:" << value.regionA << " routeHeading:" << value.enuHeadingRouteA
1122 : 0 : << " objectHeading:" << value.objectA.enuPosition.heading << " ratingAlong:" << value.headingRatingAlongRouteA
1123 : 0 : << " ratingOpposite:" << value.headingRatingOppositeRouteA << " B:" << value.regionB
1124 : 0 : << " routeHeading:" << value.enuHeadingRouteB << " objectHeading:" << value.objectB.enuPosition.heading
1125 : 0 : << " ratingAlong:" << value.headingRatingAlongRouteB << " ratingOpposite:" << value.headingRatingOppositeRouteB
1126 : 0 : << " len:" << value.fullRouteLength << " feas:" << value.headingFeasibility << " " << value.fullRoute
1127 : 0 : << " raw:" << value.rawRoute << "result: " << value.resultRoute << ")";
1128 : 0 : return os;
1129 : : }
1130 : :
1131 : 0 : bool compareCandidateRawRoutes(ConnectingRouteCandidate const &left, ConnectingRouteCandidate const &right)
1132 : : {
1133 : 0 : if ((left.rawRoute.routeDistance < right.rawRoute.routeDistance)
1134 [ # # # # : 0 : || ((left.rawRoute.routeDistance == right.rawRoute.routeDistance)
# # ]
1135 [ # # ]: 0 : && (left.rawRoute.paraPointList.size() < right.rawRoute.paraPointList.size())))
1136 : : {
1137 : 0 : return true;
1138 : : }
1139 : 0 : return false;
1140 : : }
1141 : :
1142 : 0 : bool compareCandidateFeasibility(ConnectingRouteCandidate const &left, ConnectingRouteCandidate const &right)
1143 : : {
1144 : 0 : return left.headingFeasibility < right.headingFeasibility;
1145 : : }
1146 : :
1147 : 6 : FullRoute ConnectingRouteCandidate::calculateFullRoute(match::LaneOccupiedRegion const ®ionDropStart,
1148 : : match::LaneOccupiedRegion const ®ionDropEnd,
1149 : : bool const updateHeading)
1150 : : {
1151 : 6 : auto route = createFullRoute(rawRoute, RouteCreationMode::AllRoutableLanes, relevantLanes);
1152 : :
1153 [ + - ]: 6 : if (!route.roadSegments.empty())
1154 : : {
1155 : : // drop the lane regions which led to the creation of the route as start and destination
1156 : : // and calculate the heading of the lanes in the respective regions
1157 [ + - ]: 8 : for (auto &laneSegment : route.roadSegments.front().drivableLaneSegments)
1158 : : {
1159 [ + - + + ]: 8 : if (laneSegment.laneInterval.laneId == regionDropStart.laneId)
1160 : : {
1161 : 6 : bool alignmentRequired = false;
1162 [ + - + - ]: 6 : if (isWithinInterval(laneSegment.laneInterval, regionDropStart.longitudinalRange.minimum))
1163 : : {
1164 [ + - + + ]: 6 : if (laneSegment.laneInterval.start != regionDropStart.longitudinalRange.minimum)
1165 : : {
1166 : 1 : laneSegment.laneInterval.start = regionDropStart.longitudinalRange.minimum;
1167 : 1 : alignmentRequired = true;
1168 : : }
1169 : : }
1170 [ + - + + ]: 6 : if (isWithinInterval(laneSegment.laneInterval, regionDropStart.longitudinalRange.maximum))
1171 : : {
1172 [ + - + - ]: 3 : if (laneSegment.laneInterval.start != regionDropStart.longitudinalRange.maximum)
1173 : : {
1174 : 3 : laneSegment.laneInterval.start = regionDropStart.longitudinalRange.maximum;
1175 : 3 : alignmentRequired = true;
1176 : : }
1177 : : }
1178 [ + + ]: 6 : if (alignmentRequired)
1179 : : {
1180 [ + - + - ]: 4 : alignRouteStartingPoints(getIntervalStart(laneSegment.laneInterval), route);
1181 : : }
1182 : :
1183 [ + + ]: 6 : if (updateHeading)
1184 : : {
1185 [ + - + - ]: 4 : enuHeadingRouteA = lane::getLaneENUHeading(route::getIntervalStart(laneSegment.laneInterval));
1186 [ + + ]: 4 : if (laneSegment.laneInterval.wrongWay)
1187 : : {
1188 : : // on a wrong way segment, we have to turn the lane heading in route direction
1189 [ + - ]: 2 : enuHeadingRouteA = point::createENUHeading(static_cast<double>(enuHeadingRouteA) + M_PI);
1190 : : }
1191 : : }
1192 : 6 : break;
1193 : : }
1194 : : }
1195 [ + - ]: 9 : for (auto &laneSegment : route.roadSegments.back().drivableLaneSegments)
1196 : : {
1197 [ + - + + ]: 9 : if (laneSegment.laneInterval.laneId == regionDropEnd.laneId)
1198 : : {
1199 : 6 : bool alignmentRequired = false;
1200 [ + - + + ]: 6 : if (isWithinInterval(laneSegment.laneInterval, regionDropEnd.longitudinalRange.minimum))
1201 : : {
1202 [ + - + + ]: 5 : if (laneSegment.laneInterval.end != regionDropEnd.longitudinalRange.minimum)
1203 : : {
1204 : 1 : laneSegment.laneInterval.end = regionDropEnd.longitudinalRange.minimum;
1205 : 1 : alignmentRequired = true;
1206 : : }
1207 : : }
1208 [ + - + + ]: 6 : if (isWithinInterval(laneSegment.laneInterval, regionDropEnd.longitudinalRange.maximum))
1209 : : {
1210 [ + - + + ]: 3 : if (laneSegment.laneInterval.end != regionDropEnd.longitudinalRange.maximum)
1211 : : {
1212 : 2 : laneSegment.laneInterval.end = regionDropEnd.longitudinalRange.maximum;
1213 : 2 : alignmentRequired = true;
1214 : : }
1215 : : }
1216 [ + + ]: 6 : if (alignmentRequired)
1217 : : {
1218 [ + - + - ]: 3 : alignRouteEndingPoints(getIntervalEnd(laneSegment.laneInterval), route);
1219 : : }
1220 [ + + ]: 6 : if (updateHeading)
1221 : : {
1222 [ + - + - ]: 4 : enuHeadingRouteB = lane::getLaneENUHeading(route::getIntervalEnd(laneSegment.laneInterval));
1223 [ + + ]: 4 : if (laneSegment.laneInterval.wrongWay)
1224 : : {
1225 : : // on a wrong way segment, we have to turn the lane heading in route direction
1226 [ + - ]: 2 : enuHeadingRouteB = point::createENUHeading(static_cast<double>(enuHeadingRouteB) + M_PI);
1227 : : }
1228 : : }
1229 : 6 : break;
1230 : : }
1231 : : }
1232 : : }
1233 : :
1234 : 6 : return route;
1235 : : }
1236 : :
1237 : 10 : ConnectingRoute calculateConnectingRoute(const match::Object &objectA,
1238 : : const match::Object &objectB,
1239 : : physics::Distance const &maxDistance,
1240 : : physics::Duration const &maxDuration,
1241 : : std::vector<route::FullRoute> const &objectAPredictionHints,
1242 : : std::vector<route::FullRoute> const &objectBPredictionHints,
1243 : : lane::LaneIdSet const &relevantLanes)
1244 : : {
1245 : 20 : std::list<ConnectingRouteCandidate> connectingRouteCandidates;
1246 [ + + ]: 22 : for (auto const &startMatchingResult : objectA.mapMatchedBoundingBox.laneOccupiedRegions)
1247 : : {
1248 [ + - - + ]: 12 : if (!isLaneRelevantForExpansion(startMatchingResult.laneId, relevantLanes))
1249 : : {
1250 : 0 : continue;
1251 : : }
1252 [ + - ]: 12 : auto const routingStart = createRoutingPoint(startMatchingResult);
1253 [ + + ]: 24 : for (auto const &destMatchingResult : objectB.mapMatchedBoundingBox.laneOccupiedRegions)
1254 : : {
1255 [ + - - + ]: 12 : if (!isLaneRelevantForExpansion(destMatchingResult.laneId, relevantLanes))
1256 : : {
1257 : 0 : continue;
1258 : : }
1259 [ + - ]: 12 : auto const routingDest = createRoutingPoint(destMatchingResult);
1260 : :
1261 : : RouteAstar routePlanning(
1262 [ + - ]: 24 : routingStart, routingDest, maxDistance, maxDuration, Route::Type::SHORTEST_IGNORE_DIRECTION);
1263 [ + - ]: 12 : routePlanning.setRelevantLanes(relevantLanes);
1264 [ + - + + ]: 12 : if (routePlanning.calculate())
1265 : : {
1266 [ + - + - ]: 4 : auto rawRoute = routePlanning.getRawRoute();
1267 [ + - ]: 4 : connectingRouteCandidates.push_back(
1268 [ + - ]: 8 : ConnectingRouteCandidate(objectA, objectB, rawRoute, startMatchingResult, destMatchingResult, relevantLanes));
1269 : : }
1270 : : }
1271 : : }
1272 : :
1273 [ + - + - ]: 20 : access::getLogger()->trace("calculateConnectingRoute[]: created {} candidates for {} <-> {} max_d:{} max_t:{}",
1274 : 20 : connectingRouteCandidates.size(),
1275 : : objectA,
1276 : : objectB,
1277 : : maxDistance,
1278 : : maxDuration);
1279 [ + + ]: 10 : if (connectingRouteCandidates.size() > 0u)
1280 : : {
1281 : : // first sort our candidates by raw route distance distance
1282 [ + - ]: 4 : connectingRouteCandidates.sort(compareCandidateRawRoutes);
1283 : :
1284 [ + + ]: 5 : for (auto candidateIter = connectingRouteCandidates.begin(); candidateIter != connectingRouteCandidates.end();)
1285 : : {
1286 : : // create the full routes
1287 [ + - ]: 4 : candidateIter->createFullRouteAndRating();
1288 : :
1289 [ + - + - ]: 8 : access::getLogger()->trace("calculateConnectingRoute: analyze {}", *candidateIter);
1290 : :
1291 : : // special handling for degenerated routes upfront
1292 [ + - - + ]: 4 : if (candidateIter->isDegenerated())
1293 : : {
1294 [ # # ]: 0 : if (candidateIter->rawRoute.paraPointList.size() == 1u)
1295 : : {
1296 : : // same lane, same point longitudinally:
1297 : : // define that A is following B
1298 [ # # ]: 0 : candidateIter->setAFollowsB();
1299 : : }
1300 [ # # ]: 0 : if (candidateIter->objectADrivesAlongRoute() == candidateIter->objectBDrivesOppositeToRoute())
1301 : : {
1302 : : // opposite case
1303 [ # # ]: 0 : if (candidateIter->objectADrivesAlongRoute())
1304 : : {
1305 : : // both driving in positive route direction as usual
1306 [ # # ]: 0 : candidateIter->setOpposing();
1307 : : }
1308 : : else
1309 : : {
1310 : : // switch the routes to have both again in proper direction
1311 [ # # ]: 0 : candidateIter->setInvertedOpposing();
1312 : : }
1313 : : }
1314 : : }
1315 : :
1316 [ + + + + : 4 : if (candidateIter->objectADrivesAlongRoute() && candidateIter->objectBDrivesOppositeToRoute())
+ + ]
1317 : : {
1318 [ + - ]: 1 : candidateIter->setOpposing();
1319 : : }
1320 [ + + ]: 3 : else if (candidateIter->objectADrivesAlongRoute())
1321 : : {
1322 [ + - ]: 1 : candidateIter->setAFollowsB();
1323 : : }
1324 [ + + ]: 2 : else if (candidateIter->objectBDrivesOppositeToRoute())
1325 : : {
1326 [ + - ]: 1 : candidateIter->setBFollowsA();
1327 : : }
1328 : : else
1329 : : {
1330 : : // both are driving away from each other, no result here
1331 : : }
1332 : :
1333 [ + + ]: 4 : if (candidateIter->isResultFeasible())
1334 : : {
1335 : : // found a valid and feasible connecting route, return
1336 [ + - + - ]: 6 : access::getLogger()->trace("calculateConnectingRoute[]: select valid and feasible result {}", *candidateIter);
1337 [ + - ]: 3 : return candidateIter->resultRoute;
1338 : : }
1339 : :
1340 [ - + ]: 1 : if (candidateIter->isResultValid())
1341 : : {
1342 : : // not feasible, continue evaluation
1343 [ # # # # ]: 0 : access::getLogger()->trace("calculateConnectingRoute[]: valid, but not feasible. Continue... {}",
1344 : 0 : *candidateIter);
1345 : 0 : candidateIter++;
1346 : : }
1347 : : else
1348 : : {
1349 : : // not valid, drop
1350 [ + - + - ]: 2 : access::getLogger()->trace("calculateConnectingRoute[]: not valid, dropping. Continue... {}", *candidateIter);
1351 : 1 : candidateIter = connectingRouteCandidates.erase(candidateIter);
1352 : : }
1353 : : }
1354 : :
1355 [ - + ]: 1 : if (connectingRouteCandidates.size() > 0u)
1356 : : {
1357 : : // take the most feasible of the remaining
1358 [ # # ]: 0 : connectingRouteCandidates.sort(compareCandidateFeasibility);
1359 [ # # # # ]: 0 : access::getLogger()->trace("calculateConnectingRoute[]: select most feasible of the remaining {}",
1360 : 0 : connectingRouteCandidates.front());
1361 [ # # ]: 0 : return connectingRouteCandidates.front().resultRoute;
1362 : : }
1363 : : }
1364 : : else
1365 : : {
1366 : : // no direct connecting route between the two objects found
1367 : : // check if we have a ConnectingRouteType::Merging case
1368 : : // search if one of the route lanes of A is part of one of the route lanes of B
1369 [ + - ]: 12 : auto objectAPredictions = objectAPredictionHints;
1370 [ + - ]: 6 : if (objectAPredictions.empty())
1371 : : {
1372 [ + - ]: 12 : objectAPredictions = predictRoutes(objectA.mapMatchedBoundingBox,
1373 : : maxDistance,
1374 : : maxDuration,
1375 : : RouteCreationMode::SameDrivingDirection,
1376 : : FilterDuplicatesMode::SubRoutesPreferLongerOnes,
1377 : 6 : relevantLanes);
1378 : : }
1379 [ + - ]: 12 : auto objectBPredictions = objectBPredictionHints;
1380 [ + - ]: 6 : if (objectBPredictions.empty())
1381 : : {
1382 [ + - ]: 12 : objectBPredictions = predictRoutes(objectB.mapMatchedBoundingBox,
1383 : : maxDistance,
1384 : : maxDuration,
1385 : : RouteCreationMode::SameDrivingDirection,
1386 : : FilterDuplicatesMode::SubRoutesPreferLongerOnes,
1387 : 6 : relevantLanes);
1388 : : }
1389 : :
1390 : 6 : auto resultDistance = std::numeric_limits<physics::Distance>::max();
1391 : 12 : ConnectingRoute resultRoute;
1392 [ + + ]: 11 : for (auto &objectAPrediction : objectAPredictions)
1393 : : {
1394 [ + + ]: 10 : for (auto &objectBPrediction : objectBPredictions)
1395 : : {
1396 [ + - ]: 10 : auto mergingRouteResult = calculateConnectingRouteCheckForMergingRoute(objectAPrediction, objectBPrediction);
1397 [ + + ]: 5 : if (mergingRouteResult.type == ConnectingRouteType::Merging)
1398 : : {
1399 [ + - + - : 1 : auto mergeDistance = std::min(calcLength(mergingRouteResult.routeA), calcLength(mergingRouteResult.routeB));
+ - ]
1400 [ + - + - ]: 1 : if (mergeDistance < resultDistance)
1401 : : {
1402 : 1 : resultDistance = mergeDistance;
1403 : 1 : resultRoute = std::move(mergingRouteResult);
1404 : : }
1405 : : }
1406 : : }
1407 : : }
1408 [ + - + - ]: 12 : access::getLogger()->trace("calculateConnectingRoute[]: merge result {}", resultRoute);
1409 : 6 : return resultRoute;
1410 : : }
1411 : :
1412 [ + - + - ]: 1 : access::getLogger()->trace("calculateConnectingRoute[]: empty result");
1413 : 1 : return ConnectingRoute();
1414 : : }
1415 : :
1416 : 10 : ConnectingRoute calculateConnectingRoute(const match::Object &startObject,
1417 : : const match::Object &destObject,
1418 : : physics::Distance const &maxDistance,
1419 : : std::vector<route::FullRoute> const &objectAPredictionHints,
1420 : : std::vector<route::FullRoute> const &objectBPredictionHints,
1421 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
1422 : : {
1423 : : return calculateConnectingRoute(startObject,
1424 : : destObject,
1425 : : maxDistance,
1426 : 20 : std::numeric_limits<physics::Duration>::max(),
1427 : : objectAPredictionHints,
1428 : : objectBPredictionHints,
1429 [ + - ]: 20 : relevantLanes);
1430 : : }
1431 : :
1432 : 0 : ConnectingRoute calculateConnectingRoute(const match::Object &startObject,
1433 : : const match::Object &destObject,
1434 : : physics::Duration const &maxDuration,
1435 : : std::vector<route::FullRoute> const &objectAPredictionHints,
1436 : : std::vector<route::FullRoute> const &objectBPredictionHints,
1437 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
1438 : : {
1439 : : return calculateConnectingRoute(startObject,
1440 : : destObject,
1441 : 0 : std::numeric_limits<physics::Distance>::max(),
1442 : : maxDuration,
1443 : : objectAPredictionHints,
1444 : : objectBPredictionHints,
1445 [ # # ]: 0 : relevantLanes);
1446 : : }
1447 : :
1448 : : } // namespace planning
1449 : : } // namespace route
1450 : : } // namespace map
1451 : : } // namespace ad
|
