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/LaneIdValidInputRange.hpp"
15 : : #include "ad/map/lane/LaneValidInputRange.hpp"
16 : : #include "ad/map/lane/Types.hpp"
17 : : #include "ad/map/match/Types.hpp"
18 : : #include "ad/map/point/BoundingSphereOperation.hpp"
19 : : #include "ad/map/point/Types.hpp"
20 : : #include "ad/map/restriction/RestrictionOperation.hpp"
21 : : #include "ad/physics/Duration.hpp"
22 : :
23 : : /** @brief namespace ad */
24 : : namespace ad {
25 : : /** @brief namespace map */
26 : : namespace map {
27 : : /** @brief namespace lane */
28 : : namespace lane {
29 : :
30 : : /**
31 : : * @brief checks if the given LaneId is valid
32 : : *
33 : : * The laneId is valid if it's within valid input range.
34 : : */
35 : 32248 : inline bool isValid(LaneId const &laneId, bool const logErrors = true)
36 : : {
37 : 32248 : return withinValidInputRange(laneId, logErrors);
38 : : }
39 : :
40 : : /**
41 : : * @brief checks if the given Lane is valid
42 : : *
43 : : * The lane is valid if it's within valid input range.
44 : : */
45 : 49 : inline bool isValid(Lane const &lane, bool const logErrors = true)
46 : : {
47 : 49 : return withinValidInputRange(lane, logErrors);
48 : : }
49 : :
50 : : /**
51 : : * @brief Method to be called to retrieve Lane from the Store.
52 : : * @param[in] id Lane identifier.
53 : : * @returns Lane with given identifier.
54 : : * Throws std::invalid_argument if id is not valid or if lane does not exist in the store.
55 : : */
56 : : Lane::ConstPtr getLanePtr(LaneId const &id);
57 : :
58 : : /**
59 : : * @brief Method to be called to retrieve Lane from the Store.
60 : : * @param[in] id Lane identifier.
61 : : * @returns Lane with given identifier.
62 : : * Throws std::invalid_argument if id is not valid or if lane does not exist in the store.
63 : : */
64 : : Lane const &getLane(lane::LaneId const &id);
65 : :
66 : : /**
67 : : * @brief Method to be called to retrieve identifiers of all Lanes in the Store.
68 : : * @returns Identifiers of all lanes in the store.
69 : : */
70 : : LaneIdList getLanes();
71 : :
72 : : /**
73 : : * @return lane heading at a mapMatchedPosition
74 : : */
75 : : point::ECEFHeading getLaneECEFHeading(match::MapMatchedPosition const &mapMatchedPosition);
76 : :
77 : : /**
78 : : * @return lane heading at a paraPoint
79 : : */
80 : : point::ECEFHeading getLaneECEFHeading(point::ParaPoint const ¶Point);
81 : :
82 : : /**
83 : : * @return lane heading at a mapMatchedPosition
84 : : */
85 : : point::ENUHeading getLaneENUHeading(match::MapMatchedPosition const &mapMatchedPosition);
86 : :
87 : : /**
88 : : * @return lane heading at a paraPoint with given gnssReference
89 : : */
90 : : point::ENUHeading getLaneENUHeading(point::ParaPoint const ¶Point, point::GeoPoint const &gnssReference);
91 : :
92 : : /**
93 : : * @return lane id at given location
94 : : *
95 : : * Throws if there is more than one lane at the given position
96 : : */
97 : : LaneId uniqueLaneId(point::GeoPoint const &point);
98 : :
99 : : /**
100 : : * @return parametric point at given location
101 : : *
102 : : * Throws if there is more than one lane at the given position
103 : : */
104 : : point::ParaPoint uniqueParaPoint(point::GeoPoint const &point);
105 : :
106 : : /**
107 : : * @brief Checks if vehicle fits the lanes restriction criteria.
108 : : * @param[in] lane the lane.
109 : : * @param[in] vehicle Description of the vehicle.
110 : : * @returns true of vehicle fits the restrictions criteria.
111 : : */
112 : 12 : inline bool isAccessOk(Lane const &lane, restriction::VehicleDescriptor const &vehicle)
113 : : {
114 : 12 : return isAccessOk(lane.restrictions, vehicle);
115 : : }
116 : :
117 : : /**
118 : : * @brief Checks if given sphere intersects with lane bounding ball.
119 : : * @param[in] lane the lane.
120 : : * @param[in] boundingSphere input sphere
121 : : * @returns true given sphere intersects with lane bounding ball.
122 : : */
123 : 129192 : inline bool isNear(Lane const &lane, point::BoundingSphere const &boundingSphere)
124 : : {
125 [ + - + - ]: 129192 : return distance(lane.boundingSphere, boundingSphere) == physics::Distance(0.);
126 : : }
127 : :
128 : : /**
129 : : * @param[in] lane the lane.
130 : : * @param[in] longitudinalOffset the longitudinal parametric offset
131 : : * @returns Width of the lane at the given longitudinal parametric offset \a longitudinalOffset
132 : : */
133 : : physics::Distance getWidth(Lane const &lane, physics::ParametricValue const &longitudinalOffset);
134 : :
135 : : /**
136 : : * @returns Speed limits on the provided parametric \a range along the \a lane.
137 : : */
138 : : physics::Speed getMaxSpeed(Lane const &lane, physics::ParametricRange const &range);
139 : :
140 : : /**
141 : : * @returns Speed limits on the provided parametric \a range along the \a lane.
142 : : */
143 : : restriction::SpeedLimitList getSpeedLimits(Lane const &lane, physics::ParametricRange const &range);
144 : :
145 : : /**
146 : : * @return minimum duration to drive the provided parametric \a range along the \a lane based on the speed limits of the
147 : : * lane.
148 : : */
149 : : physics::Duration getDuration(Lane const &lane, physics::ParametricRange const &range);
150 : :
151 : : /**
152 : : * @returns Maximum of HOV restriction for the \a lane
153 : : */
154 : 5 : inline restriction::PassengerCount getHOV(Lane const &lane)
155 : : {
156 : 5 : return getHOV(lane.restrictions);
157 : : }
158 : :
159 : : /**
160 : : * @brief Find contact lanes at specific relative location.
161 : : * @param[in] lane the lane.
162 : : * @param[in] location Location of interest.
163 : : * @returns Contacts to lane at specified location.
164 : : */
165 : : ContactLaneList getContactLanes(Lane const &lane, ContactLocation const &location);
166 : :
167 : : /**
168 : : * @brief Find contact lanes at specific relative locations.
169 : : * @param[in] locations Locations of interest.
170 : : * @returns Contacts to lanes at specified locations.
171 : : */
172 : : ContactLaneList getContactLanes(Lane const &lane, ContactLocationList const &locations);
173 : :
174 : : /**
175 : : * @brief Check nature of contact between this lane and another lane.
176 : : * @param[in] to_lane_id Another lane identifier.
177 : : * @returns Type of the contact. Can be INVALID if there is no contact between lanes.
178 : : */
179 : : ContactLocation getContactLocation(Lane const &lane, LaneId const &to_lane_id);
180 : :
181 : : /**
182 : : * @returns true if direction is POSITIVE or BIDIRECTIONAL.
183 : : */
184 : 168562 : inline bool isLaneDirectionPositive(Lane const &lane)
185 : : {
186 [ + + - + ]: 168562 : return lane.direction == LaneDirection::POSITIVE || lane.direction == LaneDirection::BIDIRECTIONAL;
187 : : }
188 : :
189 : : /**
190 : : * @returns true if direction is NEGATIVE or BIDIRECTIONAL.
191 : : */
192 : 47880 : inline bool isLaneDirectionNegative(Lane const &lane)
193 : : {
194 [ + + - + ]: 47880 : return lane.direction == LaneDirection::NEGATIVE || lane.direction == LaneDirection::BIDIRECTIONAL;
195 : : }
196 : :
197 : : /**
198 : : * @returns true if lane can be used in Routing.
199 : : */
200 : 88133 : inline bool isRouteable(Lane const &lane)
201 : : {
202 [ + - + + ]: 38924 : return lane.type == LaneType::INTERSECTION || lane.type == LaneType::MULTI || lane.type == LaneType::NORMAL
203 [ + + - + ]: 127057 : || lane.type == LaneType::TURN;
204 : : }
205 : :
206 : : /**
207 : : * @returns true if lane is part of an intersection.
208 : : */
209 : 9837 : inline bool isLanePartOfAnIntersection(Lane const &lane)
210 : : {
211 : 9837 : return (lane.type == lane::LaneType::INTERSECTION);
212 : : }
213 : :
214 : : /**
215 : : * @returns true if there are no lanes left of this one.
216 : : */
217 : : inline bool isLeftMost(Lane const &lane)
218 : : {
219 : : return getContactLanes(lane, ContactLocation::LEFT).empty();
220 : : }
221 : :
222 : : /**
223 : : * @returns true if there are no lanes right of this one.
224 : : */
225 : : inline bool isRightMost(Lane const &lane)
226 : : {
227 : : return getContactLanes(lane, ContactLocation::RIGHT).empty();
228 : : }
229 : :
230 : : /**
231 : : * @brief Calculates parametric point on the lane.
232 : : * Point is calculated by first calculating t_long parametric point
233 : : * on the left and on the right boundary, and then parametric point
234 : : * t_lat between those two points.
235 : : * @param[in] t_long Longitudinal parameter.
236 : : * @param[in] t_lat Lateral parameter relative to the left edge.
237 : : * @return Parametric point on the lane. Can be invalid.
238 : : */
239 : : point::ECEFPoint getParametricPoint(Lane const &lane,
240 : : physics::ParametricValue const &longitudinalOffset,
241 : : physics::ParametricValue const &lateralOffset);
242 : :
243 : : /**
244 : : * @brief Calculates the projection on left and right boundary of the given referencePoint.
245 : : * @param[in] referencePoint reference point.
246 : : * @param[out] point_on_left_edge projected point on the left boundary
247 : : * @param[out] point_on_right_edge projected point on the right boundary
248 : : * @return True if the operation succeeded.
249 : : */
250 : : bool projectParametricPointToEdges(Lane const &lane,
251 : : point::ECEFPoint const &referencePoint,
252 : : point::ECEFPoint &point_on_left_edge,
253 : : point::ECEFPoint &point_on_right_edge);
254 : :
255 : : /**
256 : : * @brief Calculates the projection on left and right boundary of the parametric point on the center of the lane.
257 : : * @param[in] longitudinalOffset Longitudinal parameter on the lane.
258 : : * @param[out] point_on_left_edge projected point on the left boundary
259 : : * @param[out] point_on_right_edge projected point on the right boundary
260 : : * @return True if the operation succeeded.
261 : : */
262 : : bool projectParametricPointToEdges(Lane const &lane,
263 : : physics::ParametricValue const &longitudinalOffset,
264 : : point::ECEFPoint &point_on_left_edge,
265 : : point::ECEFPoint &point_on_right_edge);
266 : :
267 : : /**
268 : : * @brief Calculates projected parametric point on the lane.
269 : : * Point is calculated by first calculating the projected points on left and right boundary using
270 : : * ProjectParametricPointToEdges(), and then parametric point t_lat between those two points.
271 : : * @param[in] longitudinalOffset Longitudinal parameter.
272 : : * @param[in] lateralOffset Lateral parameter relative to the left edge.
273 : : * @return Parametric point on the lane. Can be invalid.
274 : : */
275 : : point::ECEFPoint getProjectedParametricPoint(Lane const &lane,
276 : : physics::ParametricValue const &longitudinalOffset,
277 : : physics::ParametricValue const &lateralOffset);
278 : :
279 : : /**
280 : : * @returns Middle of the lane at the start.
281 : : */
282 : 28 : inline point::ECEFPoint getStartPoint(Lane const &lane)
283 : : {
284 [ + - ]: 56 : return getParametricPoint(lane, physics::ParametricValue(0.), physics::ParametricValue(0.5));
285 : : }
286 : :
287 : : /**
288 : : * @returns Middle of the lane at the end.
289 : : */
290 : 18 : inline point::ECEFPoint getEndPoint(Lane const &lane)
291 : : {
292 [ + - ]: 36 : return getParametricPoint(lane, physics::ParametricValue(1.), physics::ParametricValue(0.5));
293 : : }
294 : :
295 : : /**
296 : : * @brief Checks if Lane is longitudinally connected with another Lane at the end.
297 : : * @param[in] other Other object. Must be IsValid()!
298 : : * @returns True if this Lane longitudinally connected with another Lane at the end.
299 : : */
300 : : bool isPyhsicalSuccessor(Lane const &lane, Lane const &other);
301 : :
302 : : /**
303 : : * @brief Checks if Lane is longitudinally connected with another Lane at the start.
304 : : * @param[in] other Other object. Must be IsValid()!
305 : : * @returns True if this Lane longitudinally connected with another Lane at the start.
306 : : */
307 : : bool isPhysicalPredecessor(Lane const &lane, Lane const &other);
308 : :
309 : : /**
310 : : * @returns true if this lane is vanishing at the start.
311 : : */
312 : : bool isVanishingLaneStart(Lane const &lane);
313 : :
314 : : /**
315 : : * @returns true if this lane is vanishing at the end.
316 : : */
317 : : bool isVanishingLaneEnd(Lane const &lane);
318 : :
319 : : /**
320 : : * @brief Checks if Lane satisfies filter condition.
321 : : * @param[in] typeFilter Type of the lane as string.
322 : : * @param[in] isHov True if only lanes with HOV restriction shall be returned.
323 : : * @returns true if Lane satisfies filter condition.
324 : : */
325 : : bool satisfiesFilter(Lane const &lane, std::string const &typeFilter, bool isHov);
326 : :
327 : : /**
328 : : * @brief get the neighborhood relation to the other lane
329 : : *
330 : : * @param[in] laneId the main lane
331 : : * @param[in] checkLaneId the lane to be checked on the
332 : : *
333 : : * @retval lane::ContactLocation::OVERLAP: if it's the same lane
334 : : * @retval lane::ContactLocation::LEFT: if it's a left neighbor
335 : : * @retval lane::ContactLocation::RIGHT: if it's a right neighbor
336 : : * @retval lane::ContactLocation::SUCCESSOR: if it a successor lane
337 : : * @retval lane::ContactLocation::PREDECESSOR: if it's a predecessor lane
338 : : * @retval lane::ContactLocation::INVALID: if it's nothing of the above
339 : : */
340 : : ContactLocation getDirectNeighborhoodRelation(LaneId const laneId, LaneId const checkLaneId);
341 : :
342 : : /**
343 : : * @brief Check if a lane is successor or predecessor of a lane
344 : : *
345 : : * @param[in] laneId the main lane
346 : : * @param[in] checkLaneId the lane to be checked if it's a successor or predecessor of the main lane
347 : : *
348 : : * @returns \c true if the \a checkLaneId is a successor or predecessor of \a laneId
349 : : */
350 : : bool isSuccessorOrPredecessor(LaneId const laneId, LaneId const checkLaneId);
351 : :
352 : : /**
353 : : * @brief Check if two lanes are direct neighbors of even the same
354 : : *
355 : : * @returns \c true if the ids belong to the same or neighboring lanes, false otherwise
356 : : */
357 : : bool isSameOrDirectNeighbor(LaneId const id, LaneId const neighbor);
358 : :
359 : : /**
360 : : * @brief Check if the lane direction is positive
361 : : *
362 : : * @param[in] laneId the id of the lane to be checked
363 : : *
364 : : * @returns true if the lane direction is positive (positive || bidirectional)
365 : : */
366 : : bool isLaneDirectionPositive(LaneId const &laneId);
367 : :
368 : : /**
369 : : * @brief Check if the lane direction is negative
370 : : *
371 : : * @param[in] laneId the id of the lane to be checked
372 : : *
373 : : * @returns true if the lane direction is negative (negative || bidirectional)
374 : : */
375 : : bool isLaneDirectionNegative(LaneId const &laneId);
376 : :
377 : : /**
378 : : * @brief Get the distance of an object to the lane boundaries
379 : : *
380 : : * If the object provides a map matched bounding box, this information is used first (speed-up and more accurate).
381 : : * If the object's map matched bounding box is not touching the \c laneId at all, the object's center distance to the
382 : : * lane is calculated and reduced by max(width, length)/2 to obtain a good estimate for the shortest distance to the
383 : : * lane.
384 : : */
385 : : physics::Distance getDistanceToLane(LaneId laneId, match::Object const &object);
386 : :
387 : : /**
388 : : * @return the ENU heading of the lane at the given position
389 : : */
390 : : point::ENUHeading getLaneENUHeading(point::ParaPoint const &position);
391 : :
392 : : /**
393 : : * @brief Check if the provided heading is along the lane direction
394 : : *
395 : : * @param[in] position is the position at which the lane heading is obtained
396 : : * @param[in] heading is the heading to be checked
397 : : *
398 : : * @return true if the heading is within 90° of the lane direction, false otherwise
399 : : */
400 : : bool isHeadingInLaneDirection(point::ParaPoint const &position, point::ENUHeading const &heading);
401 : :
402 : : /**
403 : : * @brief project a position to a neighboring lane (incl. the lane itself), which has the provided heading
404 : : *
405 : : * @param[in] position to be projected
406 : : * @param[in] heading to be checked
407 : : * @param[out] projectedPosition is the projected position (can be position, if the lane is already in heading
408 : : * direction)
409 : : *
410 : : * @return true if a valid projection was found, false otherwise
411 : : */
412 : : bool projectPositionToLaneInHeadingDirection(point::ParaPoint const &position,
413 : : point::ENUHeading const &heading,
414 : : point::ParaPoint &projectedPosition);
415 : :
416 : : /**
417 : : * @brief Finds point on the lane nearest to the given point.
418 : : * @param[in] lane the lane.
419 : : * @param[in] pt Given point.
420 : : * @param[out] mmpos Resulting map-matched position.
421 : : * The resulting probability of the map matched position is derived from lateralT value:
422 : : * spans between [0.5; 1.0] for LANE_IN matches
423 : : * spans between [0.1; 0.5] for LANE_RIGHT/LANE_LEFT matches
424 : : *
425 : : * @returns true if successful.
426 : : */
427 : : bool findNearestPointOnLane(Lane const &lane, point::ECEFPoint const &pt, match::MapMatchedPosition &mmpos);
428 : :
429 : : /**
430 : : * @brief Finds point on the lane interval nearest to the given point.
431 : : * @param[in] laneInterval the interval of the lane to be considered.
432 : : * @param[in] pt Given point.
433 : : * @param[out] mmpos Resulting map-matched position.
434 : : * The resulting probability of the map matched position is derived from lateralT value:
435 : : * spans between [0.5; 1.0] for LANE_IN matches
436 : : * spans between [0.1; 0.5] for LANE_RIGHT/LANE_LEFT matches
437 : : *
438 : : * @returns true if successful.
439 : : */
440 : : bool findNearestPointOnLaneInterval(route::LaneInterval const &laneInterval,
441 : : point::ECEFPoint const &pt,
442 : : match::MapMatchedPosition &mmpos);
443 : :
444 : : /**
445 : : * @brief get the ENU point of a lane
446 : : *
447 : : */
448 : : point::ENUPoint getENULanePoint(point::ParaPoint const parametricPoint,
449 : : physics::ParametricValue const &lateralOffset = physics::ParametricValue(0.5));
450 : :
451 : : /** @brief calculate length of a lane with a given id
452 : : */
453 : : physics::Distance calcLength(LaneId const &laneId);
454 : :
455 : : /** @brief calculate length of a lane occupied region
456 : : */
457 : : physics::Distance calcLength(match::LaneOccupiedRegion const &laneOccupiedRegion);
458 : :
459 : : /** @brief calculate the width of the lane at the provided lanePoint
460 : : */
461 : : physics::Distance calcWidth(point::ParaPoint const ¶Point);
462 : :
463 : : /** @brief calculate the width of the provided lane at the provided longitudinal position
464 : : */
465 : : physics::Distance calcWidth(LaneId const &laneId, physics::ParametricValue const &longOffset);
466 : :
467 : : /**
468 : : * @brief calculate the width of a lane at the provided ENU position
469 : : *
470 : : * @return -1 in case the enuPoint could not be found on a lane
471 : : */
472 : : physics::Distance calcWidth(point::ENUPoint const &enuPoint);
473 : :
474 : : /** @brief calculate width of a lane occupied region
475 : : * scales the with at the center of the occupied region by the lateral extend of the region
476 : : */
477 : : physics::Distance calcWidth(match::LaneOccupiedRegion const &laneOccupiedRegion);
478 : :
479 : : /**
480 : : * @brief Struct to hold the altitude range of a lane
481 : : */
482 : : struct LaneAltitudeRange
483 : : {
484 : : point::Altitude minimum;
485 : : point::Altitude maximum;
486 : : };
487 : :
488 : : /**
489 : : * @brief Calculates the altitude range of a lane
490 : : *
491 : : * @returns The range of the lanes altitude
492 : : */
493 : : LaneAltitudeRange calcLaneAltitudeRange(Lane const &lane);
494 : :
495 : : /**
496 : : * @brief Checks if the laneId is relevant for expansion given the set of relevant lanes.
497 : : *
498 : : * @returns \c true if the \a relevantLanes set is empty, or if the laneId can be found in the set of \a relevantLanes.
499 : : */
500 : : bool isLaneRelevantForExpansion(lane::LaneId const &laneId, lane::LaneIdSet const &relevantLanes);
501 : :
502 : : } // namespace lane
503 : : } // namespace map
504 : : } // namespace ad
|
