Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-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 <utility>
15 : :
16 : : #include "ad/map/lane/LaneOperation.hpp"
17 : : #include "ad/map/route/Route.hpp"
18 : : #include "ad/physics/Operation.hpp"
19 : :
20 : : /** @brief namespace ad */
21 : : namespace ad {
22 : : /** @brief namespace map */
23 : : namespace map {
24 : : /** @brief namespace route */
25 : : namespace route {
26 : : /**
27 : : * @brief provides route planning capabilities on the road network of the map
28 : : */
29 : : namespace planning {
30 : :
31 : : static const physics::Distance cMinimumNeighborDistance{0.1};
32 : :
33 : : static const physics::Duration cMinimumNeighborDuration{0.05};
34 : :
35 : : static const physics::Speed cMinimumSpeed{10.};
36 : :
37 : : /**
38 : : * @brief Implements routing support functionality on the lane network.
39 : : *
40 : : * This class is used to expand a route by its reachable neighbors.
41 : : * The routing cost data is defined by a template type to be defined by the actual routing class.
42 : : */
43 : : template <class RoutingCostData> class RouteExpander : public Route
44 : : {
45 : : public:
46 : : //! definition of the routing cost data
47 : : struct RoutingCost
48 : : {
49 : : physics::Distance routeDistance{0.};
50 : : physics::Duration routeDuration{0.};
51 : : RoutingCostData costData{};
52 : : };
53 : : //! definition of the routing point
54 : : typedef std::pair<RoutingParaPoint, RoutingCost> RoutingPoint;
55 : :
56 : : /**
57 : : * @brief Constructor
58 : : *
59 : : * @param[in] start Start point.
60 : : * @param[in] dest Destination point.
61 : : * @param[in] maxDistance maximum route search distance.
62 : : * @param[in] maxDuration maximum route search duration.
63 : : * @param[in] typ Type of the route to be calculated.
64 : : */
65 : 2763 : RouteExpander(const RoutingParaPoint &start,
66 : : const RoutingParaPoint &dest,
67 : : physics::Distance const &maxDistance,
68 : : physics::Duration const &maxDuration,
69 : : Type const &routingType)
70 : 2763 : : Route(start, dest, maxDistance, maxDuration, routingType)
71 : : {
72 : 2763 : }
73 : :
74 : : RouteExpander(RouteExpander const &) = delete;
75 : : RouteExpander(RouteExpander const &&) = delete;
76 : : RouteExpander &operator=(RouteExpander const &) = delete;
77 : : RouteExpander &operator=(RouteExpander &&) = delete;
78 : :
79 : : /**
80 : : * @brief destructor.
81 : : */
82 : 2763 : virtual ~RouteExpander() = default;
83 : :
84 : : /**
85 : : * @brief Triggers the expansion of the given routing point \a origin to its valid neighborhood
86 : : */
87 : : void expandNeighbors(RoutingPoint const &origin);
88 : :
89 : : /**
90 : : * @brief set the lanes that are relevant for routing
91 : : * All the rest of the lanes in the map are ignored.
92 : : */
93 : 51 : void setRelevantLanes(::ad::map::lane::LaneIdSet const &relevantLanes)
94 : : {
95 : 51 : mRelevantLanes = relevantLanes;
96 : 51 : }
97 : :
98 : : /**
99 : : * @brief clear the list of relevant lanes.
100 : : */
101 : : void clearRelevantLanes()
102 : : {
103 : : mRelevantLanes.clear();
104 : : }
105 : :
106 : : protected:
107 : : /**
108 : : * @brief Definition of the reasons for route expansion
109 : : */
110 : : enum class ExpandReason
111 : : {
112 : : SameLaneNeighbor, //!< expand the route by a neighbor point within the same lane
113 : : LongitudinalNeighbor, //!< expand the route by a longitudinal neighbor lane point
114 : : LateralNeighbor, //!< expand the route by a lateral neighbor lane point
115 : : Destination, //!< expand the destination
116 : : };
117 : :
118 : : /**
119 : : * @brief a neighbor is added
120 : : *
121 : : * Override this function in the derived class to get notified on expanded neighbors.
122 : : *
123 : : * @param[in] originLane the lane object of the \a origin point
124 : : * @param[in] origin the origin RoutingPoint provided to the ExpandNeighbors() function
125 : : * @param[in] neighborLane the lane object of the \a neighbor point
126 : : * @param[in] neighbor the neighbor RoutingPoint which is added (RoutingCostData not filled!)
127 : : * @param[in] expandReason the reason why the origin was expanded to this neighbor
128 : : */
129 : : virtual void addNeighbor(lane::Lane::ConstPtr originLane,
130 : : RoutingPoint const &origin,
131 : : lane::Lane::ConstPtr neighborLane,
132 : : RoutingPoint const &neighbor,
133 : : ExpandReason const &expandReason)
134 : : = 0;
135 : :
136 : : //! @returns \c true if the given origin point is the parametric start
137 : 35121 : bool isStart(RoutingPoint const &origin)
138 : : {
139 [ + - ]: 35121 : return origin.first.point.parametricOffset == physics::ParametricValue(0.);
140 : : }
141 : :
142 : : //! @returns \c true if the given origin point is the parametric end
143 : 44374 : bool isEnd(RoutingPoint const &origin)
144 : : {
145 [ + - ]: 44374 : return origin.first.point.parametricOffset == physics::ParametricValue(1.);
146 : : }
147 : :
148 : : //! @returns \c true if the given origin point on the given lane defines a positive movement
149 : 42797 : bool isPositiveMovement(lane::Lane::ConstPtr lane, RoutingPoint const &origin)
150 : : {
151 [ + + ]: 85257 : return (laneDirectionIsIgnored() || isLaneDirectionPositive(*lane))
152 [ + + + + ]: 85257 : && (origin.first.direction == RoutingDirection::POSITIVE);
153 : : }
154 : :
155 : : //! @returns \c true if the given origin point on the given lane defines a negative movement
156 : 42605 : bool isNegativeMovement(lane::Lane::ConstPtr lane, RoutingPoint const &origin)
157 : : {
158 [ + + ]: 84884 : return (laneDirectionIsIgnored() || isLaneDirectionNegative(*lane))
159 [ + + + + ]: 84884 : && (origin.first.direction == RoutingDirection::NEGATIVE);
160 : : }
161 : :
162 : : //! check if lane is relevant for route expander
163 : 55738 : bool isLaneRelevantForExpansion(lane::LaneId const laneId) const
164 : : {
165 : 55738 : return lane::isLaneRelevantForExpansion(laneId, mRelevantLanes);
166 : : }
167 : :
168 : : //! perform the expansion of the neighbor points on the same lane
169 : : void expandSameLaneNeighbors(lane::Lane::ConstPtr lane, RoutingPoint const &origin);
170 : : //! perform the expansion of the neighbor points in longitudinal (contacts: successor/predecessor) lane direction
171 : : void expandLongitudinalNeighbors(lane::Lane::ConstPtr lane, RoutingPoint const &origin);
172 : : //! perform the expansion of the neighbor points in lateral (contacts: left/right) lane direction
173 : : void expandLateralNeighbors(lane::Lane::ConstPtr lane, RoutingPoint const &origin);
174 : :
175 : : //! create neighbor point and calculate the actual distance/duration to reach it
176 : : RoutingPoint createNeighbor(lane::Lane::ConstPtr originLane,
177 : : RoutingPoint const &origin,
178 : : lane::Lane::ConstPtr neighborLane,
179 : : RoutingParaPoint neighborRoutingParaPoint);
180 : : //! create longitudinal neighbor point with minimal actual distance/duration to reach
181 : : RoutingPoint createLongitudinalNeighbor(RoutingPoint const &origin, RoutingParaPoint neighborRoutingParaPoint);
182 : :
183 : : //! relevant lanes for the routing
184 : : ::ad::map::lane::LaneIdSet mRelevantLanes;
185 : : };
186 : :
187 : : template <class RoutingCostData>
188 : 29505 : void RouteExpander<RoutingCostData>::expandNeighbors(
189 : : typename RouteExpander<RoutingCostData>::RoutingPoint const &origin)
190 : : {
191 [ - + ]: 29505 : if (origin.first.direction == RoutingDirection::DONT_CARE)
192 : : {
193 [ # # ]: 0 : throw std::runtime_error("RouteExpander::ExpandNeighbors invalid routing direction!");
194 : : }
195 : :
196 [ + - ]: 59010 : lane::Lane::ConstPtr lane = lane::getLanePtr(origin.first.point.laneId);
197 [ + - ]: 29505 : if (lane)
198 : : {
199 : 29505 : if ( // lane has to be routable to expand
200 : 29505 : isRouteable(*lane)
201 [ + - + + ]: 59010 : && ( // max distance and max duration are not yet reached
202 [ + - + + : 29505 : ((origin.second.routeDistance < mMaxDistance) && (origin.second.routeDuration < mMaxDuration))
+ - + + ]
203 : : // or we are within an intersection
204 [ + + ]: 354 : || lane::isLanePartOfAnIntersection(*lane)))
205 : : {
206 [ + - ]: 29301 : expandSameLaneNeighbors(lane, origin);
207 [ + - ]: 29301 : expandLongitudinalNeighbors(lane, origin);
208 [ + - ]: 29301 : expandLateralNeighbors(lane, origin);
209 : : }
210 : : }
211 : : else
212 : : {
213 [ # # ]: 0 : throw std::runtime_error("RouteExpander::ExpandNeighbors No lane!");
214 : : }
215 : 29505 : }
216 : :
217 : : template <class RoutingCostData>
218 : : typename RouteExpander<RoutingCostData>::RoutingPoint
219 : 29789 : RouteExpander<RoutingCostData>::createNeighbor(lane::Lane::ConstPtr originLane,
220 : : RoutingPoint const &origin,
221 : : lane::Lane::ConstPtr neighborLane,
222 : : RoutingParaPoint neighborRoutingParaPoint)
223 : : {
224 [ + - ]: 29789 : RoutingPoint neighbor;
225 : 29789 : neighbor.first = neighborRoutingParaPoint;
226 : :
227 : 29789 : physics::Distance neighborDistance{0.};
228 : 29789 : physics::Duration neighborDuration{0.};
229 : : point::ECEFPoint pt_origin
230 [ + - ]: 29789 : = getParametricPoint(*originLane, origin.first.point.parametricOffset, physics::ParametricValue(0.5));
231 : : point::ECEFPoint pt_neighbor
232 [ + - ]: 29789 : = getParametricPoint(*neighborLane, neighbor.first.point.parametricOffset, physics::ParametricValue(0.5));
233 [ + - ]: 29789 : neighborDistance = point::distance(pt_neighbor, pt_origin);
234 [ + - ]: 29789 : physics::ParametricRange drivingRange;
235 [ + - + + ]: 29789 : if (origin.first.point.parametricOffset < neighbor.first.point.parametricOffset)
236 : : {
237 : 11455 : drivingRange.minimum = origin.first.point.parametricOffset;
238 : 11455 : drivingRange.maximum = neighbor.first.point.parametricOffset;
239 : : }
240 : : else
241 : : {
242 : 18334 : drivingRange.minimum = neighbor.first.point.parametricOffset;
243 : 18334 : drivingRange.maximum = origin.first.point.parametricOffset;
244 : : }
245 : :
246 [ + + ]: 29789 : if (originLane == neighborLane)
247 : : {
248 [ + - ]: 19873 : neighborDuration = getDuration(*originLane, drivingRange);
249 : : }
250 : : else
251 : : {
252 [ + - + - ]: 9916 : auto const maxSpeed = std::max(getMaxSpeed(*originLane, drivingRange), cMinimumSpeed);
253 [ + - ]: 9916 : neighborDuration = neighborDistance / maxSpeed;
254 : : }
255 [ + - ]: 29789 : neighborDistance = std::max(neighborDistance, cMinimumNeighborDistance);
256 [ + - ]: 29789 : neighborDuration = std::max(neighborDuration, cMinimumNeighborDuration);
257 [ + - ]: 29789 : neighbor.second.routeDistance = origin.second.routeDistance + neighborDistance;
258 [ + - ]: 29789 : neighbor.second.routeDuration = origin.second.routeDuration + neighborDuration;
259 : 59578 : return neighbor;
260 : : }
261 : :
262 : : template <class RoutingCostData>
263 : : typename RouteExpander<RoutingCostData>::RoutingPoint
264 : 24368 : RouteExpander<RoutingCostData>::createLongitudinalNeighbor(RoutingPoint const &origin,
265 : : RoutingParaPoint neighborRoutingParaPoint)
266 : : {
267 : 24368 : RoutingPoint neighbor;
268 : 24368 : neighbor.first = neighborRoutingParaPoint;
269 : 24368 : neighbor.second.routeDistance = origin.second.routeDistance + cMinimumNeighborDistance;
270 : 24368 : neighbor.second.routeDuration = origin.second.routeDuration + cMinimumNeighborDuration;
271 : 24368 : return neighbor;
272 : : }
273 : :
274 : : template <class RoutingCostData>
275 : 29301 : void RouteExpander<RoutingCostData>::expandSameLaneNeighbors(
276 : : lane::Lane::ConstPtr lane, typename RouteExpander<RoutingCostData>::RoutingPoint const &origin)
277 : : {
278 [ + - ]: 29301 : if ((lane->id == getDest().laneId)
279 [ + + + - : 89119 : && ((isPositiveMovement(lane, origin) && (origin.first.point.parametricOffset <= getDest().parametricOffset))
+ + + - +
+ + + -
- ]
280 [ + - + + : 30517 : || (isNegativeMovement(lane, origin) && (origin.first.point.parametricOffset >= getDest().parametricOffset))))
+ - + + +
+ + + -
- ]
281 : : {
282 : : // approaching the destination from valid side
283 [ + - ]: 2756 : auto const neighbor = createNeighbor(lane, origin, lane, getRoutingDest());
284 [ + - ]: 2756 : addNeighbor(lane, origin, lane, neighbor, ExpandReason::Destination);
285 : : }
286 [ + - + + : 29301 : if (isPositiveMovement(lane, origin) && !isEnd(origin))
+ - + + +
- + + -
- ]
287 : : {
288 [ + - ]: 9893 : auto const neighbor = createNeighbor(
289 [ + - ]: 9893 : lane, origin, lane, createRoutingParaPoint(lane->id, physics::ParametricValue(1.), origin.first.direction));
290 [ + - ]: 9893 : addNeighbor(lane, origin, lane, neighbor, ExpandReason::SameLaneNeighbor);
291 : : }
292 [ + - + + : 29301 : if (isNegativeMovement(lane, origin) && !isStart(origin))
+ - + + +
- + + -
- ]
293 : : {
294 [ + - ]: 7224 : auto const neighbor = createNeighbor(
295 [ + - ]: 7224 : lane, origin, lane, createRoutingParaPoint(lane->id, physics::ParametricValue(0.), origin.first.direction));
296 [ + - ]: 7224 : addNeighbor(lane, origin, lane, neighbor, ExpandReason::SameLaneNeighbor);
297 : : }
298 : 29301 : }
299 : :
300 : : template <class RoutingCostData>
301 : 29301 : void RouteExpander<RoutingCostData>::expandLongitudinalNeighbors(
302 : : lane::Lane::ConstPtr lane, typename RouteExpander<RoutingCostData>::RoutingPoint const &origin)
303 : : {
304 : 58602 : lane::ContactLaneList contactLanes;
305 [ + - + + : 29301 : if (isEnd(origin) && isPositiveMovement(lane, origin))
+ - + + +
+ + + -
- ]
306 : : {
307 [ + - ]: 5180 : contactLanes = lane::getContactLanes(*lane, lane::ContactLocation::SUCCESSOR);
308 : : }
309 [ + - + + : 24121 : else if (isStart(origin) && isNegativeMovement(lane, origin))
+ - + + +
+ + + -
- ]
310 : : {
311 [ + - ]: 3776 : contactLanes = lane::getContactLanes(*lane, lane::ContactLocation::PREDECESSOR);
312 : : }
313 [ + + + - ]: 53674 : for (auto contactLane : contactLanes)
314 : : {
315 [ + - + + ]: 24373 : if (!isLaneRelevantForExpansion(contactLane.toLane))
316 : : {
317 : 5 : continue;
318 : : }
319 [ + - ]: 48736 : lane::Lane::ConstPtr otherLane = lane::getLanePtr(contactLane.toLane);
320 [ + - ]: 24368 : if (otherLane)
321 : : {
322 [ + - ]: 24368 : if (isRouteable(*otherLane))
323 : : {
324 [ + - ]: 24368 : lane::ContactLocation otherToLane = getContactLocation(*otherLane, lane->id);
325 [ + + ]: 24368 : if (otherToLane == lane::ContactLocation::SUCCESSOR)
326 : : {
327 : 9981 : auto routingDirection = RoutingDirection::NEGATIVE;
328 [ + - ]: 19962 : auto const neighbor = createLongitudinalNeighbor(
329 [ + - ]: 9981 : origin, createRoutingParaPoint(otherLane->id, physics::ParametricValue(1.), routingDirection));
330 : :
331 [ + - ]: 9981 : addNeighbor(lane, origin, otherLane, neighbor, ExpandReason::LongitudinalNeighbor);
332 : : }
333 [ + - ]: 14387 : else if (otherToLane == lane::ContactLocation::PREDECESSOR)
334 : : {
335 : 14387 : auto routingDirection = RoutingDirection::POSITIVE;
336 [ + - ]: 28774 : auto const neighbor = createLongitudinalNeighbor(
337 [ + - ]: 14387 : origin, createRoutingParaPoint(otherLane->id, physics::ParametricValue(0.), routingDirection));
338 : :
339 [ + - ]: 14387 : addNeighbor(lane, origin, otherLane, neighbor, ExpandReason::LongitudinalNeighbor);
340 : : }
341 : : else
342 : : {
343 [ # # ]: 0 : throw std::runtime_error("Other lane neither SUCCESSOR not PREDECESSOR!");
344 : : }
345 : : }
346 : : }
347 : : else
348 : : {
349 [ # # ]: 0 : throw std::runtime_error("No other lane!");
350 : : }
351 : : }
352 : 29301 : }
353 : :
354 : : template <class RoutingCostData>
355 : 29301 : void RouteExpander<RoutingCostData>::expandLateralNeighbors(
356 : : lane::Lane::ConstPtr lane, typename RouteExpander<RoutingCostData>::RoutingPoint const &origin)
357 : : {
358 [ + - + - : 89967 : for (auto const &contactLane :
+ + ]
359 : 29301 : lane::getContactLanes(*lane, {lane::ContactLocation::LEFT, lane::ContactLocation::RIGHT}))
360 : : {
361 [ + - + + ]: 31365 : if (!isLaneRelevantForExpansion(contactLane.toLane))
362 : : {
363 : 8 : continue;
364 : : }
365 [ + - ]: 62714 : lane::Lane::ConstPtr otherLane = lane::getLanePtr(contactLane.toLane);
366 [ + - ]: 31357 : if (otherLane)
367 : : {
368 [ + + ]: 31357 : if (isRouteable(*otherLane))
369 : : {
370 [ + - + + : 30104 : if (laneDirectionIsIgnored() || (lane->direction == otherLane->direction))
+ + + + ]
371 : : {
372 [ + - ]: 9916 : auto const neighbor = createNeighbor(
373 : : lane,
374 : : origin,
375 : : otherLane,
376 [ + - ]: 9916 : createRoutingParaPoint(otherLane->id, origin.first.point.parametricOffset, origin.first.direction));
377 [ + - ]: 9916 : addNeighbor(lane, origin, otherLane, neighbor, ExpandReason::LateralNeighbor);
378 : : }
379 : : }
380 : : }
381 : : else
382 : : {
383 [ # # ]: 0 : throw std::runtime_error("No other lane!");
384 : : }
385 : : }
386 : 29301 : }
387 : :
388 : : } // namespace planning
389 : : } // namespace route
390 : : } // namespace map
391 : : } // namespace ad
|
