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 : : * @file
10 : : */
11 : :
12 : : #pragma once
13 : :
14 : : #include "ad/map/lane/Types.hpp"
15 : : #include "ad/map/match/Types.hpp"
16 : : #include "ad/map/point/Types.hpp"
17 : : #include "ad/map/route/LaneIntervalOperation.hpp"
18 : : #include "ad/map/route/Routing.hpp"
19 : : #include "ad/map/route/Types.hpp"
20 : : #include "ad/physics/Types.hpp"
21 : :
22 : : /** @brief namespace ad */
23 : : namespace ad {
24 : : /** @brief namespace map */
25 : : namespace map {
26 : :
27 : : /** @brief namespace intersection */
28 : : namespace intersection {
29 : : /**
30 : : * @brief forward declaration of Intersection class
31 : : */
32 : : class Intersection;
33 : : } // namespace intersection
34 : :
35 : : /** @brief namespace route */
36 : : namespace route {
37 : :
38 : : /**
39 : : * @brief struct defining iterator information on a route
40 : : */
41 : : struct RouteIterator
42 : : {
43 : 391 : RouteIterator(FullRoute const &iRoute, RoadSegmentList::const_iterator const &iRoadSegmentIterator)
44 : 391 : : route(iRoute)
45 : 391 : , roadSegmentIterator(iRoadSegmentIterator)
46 : : {
47 : 391 : }
48 : :
49 : : /**
50 : : * @brief check for validity of the iterator
51 : : */
52 : 16 : bool isValid() const
53 : : {
54 : 16 : return roadSegmentIterator != std::end(route.roadSegments);
55 : : }
56 : :
57 : : /** @brief reference to the route */
58 : : FullRoute const &route;
59 : : /** @brief iterator referencing an element of the queryRoute */
60 : : RoadSegmentList::const_iterator roadSegmentIterator;
61 : : };
62 : :
63 : : /**
64 : : * @brief get the route iterator from a given route position
65 : : *
66 : : * @param[in] routePosition the route position to find
67 : : * @param[in] route the route to check for the position
68 : : *
69 : : * @returns the RouteIterator pointing to the corresponding route segment.
70 : : * If the routePosition is not found an invalid RouteIterator is returned.
71 : : */
72 : : RouteIterator getRouteIterator(route::RouteParaPoint const &routePosition, route::FullRoute const &route);
73 : :
74 : : /**
75 : : * @brief get the lane para points in respect to the given route parametric offset
76 : : *
77 : : * @param[in] routeParametricOffset the parametric offset in respect to the route
78 : : * @param[in] laneInterval the laneInterval to consider, the whole lane interval is considered to be a full segment
79 : : * of a route.
80 : : *
81 : : * @returns the parametric point within the corresponding lane interval in respect to the given route parametric offset.
82 : : */
83 : : point::ParaPoint getLaneParaPoint(physics::ParametricValue const &routeParametricOffset,
84 : : route::LaneInterval const &laneInterval);
85 : :
86 : : /**
87 : : * @brief get the signed distance to a lane w.r.t to a given route (direction) for the map matched positions
88 : : *
89 : : * This check searches the mapMatchedPositions for the given \a checkLaneId and returns a signed distance value to the
90 : : * lane.
91 : : *
92 : : * @param[in] checkLaneId the lane id to find
93 : : * @param[in] route in which the lane should be in
94 : : * @param[in] mapMatchedPositions the map matched positions to check
95 : : *
96 : : * @returns:
97 : : * If not part of the mapMatchedPositions: distance = std::numeric_limits<physics::Distance>::max()
98 : : * If it's found and map matched type is:
99 : : * - map matched position is on the lane: distance = 0
100 : : * - map matched position is left of lane: distance < 0
101 : : * - map matched position is right of lane: distance > 0
102 : : *
103 : : * @throws std::runtime_error if the lane is not found inside the route
104 : : */
105 : : physics::Distance signedDistanceToLane(lane::LaneId const &checkLaneId,
106 : : FullRoute const &route,
107 : : match::MapMatchedPositionConfidenceList const &mapMatchedPositions);
108 : :
109 : : /**
110 : : * @brief get the lane para points in respect to the given route position
111 : : *
112 : : * @param[in] routePosition the route position to find
113 : : * @param[in] route the route to check for the position
114 : : *
115 : : * @returns the lane segment para points within the corresponding route segment.
116 : : * If the routePosition is not found an empty list is returned.
117 : : */
118 : : point::ParaPointList getLaneParaPoints(route::RouteParaPoint const &routePosition, route::FullRoute const &route);
119 : :
120 : : /** @brief calculate the length of the provided full route
121 : : */
122 : : physics::Distance calcLength(FullRoute const &fullRoute);
123 : :
124 : : /** @brief calculate the length of the provided road segment
125 : : *
126 : : * For length calculations the shortest road segment of this is used as reference.
127 : : */
128 : : physics::Distance calcLength(RoadSegment const &roadSegment);
129 : :
130 : : /** @brief calculate the length of the provided lane segment
131 : : */
132 : 196 : inline physics::Distance calcLength(LaneSegment const &laneSegment)
133 : : {
134 : 196 : return calcLength(laneSegment.laneInterval);
135 : : }
136 : :
137 : : /** @brief calculate the length of the provided connecting route
138 : : *
139 : : * For length calculations the longest route section of this is used as reference.
140 : : */
141 : : physics::Distance calcLength(ConnectingRoute const &connectingRoute);
142 : :
143 : : /** @brief calculate the length between the two given iterators
144 : : */
145 : : physics::Distance calcLength(RouteIterator const &startIterator, RouteIterator const &endIterator);
146 : :
147 : : physics::Distance
148 : : calcLength(RouteParaPoint const &startRouteParaPoint, RouteParaPoint const &endRouteParaPoint, FullRoute const &route);
149 : :
150 : : /** @brief calculate the Duration of the provided full route
151 : : */
152 : : physics::Duration calcDuration(FullRoute const &fullRoute);
153 : :
154 : : /** @brief calculate the Duration of the provided road segment
155 : : *
156 : : * For Duration calculations the shortest road segment of this is used as reference.
157 : : */
158 : : physics::Duration calcDuration(RoadSegment const &roadSegment);
159 : :
160 : : /** @brief calculate the Duration of the provided lane segment
161 : : */
162 : 5 : inline physics::Duration calcDuration(LaneSegment const &laneSegment)
163 : : {
164 : 5 : return calcDuration(laneSegment.laneInterval);
165 : : }
166 : :
167 : : /** @brief calculate the Duration of the provided connecting route
168 : : *
169 : : * For Duration calculations the longest route section of this is used as reference.
170 : : */
171 : : physics::Duration calcDuration(ConnectingRoute const &connectingRoute);
172 : :
173 : : /**
174 : : * @brief get the speed limits of the road segment
175 : : */
176 : : restriction::SpeedLimitList getSpeedLimits(RoadSegment const &roadSegment);
177 : :
178 : : /**
179 : : * @brief get the speed limits of the lane segment
180 : : */
181 : : restriction::SpeedLimitList getSpeedLimits(LaneSegment const &laneSegment);
182 : :
183 : : /**
184 : : * @brief get the speed limits between the two given iterators
185 : : */
186 : : restriction::SpeedLimitList getSpeedLimits(RouteIterator const &startIterator, RouteIterator const &endIterator);
187 : :
188 : : /**
189 : : * @brief get the speed limits of a complete route
190 : : */
191 : : restriction::SpeedLimitList getSpeedLimits(FullRoute const &fullRoute);
192 : :
193 : : /**
194 : : * @brief get the speed limits of a connecting route
195 : : */
196 : : restriction::SpeedLimitList getSpeedLimits(ConnectingRoute const &connectingRoute);
197 : :
198 : : /**
199 : : * @brief checks if the point is within the roadSegment
200 : : *
201 : : * @param[in] point parametric point to be checked against the road segment
202 : : *
203 : : * @returns \c true if the point is within one of the segments lane intervals
204 : : */
205 : : bool isWithinInterval(RoadSegment const &roadSegment, point::ParaPoint const &point);
206 : :
207 : : /**
208 : : * @brief struct defining the result type of findWaypoint() and findNearestWaypoint()
209 : : */
210 : : struct FindWaypointResult
211 : : {
212 : : /**
213 : : * @brief constructor
214 : : *
215 : : * constructs a result that is invalid
216 : : */
217 : : FindWaypointResult(FullRoute const &route);
218 : :
219 : : /**
220 : : * @brief constructor
221 : : *
222 : : * constructs a result with the provided data
223 : : */
224 : 73 : FindWaypointResult(FullRoute const &route,
225 : : point::ParaPoint const &iQueryPosition,
226 : : RoadSegmentList::const_iterator roadSegmentIter,
227 : : LaneSegmentList::const_iterator laneIntervalIter)
228 : 73 : : queryRoute(route)
229 : : , roadSegmentIterator(roadSegmentIter)
230 : : , laneSegmentIterator(laneIntervalIter)
231 : 73 : , queryPosition(iQueryPosition)
232 : : {
233 : 73 : }
234 : :
235 : : /**
236 : : * @brief copy constructor
237 : : */
238 : : FindWaypointResult(FindWaypointResult const &other) = default;
239 : :
240 : : /** @brief reference to the route */
241 : : FullRoute const &queryRoute;
242 : : /** @brief iterator referencing an element of the queryRoute */
243 : : RoadSegmentList::const_iterator roadSegmentIterator;
244 : : /** @brief iterator referencing an element of the routeIterator content; only valid if routeIterator !=
245 : : * queryRoute.end() */
246 : : LaneSegmentList::const_iterator laneSegmentIterator;
247 : : /** @brief copy of the query position this waypoint result refers to */
248 : : point::ParaPoint queryPosition;
249 : :
250 : : /**
251 : : * @brief checks whether the FindWaypointResult contains valid data
252 : : */
253 : 329 : bool isValid() const
254 : : {
255 : 329 : return roadSegmentIterator != queryRoute.roadSegments.end()
256 [ + + + - ]: 329 : && laneSegmentIterator != roadSegmentIterator->drivableLaneSegments.end();
257 : : }
258 : : /**
259 : : * @brief assignment operator
260 : : *
261 : : * The assignment is only possible if the other refers to the same query route as this.
262 : : * If there is a miss match a std::invalid_argument exception is thrown
263 : : */
264 : : FindWaypointResult &operator=(FindWaypointResult const &other);
265 : :
266 : : /**
267 : : * @brief return a FindWayPointResult that is iterated to the left lane of this
268 : : *
269 : : * If there is no left lane, the result is invalid (see isValid())
270 : : * @throws std::runtime_error() in case of an inconsistent route
271 : : */
272 : : FindWaypointResult getLeftLane() const;
273 : :
274 : : /**
275 : : * @brief return a FindWayPointResult that is iterated to the right lane of this
276 : : *
277 : : * If there is no right lane, the result is invalid (see isValid())
278 : : * @throws std::runtime_error() in case of an inconsistent route
279 : : */
280 : : FindWaypointResult getRightLane() const;
281 : :
282 : : /**
283 : : * @brief return a vector of FindWayPointResult that is iterated to the successor lanes of this
284 : : *
285 : : * If there are no successor lanes, the result is empty.
286 : : * @throws std::runtime_error() in case of an inconsistent route
287 : : */
288 : : std::vector<FindWaypointResult> getSuccessorLanes() const;
289 : :
290 : : /**
291 : : * @brief return a vector of FindWayPointResult that is iterated to the predecessor lanes of this
292 : : *
293 : : * If there are no predecessor lanes, the result is empty.
294 : : * @throws std::runtime_error() in case of an inconsistent route
295 : : */
296 : : std::vector<FindWaypointResult> getPredecessorLanes() const;
297 : : };
298 : :
299 : : /** @brief calculate the length of the route in respect to FindWaypointResult
300 : : *
301 : : * calculate the length of the route up to the queryPosition of the FindWaypointResult
302 : : */
303 : : physics::Distance calcLength(FindWaypointResult const &findWaypointResult);
304 : :
305 : : /** @brief find a waypoint on the route containing the given position
306 : : *
307 : : * The position is searched within the given full route. If the position's LaneId is present in the route,
308 : : * and the position's offset is within the route (i.e. the point lies within the route),
309 : : * an iterator of the route is returned in conjunction with the RouteInterval defining the interval of the route
310 : : * covering the corresponding lane.
311 : : *
312 : : * @param[in] position the position defining the lane id which should be searched for within the route
313 : : * @param[in] route the route to search in
314 : : *
315 : : * @returns waypoint result containing an iterator pointing to the route segment the lane of
316 : : * the position is part of and the corresponding route interval having that lane id.
317 : : * The query position is the provided \a position.
318 : : */
319 : : FindWaypointResult findWaypoint(point::ParaPoint const &position, route::FullRoute const &route);
320 : :
321 : : /** @brief find a waypoint on the route containing the given laneId
322 : : *
323 : : * The position is searched within the given full route. If the position's LaneId is present in the route,
324 : : * an iterator of the route is returned in conjunction with the RouteInterval defining the interval of the route
325 : : * covering the corresponding lane.
326 : : *
327 : : * @param[in] laneId the lane id which should be searched for within the route
328 : : * @param[in] route the route to search in
329 : : *
330 : : * @returns waypoint result containing an iterator pointing to the route segment the lane of
331 : : * the position is part of and the corresponding route interval having that lane id.
332 : : * The query position is (on success) the start of the lane interval within the route.
333 : : */
334 : : FindWaypointResult findWaypoint(lane::LaneId const &laneId, route::FullRoute const &route);
335 : :
336 : : /** @brief find waypoint on the route containing the given positions
337 : : *
338 : : * A version of findWaypoint() allowing the position to be a vector of positions to consider
339 : : * and select the result near to the begin of the route.
340 : : * All the given positions are searched using the findWaypoint(); therefore @see findWaypoint() for further details.
341 : : *
342 : : * @param[in] positions the position vector which should be searched for within the route
343 : : * @param[in] route the route to search in
344 : : *
345 : : * @returns waypoint result containing an iterator pointing to the route segment the lane of
346 : : * the position is part of and the corresponding route interval having that lane id.
347 : : * If multiple are matching the one nearest the begin of the route is selected.
348 : : */
349 : : FindWaypointResult findNearestWaypoint(point::ParaPointList const &positions, route::FullRoute const &route);
350 : :
351 : : /**
352 : : * @brief find nearest waypoint on the route containing the given mapmatched positions
353 : : *
354 : : * This function will return the nearest waypoint along the route
355 : : *
356 : : * @param[in] mapMatchedPositions the map matched positions which should be searched for within the route
357 : : * @param[in] route the route to search in
358 : : *
359 : : * @returns waypoint result containing an iterator pointing to the route segment the lane of
360 : : * the position is part of and the corresponding route interval having that lane id.
361 : : * If multiple are matching the one nearest the begin of the route is selected.
362 : : */
363 : : FindWaypointResult findNearestWaypoint(match::MapMatchedPositionConfidenceList const &mapMatchedPositions,
364 : : route::FullRoute const &route);
365 : :
366 : : /**
367 : : * @brief find waypoint on the route of the given object, preferable the object center
368 : : *
369 : : * This function first calls findNearestWayPoint on the map matched position
370 : : * confidence list of the center position.
371 : : * If this doesn't lead to a result, the center points of the respective occupied regions are
372 : : * used to find a waypoint. The search starts with the longest region and continues in decreasing order.
373 : : * If this still doesn't lead to a match findNearestWayPoint of the other map matched confidence list positions
374 : : * is taken.
375 : : *
376 : : * @param[in] object the object which should be searched for within the route
377 : : * @param[in] route the route to search in
378 : : *
379 : : * @returns waypoint result containing an iterator pointing to the route segment the lane of
380 : : * the position is part of and the corresponding route interval having that lane id.
381 : : */
382 : : FindWaypointResult findCenterWaypoint(match::Object const &object, route::FullRoute const &route);
383 : :
384 : : /**
385 : : * @brief find nearest waypoint on the route containing the given mapmatched bounding box
386 : : *
387 : : * This function will return the nearest waypoint along the route
388 : : *
389 : : * @param[in] object which position which should be searched for within the route
390 : : * @param[in] route the route to search in
391 : : *
392 : : * @returns waypoint result containing an iterator pointing to the route segment the lane of
393 : : * the position is part of and the corresponding route interval having that lane id.
394 : : * If multiple are matching the one nearest the begin of the route is selected.
395 : : */
396 : : FindWaypointResult objectOnRoute(match::MapMatchedObjectBoundingBox const &object, route::FullRoute const &route);
397 : :
398 : : /**
399 : : * @brief find waypoint on the route containing a lane of the given intersection
400 : : *
401 : : * This function will return the nearest waypoint along the route
402 : : *
403 : : * @param[in] intersection which inner lanes should be searched for within the route
404 : : * @param[in] route the route to search in
405 : : *
406 : : * @returns waypoint result containing an iterator pointing to the route segment the lane of the intersection
407 : : * is part of and the corresponding route interval having that lane id.
408 : : * If multiple are matching the one nearest the begin of the route is selected.
409 : : *
410 : : */
411 : : FindWaypointResult intersectionOnRoute(intersection::Intersection const &intersection, route::FullRoute const &route);
412 : :
413 : : /**
414 : : * @returns \c true if the connecting route contains road segments which are part of an intersection
415 : : */
416 : : bool isConnectedRoutePartOfAnIntersection(route::ConnectingRoute const &connectingRoute);
417 : :
418 : : /** @brief enumeration providing more insights into the actual shorten route result
419 : : */
420 : : enum class ShortenRouteResult
421 : : {
422 : : /** Shortening succeeded
423 : : */
424 : : Succeeded,
425 : : /** Shortening succeeded: the actual provided position(s) were not found with the
426 : : * findNearestWaypoint() function, but at least one position is on a lane of the
427 : : * route start lane segments, just before the route interval starts
428 : : */
429 : : SucceededBeforeRoute,
430 : : /** Shortening succeeded, but the resulting route is empty (i.e. crossed the end of the route)
431 : : */
432 : : SucceededRouteEmpty,
433 : : /** Shortening succeeded: the shortening would cut the current intersection within route.
434 : : * Therefore, actually not cut.
435 : : */
436 : : SucceededIntersectionNotCut,
437 : : /** Shortening failed, the resulting (or already the input) route is empty
438 : : */
439 : : FailedRouteEmpty
440 : : };
441 : :
442 : : /** @brief enumeration defining the mode of operation of the shortenRoute() functions
443 : : */
444 : : enum class ShortenRouteMode
445 : : {
446 : : /** Standard behavior
447 : : */
448 : : Normal,
449 : : /** If shorten route result would be: ShortenRouteResult::SucceededBeforeRoute, the route is extended
450 : : * at the beginning so that the result actually becomes ShortenRouteResult::Succeeded
451 : : */
452 : : PrependIfSucceededBeforeRoute,
453 : : /**
454 : : * If the shorten route operation would cut an intersection in between,
455 : : * the route is not shortened further, before the intersection is actually left.
456 : : *
457 : : * If shorten route result would be: ShortenRouteResult::SucceededBeforeRoute, the route is extended
458 : : * at the beginning so that the result actually becomes ShortenRouteResult::Succeeded
459 : : */
460 : : DontCutIntersectionAndPrependIfSucceededBeforeRoute
461 : : };
462 : :
463 : : /** @brief follow the route up to a certain position and shorten the route accordingly
464 : : *
465 : : * @param[in] currentPosition the position which should mark the start of the route after shortening
466 : : * @param[in/out] route the route to be shortened
467 : : * @param[in] extendIfSucceededBeforeRoute if this flag is set @c true,
468 : : *
469 : : * @retval ShortenRouteResult::Succeeded, SucceededBeforeRoute::SucceededBeforeRoute if the shortening request was
470 : : * accepted resulting in a valid route.
471 : : * @retval ShortenRouteResult::SucceededRouteEmpty if the shortening request was accepted resulting in an empty route
472 : : * (i.e. end of route just crossed).
473 : : * @retval ShortenRouteResult::FailedRouteEmpty if the shortening failed and resulting route is empty.
474 : : */
475 : : ShortenRouteResult shortenRoute(point::ParaPoint const ¤tPosition,
476 : : route::FullRoute &route,
477 : : ShortenRouteMode const shortenRouteMode = ShortenRouteMode::Normal);
478 : :
479 : : /** @brief follow the route up to a certain position and shorten the route accordingly
480 : : *
481 : : * Overloaded version of shortenRoute() allow the position to be a vector of positions to consider.
482 : : * If multiple of the positions were actually present within the route, the one is selected providing the
483 : : * longest resulting route.
484 : : *
485 : : * @param[in] currentPositions the vector of positions which should mark the start of the route after shortening
486 : : * @param[in/out] route the route to be shortened
487 : : *
488 : : * @retval ShortenRouteResult::Succeeded, SucceededBeforeRoute::SucceededBeforeRoute if the shortening request was
489 : : * accepted resulting in a valid route.
490 : : * @retval ShortenRouteResult::SucceededRouteEmpty if the shortening request was accepted resulting in an empty route
491 : : * (i.e. end of route just crossed).
492 : : * @retval ShortenRouteResult::FailedRouteEmpty if the shortening failed and resulting route is empty.
493 : : */
494 : : ShortenRouteResult shortenRoute(point::ParaPointList const ¤tPositions,
495 : : route::FullRoute &route,
496 : : ShortenRouteMode const shortenRouteMode = ShortenRouteMode::Normal);
497 : :
498 : : /** @brief follow the route up to a certain position and shorten the route accordingly
499 : : *
500 : : * Overloaded version of shortenRoute() allow the position to be a list of mapmatched positions to consider.
501 : : * If multiple of the positions were actually present within the route, the one is selected providing the
502 : : * longest resulting route.
503 : : *
504 : : * @param[in] mapMatchedPositions the map matched positions which should mark the start of the route after shortening
505 : : * @param[in/out] route the route to be shortened
506 : : *
507 : : * @retval ShortenRouteResult::Succeeded, SucceededBeforeRoute::SucceededBeforeRoute if the shortening request was
508 : : * accepted resulting in a valid route.
509 : : * @retval ShortenRouteResult::SucceededRouteEmpty if the shortening request was accepted resulting in an empty route
510 : : * (i.e. end of route just crossed).
511 : : * @retval ShortenRouteResult::FailedRouteEmpty if the shortening failed and resulting route is empty.
512 : : */
513 : : ShortenRouteResult shortenRoute(match::MapMatchedPositionConfidenceList const &mapMatchedPositions,
514 : : route::FullRoute &route,
515 : : ShortenRouteMode const shortenRouteMode = ShortenRouteMode::Normal);
516 : :
517 : : /**
518 : : * @brief calculate the RouteParaPoint at a given distance to a given RouteParaPoint
519 : : *
520 : : * @param[in] route
521 : : * @param[in] origin the reference point to this the distance should be calculated
522 : : * @param[in] distance: Negative value is towards begin of the route. Positive values is towards end of the route
523 : : * @param[out] resultingPoint: the point at distance to the origin RouteParaPoint
524 : : *
525 : : * returns @c true if the resultingParaPoint was found in the route
526 : : */
527 : : bool calculateRouteParaPointAtDistance(route::FullRoute const &route,
528 : : route::RouteParaPoint const &origin,
529 : : physics::Distance const &distance,
530 : : route::RouteParaPoint &resultingPoint);
531 : :
532 : : /**
533 : : * @brief calculate the RouteParaPoint for a given ParaPoint and FullRoute
534 : : *
535 : : * @param[in] paraPoint is the point under consideration
536 : : * @param[in] route
537 : : * @param[out] routeParaPoint: the converted result
538 : : *
539 : : * returns @c true if the paraPoint was found in the route
540 : : */
541 : : bool getRouteParaPointFromParaPoint(point::ParaPoint const ¶Point,
542 : : FullRoute const &route,
543 : : route::RouteParaPoint &routeParaPoint);
544 : :
545 : : /**
546 : : * @brief the mode how the route section will be created
547 : : */
548 : : enum class RouteSectionCreationMode
549 : : {
550 : : SingleLane, /**< In the resulting route section the LaneSegments contain on single lane interval */
551 : : AllRouteLanes /**< In the resulting route section the LaneSegments contain all the lanes of the original route */
552 : : };
553 : :
554 : : /**
555 : : * @brief Return a section of the route that is within the given distance around the given LanePoint
556 : : *
557 : : * This section will contain the LaneInterval that the center point is located at and all
558 : : * the predecessors & successors of this lane up to the given distances or the start / end of the route
559 : : * The length of the resulting route will be distanceFront + distanceEnd except the start / end of the route is within
560 : : * the delta region.
561 : : *
562 : : * @param[in] currentLane specifying the origin of the delta section as FindWaypointResult
563 : : * @param[in] distance to be included in the delta region towards begin of the route
564 : : * @param[in] distance to be included in the delta region towards end of the route
565 : : * @param[in] the route
566 : : * @param[in] routeSectionCreationMode the creation mode for the route section.
567 : : *
568 : : * return the extracted route (will be empty if the centerPoint is not found in the route)
569 : : *
570 : : * @throws std::runtime_error if the route is inconsistent
571 : : */
572 : : FullRoute getRouteSection(FindWaypointResult const ¤tLane,
573 : : physics::Distance const &distanceFront,
574 : : physics::Distance const &distanceEnd,
575 : : FullRoute const &route,
576 : : RouteSectionCreationMode const routeSectionCreationMode
577 : : = RouteSectionCreationMode::SingleLane);
578 : :
579 : : /**
580 : : * @brief Return a section of the route that is within the given distance around the given LanePoint
581 : : *
582 : : * This section will contain the LaneInterval that the center point is located at and all
583 : : * the predecessors & successors of this lane up to the given distances or the start / end of the route
584 : : * The length of the resulting route will be distanceFront + distanceEnd except the start / end of the route is within
585 : : * the delta region.
586 : : *
587 : : * @param[in] centerPoint specifying the origin of the delta section
588 : : * @param[in] distance to be included in the delta region towards begin of the route
589 : : * @param[in] distance to be included in the delta region towards end of the route
590 : : * @param[in] the route
591 : : * @param[in] routeSectionCreationMode the creation mode for the route section.
592 : : *
593 : : * return the extracted route (will be empty if the centerPoint is not found in the route)
594 : : *
595 : : * @throws std::runtime_error if the route is inconsistent
596 : : */
597 : : route::FullRoute getRouteSection(point::ParaPoint const ¢erPoint,
598 : : physics::Distance const &distanceFront,
599 : : physics::Distance const &distanceEnd,
600 : : route::FullRoute const &route,
601 : : RouteSectionCreationMode const routeSectionCreationMode
602 : : = RouteSectionCreationMode::SingleLane);
603 : :
604 : : /**
605 : : * @brief Return a section of the route that is around the given object
606 : : *
607 : : * This section will contain the LaneInterval that the center point of the object is located at and all
608 : : * the predecessors & successors of this lane up to half of the objects length or the start / end of the route
609 : : * The length of the resulting route will be object's length except the start / end of the route is within
610 : : * the delta region.
611 : : *
612 : : * @param[in] object specifying the object as the origin and dimension of the delta section
613 : : * @param[in] the route
614 : : * @param[in] routeSectionCreationMode the creation mode for the route section.
615 : : *
616 : : * return the extracted route (will be empty if the object is not found in the route)
617 : : *
618 : : * @throws std::runtime_error if the route is inconsistent
619 : : */
620 : : route::FullRoute getRouteSection(match::Object const &object,
621 : : route::FullRoute const &route,
622 : : RouteSectionCreationMode const routeSectionCreationMode
623 : : = RouteSectionCreationMode::SingleLane);
624 : :
625 : : /**
626 : : * @brief struct defining the result type of findFirstLaneChange()
627 : : */
628 : : struct FindLaneChangeResult
629 : : {
630 : : /**
631 : : * @brief constructor
632 : : *
633 : : * constructs a result that is invalid
634 : : */
635 : : explicit FindLaneChangeResult(FullRoute const &route);
636 : :
637 : : /** @brief reference to the route */
638 : : FullRoute const &queryRoute;
639 : :
640 : : /** @brief iterator referencing to route index where the lane change can start at the earliest */
641 : : RoadSegmentList::const_iterator laneChangeStartRouteIterator;
642 : :
643 : : /** @brief iterator referencing an element of the laneChangeStartRouteIterator content; only valid if
644 : : * laneChangeStartRouteIterator !=
645 : : * queryRoute.routeSegments.end() */
646 : : LaneSegmentList::const_iterator laneChangeStartLaneSegmentIterator;
647 : :
648 : : /** @brief iterator referencing to route index where the lane change needs to be ended at the latest */
649 : : RoadSegmentList::const_iterator laneChangeEndRouteIterator;
650 : :
651 : : /** @brief iterator referencing an element of the laneChangeEndRouteIterator content; only valid if
652 : : * laneChangeEndRouteIterator !=
653 : : * queryRoute.routeSegments.end() */
654 : : LaneSegmentList::const_iterator laneChangeEndLaneSegmentIterator;
655 : :
656 : : /** @brief the lane change direction */
657 : : LaneChangeDirection laneChangeDirection;
658 : :
659 : : /** @brief the number of connected lane changes to reach the desired target lane from the current position */
660 : : uint32_t numberOfConnectedLaneChanges{0u};
661 : :
662 : : /** @brief returns true, if the routeIteratorLaneChangeStart and the routeIteratorLaneChangeEnd, as well as the lane
663 : : * change direction are set */
664 : 8 : bool isValid() const
665 : : {
666 : 8 : return laneChangeStartRouteIterator != std::end(queryRoute.roadSegments)
667 [ + - ]: 5 : && laneChangeStartLaneSegmentIterator != std::end(laneChangeStartRouteIterator->drivableLaneSegments)
668 [ + - ]: 5 : && laneChangeEndRouteIterator != std::end(queryRoute.roadSegments)
669 [ + - ]: 5 : && laneChangeEndLaneSegmentIterator != std::end(laneChangeEndRouteIterator->drivableLaneSegments)
670 [ + + + - ]: 13 : && laneChangeDirection != LaneChangeDirection::Invalid;
671 : : }
672 : :
673 : : /**
674 : : * @brief calculate the length of the zone of the lane change
675 : : *
676 : : * @returns the length of the lane change zone on the route including start and end lane change segment
677 : : */
678 : : physics::Distance calcZoneLength() const;
679 : : };
680 : :
681 : : /** @brief finds the first route interval on a given route where a lane change is necessary to travel the given
682 : : * route from start to end. The function finds the first lane change only.
683 : : *
684 : : * The result is returned by an FindLaneChangeResult which holds
685 : : * - the routeIteratorLaneChangeStart as the pointer to the route index on where the lane change can start at the
686 : : * earliest. A valid routeIteratorLaneChangeStart can be in the range [std::begin(route), std::end(route)[.
687 : : * - the routeIteratorLaneChangeEnd as the point on the route where the lane change needs to be finished at the
688 : : * latest. A valid routeIteratorLaneChangeEnd can be in the range [std::begin(route), std::end(route)[
689 : : * - the laneChangeDirection specifying if the lane change goes from left to right or right to left.
690 : : *
691 : : * This function uses the logger of the AdMapAccess::instance() singleton.
692 : : *
693 : : * @param[in] currentPositionEgoVehicle the currentPosition of the ego vehicle
694 : : * @param[in] route the route on which the first lane change is searched
695 : : *
696 : : * @returns a valid lane change result as described above if the route contains a lane change. Returns an invalid
697 : : * FindLaneChangeResult if there is no lane change on the route.
698 : : *
699 : : * @throws std::runtime_error if the route contains invalid transitions.
700 : : * @throws std::runtime_error if the map is not initialized or the road network could not be obtained
701 : : *
702 : : */
703 : : FindLaneChangeResult findFirstLaneChange(match::MapMatchedPosition const ¤tPositionEgoVehicle,
704 : : route::FullRoute const &route);
705 : :
706 : : /** @brief shorten the route from the end to have at maximum the given length
707 : : *
708 : : * A cut of the route is not performed within an intersection, the intersection is kept fully within the route.
709 : : *
710 : : * @param[in/out] route the route to check and to shorten at the end
711 : : * @param[in] length the maximum length
712 : : */
713 : : void shortenRouteToDistance(route::FullRoute &route, const physics::Distance &length);
714 : :
715 : : /** @brief function to append a new lane interval to a road segment list
716 : : *
717 : : * In contrast to appendRoadSegmentToRoute() here only the lane interval without any neighbors is added
718 : : *
719 : : * @param[in] laneInterval the new lane interval to be append
720 : : * @param[in] route the route the interval has to be appended
721 : : * @param[in] segmentCountFromDestination the segment count for the new road segment to be created
722 : : *
723 : : */
724 : : void appendLaneSegmentToRoute(route::LaneInterval const &laneInterval,
725 : : route::FullRoute &route,
726 : : route::SegmentCounter const segmentCountFromDestination = 0u);
727 : :
728 : : /**
729 : : * @brief extends route to have at least the given length
730 : : *
731 : : * @param[in/out] route the route to check and to extend
732 : : * @param[in] length the minimum length
733 : : * @param[in/out] additionalRoutes additional routes in case of intersections on the extension
734 : : * @param[in] relevantLanes if not empty, the function restricts the extension to the given set of lanes
735 : : *
736 : : * @returns pair of bool and vector
737 : : *
738 : : * @c true if the route has sufficient length or was extended.
739 : : * If @c false is returned the route is/was empty/degenerated or the additional routes were not empty.
740 : :
741 : : */
742 : : bool extendRouteToDistance(route::FullRoute &route,
743 : : physics::Distance const &length,
744 : : route::FullRouteList &additionalRoutes,
745 : : lane::LaneIdSet const &relevantLanes = ad::map::lane::LaneIdSet());
746 : :
747 : : /**
748 : : * @brief extends route with the given list of destinations
749 : : *
750 : : * @param[in/out] route the route to check and to extend
751 : : * @param[in] dest Vector with supporting points as routing parametric points to be visited on the route. Last point in
752 : : * the list is the
753 : : * actual destination point.
754 : : *
755 : : * @returns @c false is returned if the route is/was empty/degenerated
756 : : */
757 : : bool extendRouteToDestinations(route::FullRoute &route, const std::vector<route::planning::RoutingParaPoint> &dest);
758 : :
759 : : /**
760 : : * @brief extends route with the given list of destinations
761 : : *
762 : : * @param[in/out] route the route to check and to extend
763 : : * @param[in] dest Vector with supporting points as geo points to be visited on the route. Last point in the list is the
764 : : * actual destination point.
765 : : *
766 : : * @returns @c false is returned if the route is/was empty/degenerated
767 : : */
768 : : bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::GeoPoint> &dest);
769 : :
770 : : /**
771 : : * @brief extends route with the given list of destinations
772 : : *
773 : : * @param[in/out] route the route to check and to extend
774 : : * @param[in] dest Vector with supporting points as ENU points to be visited on the route. Last point in the list is the
775 : : * actual destination point.
776 : : *
777 : : * @returns @c false is returned if the route is/was empty/degenerated
778 : : */
779 : : bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::ENUPoint> &dest);
780 : :
781 : : /** @brief function to append a new lane interval to a road segment list
782 : : *
783 : : * This function is mainly used while creation of a FullRoute object.
784 : : * The lane interval is expanded by it's left and right neighbor lanes (with same driving direction) to a complete
785 : : * road segment; which is then appended to the provided road segment list.
786 : : *
787 : : * After the route is finished, planning::updateRoutePlanningCounters() should be called to update the planning
788 : : * counters.
789 : : *
790 : : * @param[in] laneInterval the new lane interval to be append
791 : : * @param[in] laneOffset the lane offset of the new lane interval to be append
792 : : * @param[in] route the route the interval has to be appended
793 : : * @param[in] relevantLanes if not empty, the function restricts the extension to the given set of lanes
794 : : *
795 : : */
796 : : void appendRoadSegmentToRoute(route::LaneInterval const &laneInverval,
797 : : route::RouteLaneOffset const &laneOffset,
798 : : route::FullRoute &route,
799 : : lane::LaneIdSet const &relevantLanes);
800 : :
801 : : /**
802 : : * @brief add an opposing lane segment to an existing (and not empty) road segment with at most the given length
803 : : *
804 : : * This function must only be used, if an existing route should be extended with an opposing lane.
805 : : * Note, the length of the lane segment can be less than the provided distance, in case the neighboring lane segment
806 : : * is shorter as well.
807 : : *
808 : : * The function is used by addOpposingLaneToRoute
809 : : *
810 : : * @param[in] startPoint is the start point where the opposing lane segment will start
811 : : * @param[in] distance is the maximum distance of the newly added lane segment
812 : : * @param[in/out] roadSegment is the road segment which is updated with the opposing lane segment
813 : : * @param[in/out] route is the route the road segment belons to
814 : : *
815 : : * @return -1 on failure, otherwise the length of the new lane segment is added
816 : : */
817 : : physics::Distance addOpposingLaneSegmentToRoadSegment(point::ParaPoint const &startpoint,
818 : : physics::Distance const &distance,
819 : : route::RoadSegment &roadSegment,
820 : : route::FullRoute &route);
821 : :
822 : : /**
823 : : * @brief add a part of the opposing lane to an existing route. The part is at most distanceOnWrongLane long,
824 : : * but may be shorter (e.g. intersections stop adding more segments of the opposing lane)
825 : : *
826 : : * Note the starting point of adding the opposing lane must be in the first segment of the route
827 : : *
828 : : * @param[in] pointOnOppositeLane is the starting point at which the opposite lane should be added
829 : : * @param[in] distanceOnWrongLane maximum distance on the opposite lane
830 : : * @param[in/out] route to be updated
831 : : * @param[out] coveredDistance is the actually covered distance of the opposing lane segment(s)
832 : : */
833 : : bool addOpposingLaneToRoute(point::ParaPoint const &pointOnOppositeLane,
834 : : physics::Distance const &distanceOnWrongLane,
835 : : route::FullRoute &route,
836 : : physics::Distance &coveredDistance);
837 : :
838 : : /**
839 : : * @returns the input route expanded by all lanes in opposite driving direction
840 : : *
841 : : * Internally recreates the route by calling appendRoadSegmentToRoute() with a new route
842 : : * having RouteCreationMode::AllRoutableLanes
843 : : */
844 : : route::FullRoute getRouteExpandedToOppositeLanes(route::FullRoute const &route);
845 : :
846 : : /**
847 : : * @returns the input route expanded by all neighbor lanes
848 : : *
849 : : * Internally recreates the route by calling appendRoadSegmentToRoute() with a new route
850 : : * having RouteCreationMode::AllNeighbors
851 : : */
852 : : route::FullRoute getRouteExpandedToAllNeighborLanes(route::FullRoute const &route);
853 : :
854 : : /**
855 : : * @brief calculate a bypassing route for a given (blocked) route
856 : : *
857 : : * This function is designed to by used to get a route around parking vehicles or for overtaking
858 : : * The output route makes use of neighboring lanes and respects left-hand/right-hand traffic flow
859 : : * The calculation fails when reaching an intersection
860 : : *
861 : : * @param[in] route is the input route, which should not be used
862 : : * @param[out] bypassingRoute is the new route, making use of neighboring lanes
863 : : *
864 : : * @return false if no valid bypassing route was found
865 : : */
866 : : bool calculateBypassingRoute(route::FullRoute const &route, route::FullRoute &bypassingRoute);
867 : :
868 : : /** @brief get borders of a road segment. The road segment is cut at a given parametric offset.
869 : : *
870 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
871 : : *
872 : : * @param[in] roadSegment the road segment to extract the borders from
873 : : * @param[in] parametricOffset parametric offset the borders are cut
874 : : */
875 : : lane::ECEFBorder getECEFBorderOfRoadSegment(RoadSegment const &roadSegment,
876 : : physics::ParametricValue const parametricOffset);
877 : :
878 : : /** @brief get borders of a road segment
879 : : *
880 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
881 : : *
882 : : * @param[in] roadSegment the road segment to extract the borders from
883 : : */
884 : 0 : inline lane::ECEFBorder getECEFBorderOfRoadSegment(RoadSegment const &roadSegment)
885 : : {
886 [ # # ]: 0 : return getECEFBorderOfRoadSegment(roadSegment, physics::ParametricValue(1.));
887 : : }
888 : :
889 : : /** @brief get borders of a road segment. The road segment is cut at a given parametric offset.
890 : : *
891 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
892 : : *
893 : : * @param[in] roadSegment the road segment to extract the borders from
894 : : * @param[in] parametricOffset parametric offset the borders are cut
895 : : */
896 : : lane::ENUBorder getENUBorderOfRoadSegment(RoadSegment const &roadSegment,
897 : : physics::ParametricValue const parametricOffset);
898 : :
899 : : /** @brief get borders of a road segment
900 : : *
901 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
902 : : *
903 : : * @param[in] roadSegment the road segment to extract the borders from
904 : : */
905 : 0 : inline lane::ENUBorder getENUBorderOfRoadSegment(RoadSegment const &roadSegment)
906 : : {
907 [ # # ]: 0 : return getENUBorderOfRoadSegment(roadSegment, physics::ParametricValue(1.));
908 : : }
909 : :
910 : : /** @brief get borders of a road segment. The road segment is cut at a given parametric offset.
911 : : *
912 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
913 : : *
914 : : * @param[in] roadSegment the road segment to extract the borders from
915 : : * @param[in] parametricOffset parametric offset the borders are cut
916 : : */
917 : : lane::GeoBorder getGeoBorderOfRoadSegment(RoadSegment const &roadSegment,
918 : : physics::ParametricValue const parametricOffset);
919 : :
920 : : /** @brief get borders of a road segment
921 : : *
922 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
923 : : *
924 : : * @param[in] roadSegment the road segment to extract the borders from
925 : : */
926 : 0 : inline lane::GeoBorder getGeoBorderOfRoadSegment(RoadSegment const &roadSegment)
927 : : {
928 [ # # ]: 0 : return getGeoBorderOfRoadSegment(roadSegment, physics::ParametricValue(1.));
929 : : }
930 : :
931 : : /** @brief get borders of a full route
932 : : *
933 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
934 : : * The borders of successive road segments are collected in the result vector.
935 : : *
936 : : * @param[in] route the route to extract the borders from
937 : : */
938 : : lane::ENUBorderList getENUBorderOfRoute(FullRoute const &route);
939 : :
940 : : /** @brief get borders of a full route
941 : : *
942 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
943 : : * The borders of successive road segments are collected in the result vector.
944 : : *
945 : : * @param[in] route the route to extract the borders from
946 : : */
947 : : lane::ECEFBorderList getECEFBorderOfRoute(FullRoute const &route);
948 : :
949 : : /** @brief get borders of a full route
950 : : *
951 : : * The order of the points within the LaneGeometries are ordered according to the route direction.
952 : : * The borders of successive road segments are collected in the result vector.
953 : : *
954 : : * @param[in] route the route to extract the borders from
955 : : */
956 : : lane::GeoBorderList getGeoBorderOfRoute(FullRoute const &route);
957 : :
958 : : /**
959 : : * @brief get the ENU heading of a route at the location of the object
960 : : *
961 : : * @param object the object on the route
962 : : * @param route the route
963 : : *
964 : : * @throws std::runtime_error if the objects map matched bounding box is not on the route
965 : : */
966 : : point::ENUHeading getENUHeadingOfRoute(match::Object const &object, FullRoute const &route);
967 : :
968 : : /**
969 : : * @brief compare the ENU heading of the route at the location of the object with the ENU heading of the object
970 : : *
971 : : * @returns \c true if the difference between object heading and route heading at the object position
972 : : * is not larger than 90°
973 : : * @throws std::runtime_error if the objects map matched bounding box is not on the route
974 : : */
975 : : bool isObjectHeadingInRouteDirection(match::Object const &object, FullRoute const &route);
976 : :
977 : : /**
978 : : * @brief Shorten the provided road segment by a certain distance from the begin
979 : : *
980 : : * The intervals of the segment are shortened as described in shortenIntervalFromBegin()
981 : : *
982 : : * @throws std::runtime_error if the roadSegment is invalid
983 : : */
984 : : void shortenSegmentFromBegin(RoadSegment &roadSegment, physics::Distance const &distance);
985 : :
986 : : /**
987 : : * @brief Shorten the provided road segment by a certain distance from the end
988 : : *
989 : : * The intervals of the segment are shortened as described in shortenIntervalFromEnd()
990 : : *
991 : : * @throws std::runtime_error if the roadSegment is invalid
992 : : */
993 : : void shortenSegmentFromEnd(RoadSegment &roadSegment, physics::Distance const &distance);
994 : :
995 : : } // namespace route
996 : : } // namespace map
997 : : } // namespace ad
|
