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/RouteAStar.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include "ad/map/access/Logging.hpp"
13 : :
14 : : #define DEBUG_OUTPUT 0
15 : :
16 : : #if DEBUG_OUTPUT
17 : : #include <iostream>
18 : :
19 : : namespace std {
20 : :
21 : : inline ostream &operator<<(ostream &out, ::ad::map::route::planning::RouteAstar::RoutingPoint const &value)
22 : : {
23 : : out << value.first.point << "|" << (int)value.first.direction << " ("
24 : : << static_cast<double>(value.second.routeDistance) << ","
25 : : << static_cast<double>(value.second.costData.estimatedDistanceToTarget) << ")";
26 : : return out;
27 : : }
28 : :
29 : : } // namespace std
30 : : #endif
31 : :
32 : : namespace ad {
33 : : namespace map {
34 : : namespace route {
35 : : namespace planning {
36 : :
37 : 12 : RouteAstar::RouteAstar(const RoutingParaPoint &start,
38 : : const RoutingParaPoint &dest,
39 : : physics::Distance const &maxDistance,
40 : : physics::Duration const &maxDuration,
41 : 12 : Type typ)
42 : 12 : : RouteExpander(start, dest, maxDistance, maxDuration, typ)
43 : : {
44 [ + - ]: 12 : initLanePointer();
45 : 12 : }
46 : :
47 : 0 : RouteAstar::RouteAstar(const RoutingParaPoint &start,
48 : : const RoutingParaPoint &dest,
49 : : physics::Distance const &maxDistance,
50 : 0 : Type typ)
51 [ # # ]: 0 : : RouteExpander(start, dest, maxDistance, physics::Duration::getMax(), typ)
52 : : {
53 [ # # ]: 0 : initLanePointer();
54 : 0 : }
55 : :
56 : 0 : RouteAstar::RouteAstar(const RoutingParaPoint &start,
57 : : const RoutingParaPoint &dest,
58 : : physics::Duration const &maxDuration,
59 : 0 : Type typ)
60 [ # # ]: 0 : : RouteExpander(start, dest, physics::Distance::getMax(), maxDuration, typ)
61 : : {
62 [ # # ]: 0 : initLanePointer();
63 : 0 : }
64 : :
65 : 2711 : RouteAstar::RouteAstar(const RoutingParaPoint &start, const RoutingParaPoint &dest, Type typ)
66 [ + - ]: 2711 : : RouteExpander(start, dest, physics::Distance::getMax(), physics::Duration::getMax(), typ)
67 : : {
68 [ + - ]: 2711 : initLanePointer();
69 : 2711 : }
70 : :
71 : 2723 : void RouteAstar::initLanePointer()
72 : : {
73 : 2723 : mDestLane = lane::getLanePtr(mDest.point.laneId);
74 [ - + ]: 2723 : if (!mDestLane)
75 : : {
76 [ # # ]: 0 : throw std::runtime_error("Dest lane not found!");
77 : : }
78 : 2723 : mStartLane = lane::getLanePtr(mStart.point.laneId);
79 [ - + ]: 2723 : if (!mStartLane)
80 : : {
81 [ # # ]: 0 : throw std::runtime_error("Start lane not found!");
82 : : }
83 : 2723 : }
84 : :
85 : : // https://en.wikipedia.org/wiki/A*_search_algorithm
86 : 2723 : bool RouteAstar::calculate()
87 : : {
88 : 2723 : mProcessedPoints.clear();
89 : 2723 : mProcessingMap.clear();
90 : 2723 : mCameFrom.clear();
91 : 2723 : mRawRoutes.clear();
92 : :
93 : : // A* working structures.
94 : : // Initial values.
95 : 2723 : RoutingCost cost;
96 [ + - ]: 2723 : cost.costData.estimatedDistanceToTarget = costEstimate(mStartLane, mStart.point);
97 [ + + ]: 2723 : if (mStart.direction == RoutingDirection::DONT_CARE)
98 : : {
99 : : // Don't care routing direction means the vehicle can drive forward or backward.
100 : : // Within the routing we have to replace this by concrete directions,
101 : : // otherwise the vehicle would be able to switch between forward/backward while driving on the route
102 : : // which especially is not meant by the don't care flag.
103 : 2711 : auto startPositive = mStart;
104 : 2711 : startPositive.direction = RoutingDirection::POSITIVE;
105 [ + - ]: 2711 : mProcessingMap.insert({startPositive, cost});
106 : 2711 : auto startNegative = mStart;
107 : 2711 : startNegative.direction = RoutingDirection::NEGATIVE;
108 [ + - ]: 2711 : mProcessingMap.insert({startNegative, cost});
109 : : }
110 : : else
111 : : {
112 [ + - ]: 12 : mProcessingMap.insert({mStart, cost});
113 : : }
114 : : // Run
115 : 2723 : bool pathFound = false;
116 : : #if DEBUG_OUTPUT
117 : : std::cout << "######### (typ:" << (int)mType << " #########"
118 : : << " start: " << mStart.point << " dest: " << mDest.point << std::endl;
119 : : #endif
120 [ + + ]: 33265 : while (!mProcessingMap.empty())
121 : : {
122 : : // sort the routing points by the smallest total cost
123 : : struct FScoreCompare
124 : : {
125 : 75082 : bool operator()(RoutingPoint const &left, RoutingPoint const &right) const
126 : : {
127 : 75082 : return left.second.costData.estimatedDistanceToTarget < right.second.costData.estimatedDistanceToTarget;
128 : : }
129 : : };
130 [ + - ]: 30542 : auto minimumCostIterator = std::min_element(mProcessingMap.begin(), mProcessingMap.end(), FScoreCompare());
131 [ + - ]: 2 : if (((mDest.direction == RoutingDirection::DONT_CARE) || (mDest.direction == minimumCostIterator->first.direction))
132 [ + + + - : 30544 : && (minimumCostIterator->first.point == mDest.point))
+ + + + ]
133 : : {
134 [ + - ]: 2709 : reconstructPath(*minimumCostIterator);
135 : 2709 : pathFound = true;
136 : : #if DEBUG_OUTPUT
137 : : std::cout << "Target reached " << *minimumCostIterator << std::endl;
138 : : for (auto const &point : getRawRoute().paraPointList)
139 : : {
140 : : std::cout << " " << point << std::endl;
141 : : }
142 : : std::cout << "#########" << std::endl;
143 : : #endif
144 : 2709 : mProcessingMap.clear();
145 : : }
146 : : else
147 : : {
148 : 27833 : RoutingPoint const minimumValue = *minimumCostIterator;
149 [ + - ]: 27833 : mProcessingMap.erase(minimumCostIterator);
150 [ + - ]: 27833 : mProcessedPoints.insert(minimumValue.first);
151 : : #if DEBUG_OUTPUT
152 : : std::cout << "Expanding: " << minimumValue << std::endl;
153 : : #endif
154 [ + - ]: 27833 : expandNeighbors(minimumValue);
155 : : #if DEBUG_OUTPUT
156 : : std::cout << "Results in " << std::endl;
157 : : for (auto &element : mProcessingMap)
158 : : {
159 : : std::cout << " " << element << std::endl;
160 : : }
161 : : std::cout << "----------" << std::endl;
162 : : #endif
163 : : }
164 : : }
165 : :
166 : 2723 : mProcessedPoints.clear();
167 : 2723 : mProcessingMap.clear();
168 : 2723 : mCameFrom.clear();
169 : :
170 : 2723 : return pathFound;
171 : : }
172 : :
173 : 38423 : physics::Distance RouteAstar::costEstimate(lane::Lane::ConstPtr neighborLane, point::ParaPoint const &neighbor)
174 : : {
175 [ + - ]: 38423 : point::ECEFPoint pt_a = getParametricPoint(*neighborLane, neighbor.parametricOffset, physics::ParametricValue(0.5));
176 [ + - ]: 38423 : point::ECEFPoint pt_b = getParametricPoint(*mDestLane, getDest().parametricOffset, physics::ParametricValue(0.5));
177 [ + - ]: 38423 : physics::Distance d = distance(pt_a, pt_b);
178 : 38423 : return d;
179 : : }
180 : :
181 : 51273 : void RouteAstar::addNeighbor(lane::Lane::ConstPtr originLane,
182 : : RoutingPoint const &origin,
183 : : lane::Lane::ConstPtr neighborLane,
184 : : RoutingPoint const &neighbor,
185 : : ExpandReason const &expandReason)
186 : : {
187 : : (void)originLane;
188 : : (void)expandReason;
189 : :
190 [ + - + + ]: 51273 : if (mProcessedPoints.find(neighbor.first) == mProcessedPoints.end())
191 : : {
192 [ + - ]: 47663 : auto insertResult = mProcessingMap.insert(neighbor);
193 : 47663 : if ( // insertion succeeded
194 [ + + + + ]: 60714 : insertResult.second ||
195 : : // actual distance of new neighbor is smaller than the found duplicate
196 [ + - + + ]: 13051 : (neighbor.second.routeDistance < insertResult.first->second.routeDistance))
197 : : {
198 [ + - ]: 35700 : auto const estimatedDestCost = costEstimate(neighborLane, neighbor.first.point);
199 : 35700 : insertResult.first->second.routeDistance = neighbor.second.routeDistance;
200 [ + - ]: 35700 : insertResult.first->second.costData.estimatedDistanceToTarget = neighbor.second.routeDistance + estimatedDestCost;
201 [ + - ]: 35700 : mCameFrom[neighbor.first] = origin.first;
202 : : #if DEBUG_OUTPUT
203 : : std::cout << "Inserted: " << *insertResult.first << std::endl;
204 : : #endif
205 : : }
206 : : }
207 : 51273 : }
208 : :
209 : 2709 : void RouteAstar::reconstructPath(RoutingPoint const &dest)
210 : : {
211 : 5418 : RawRoute rawRoute;
212 : 2709 : rawRoute.routeDistance = dest.second.routeDistance;
213 : 2709 : rawRoute.routeDuration = dest.second.routeDuration;
214 : 2709 : for (auto current = dest.first;;)
215 : : {
216 [ + - ]: 18737 : rawRoute.paraPointList.insert(rawRoute.paraPointList.begin(), current.point);
217 [ + - ]: 18737 : auto it = mCameFrom.find(current);
218 [ + + ]: 18737 : if (it == mCameFrom.end())
219 : : {
220 : 2709 : break;
221 : : }
222 : : else
223 : : {
224 : 16028 : current = it->second;
225 : : }
226 : 16028 : }
227 : 2709 : mDest = dest.first;
228 : 2709 : mValid = true;
229 [ + - ]: 2709 : mRawRoutes.push_back(rawRoute);
230 : 2709 : }
231 : :
232 : : } // namespace planning
233 : : } // namespace route
234 : : } // namespace map
235 : : } // namespace ad
|
