Apollo 10.0
自动驾驶开放平台
a_star_strategy.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17#include <algorithm>
18#include <cmath>
19#include <limits>
20#include <queue>
21
26
27namespace apollo {
28namespace routing {
29namespace {
30
31struct SearchNode {
32 const TopoNode* topo_node = nullptr;
33 double f = std::numeric_limits<double>::max();
34
35 SearchNode() = default;
36 explicit SearchNode(const TopoNode* node)
37 : topo_node(node), f(std::numeric_limits<double>::max()) {}
38 SearchNode(const SearchNode& search_node) = default;
39
40 bool operator<(const SearchNode& node) const {
41 // in order to let the top of priority queue is the smallest one!
42 return f > node.f;
43 }
44
45 bool operator==(const SearchNode& node) const {
46 return topo_node == node.topo_node;
47 }
48};
49
50double GetCostToNeighbor(const TopoEdge* edge) {
51 return (edge->Cost() + edge->ToNode()->Cost());
52}
53
54const TopoNode* GetLargestNode(const std::vector<const TopoNode*>& nodes) {
55 double max_range = 0.0;
56 const TopoNode* largest = nullptr;
57 for (const auto* node : nodes) {
58 const double temp_range = node->EndS() - node->StartS();
59 if (temp_range > max_range) {
60 max_range = temp_range;
61 largest = node;
62 }
63 }
64 return largest;
65}
66
67bool AdjustLaneChangeBackward(
68 std::vector<const TopoNode*>* const result_node_vec) {
69 for (int i = static_cast<int>(result_node_vec->size()) - 2; i > 0; --i) {
70 const auto* from_node = result_node_vec->at(i);
71 const auto* to_node = result_node_vec->at(i + 1);
72 const auto* base_node = result_node_vec->at(i - 1);
73 const auto* from_to_edge = from_node->GetOutEdgeTo(to_node);
74 if (from_to_edge == nullptr) {
75 // may need to recalculate edge,
76 // because only edge from origin node to subnode is saved
77 from_to_edge = to_node->GetInEdgeFrom(from_node);
78 }
79 if (from_to_edge == nullptr) {
80 AERROR << "Get null ptr to edge:" << from_node->LaneId() << " ("
81 << from_node->StartS() << ", " << from_node->EndS() << ")"
82 << " --> " << to_node->LaneId() << " (" << to_node->StartS()
83 << ", " << to_node->EndS() << ")";
84 return false;
85 }
86 if (from_to_edge->Type() != TopoEdgeType::TET_FORWARD) {
87 if (base_node->EndS() - base_node->StartS() <
88 from_node->EndS() - from_node->StartS()) {
89 continue;
90 }
91 std::vector<const TopoNode*> candidate_set;
92 candidate_set.push_back(from_node);
93 const auto& out_edges = base_node->OutToLeftOrRightEdge();
94 for (const auto* edge : out_edges) {
95 const auto* candidate_node = edge->ToNode();
96 if (candidate_node == from_node) {
97 continue;
98 }
99 if (candidate_node->GetOutEdgeTo(to_node) != nullptr) {
100 candidate_set.push_back(candidate_node);
101 }
102 }
103 const auto* largest_node = GetLargestNode(candidate_set);
104 if (largest_node == nullptr) {
105 return false;
106 }
107 if (largest_node != from_node) {
108 result_node_vec->at(i) = largest_node;
109 }
110 }
111 }
112 return true;
113}
114
115bool AdjustLaneChangeForward(
116 std::vector<const TopoNode*>* const result_node_vec) {
117 for (size_t i = 1; i < result_node_vec->size() - 1; ++i) {
118 const auto* from_node = result_node_vec->at(i - 1);
119 const auto* to_node = result_node_vec->at(i);
120 const auto* base_node = result_node_vec->at(i + 1);
121 const auto* from_to_edge = from_node->GetOutEdgeTo(to_node);
122 if (from_to_edge == nullptr) {
123 // may need to recalculate edge,
124 // because only edge from origin node to subnode is saved
125 from_to_edge = to_node->GetInEdgeFrom(from_node);
126 }
127 if (from_to_edge == nullptr) {
128 AERROR << "Get null ptr to edge:" << from_node->LaneId() << " ("
129 << from_node->StartS() << ", " << from_node->EndS() << ")"
130 << " --> " << to_node->LaneId() << " (" << to_node->StartS()
131 << ", " << to_node->EndS() << ")";
132 return false;
133 }
134 if (from_to_edge->Type() != TopoEdgeType::TET_FORWARD) {
135 if (base_node->EndS() - base_node->StartS() <
136 to_node->EndS() - to_node->StartS()) {
137 continue;
138 }
139 std::vector<const TopoNode*> candidate_set;
140 candidate_set.push_back(to_node);
141 const auto& in_edges = base_node->InFromLeftOrRightEdge();
142 for (const auto* edge : in_edges) {
143 const auto* candidate_node = edge->FromNode();
144 if (candidate_node == to_node) {
145 continue;
146 }
147 if (candidate_node->GetInEdgeFrom(from_node) != nullptr) {
148 candidate_set.push_back(candidate_node);
149 }
150 }
151 const auto* largest_node = GetLargestNode(candidate_set);
152 if (largest_node == nullptr) {
153 return false;
154 }
155 if (largest_node != to_node) {
156 result_node_vec->at(i) = largest_node;
157 }
158 }
159 }
160 return true;
161}
162
163bool AdjustLaneChange(std::vector<const TopoNode*>* const result_node_vec) {
164 if (result_node_vec->size() < 3) {
165 return true;
166 }
167 if (!AdjustLaneChangeBackward(result_node_vec)) {
168 AERROR << "Failed to adjust lane change backward";
169 return false;
170 }
171 if (!AdjustLaneChangeForward(result_node_vec)) {
172 AERROR << "Failed to adjust lane change backward";
173 return false;
174 }
175 return true;
176}
177
178bool Reconstruct(
179 const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
180 const TopoNode* dest_node, std::vector<NodeWithRange>* result_nodes) {
181 std::vector<const TopoNode*> result_node_vec;
182 result_node_vec.push_back(dest_node);
183
184 auto iter = came_from.find(dest_node);
185 while (iter != came_from.end()) {
186 result_node_vec.push_back(iter->second);
187 iter = came_from.find(iter->second);
188 }
189 std::reverse(result_node_vec.begin(), result_node_vec.end());
190 if (!AdjustLaneChange(&result_node_vec)) {
191 AERROR << "Failed to adjust lane change";
192 return false;
193 }
194 result_nodes->clear();
195 for (const auto* node : result_node_vec) {
196 result_nodes->emplace_back(node->OriginNode(), node->StartS(),
197 node->EndS());
198 }
199 return true;
200}
201
202} // namespace
203
205 : change_lane_enabled_(enable_change) {}
206
207void AStarStrategy::Clear() {
208 closed_set_.clear();
209 open_set_.clear();
210 came_from_.clear();
211 enter_s_.clear();
212 g_score_.clear();
213}
214
215double AStarStrategy::HeuristicCost(const TopoNode* src_node,
216 const TopoNode* dest_node) {
217 const auto& src_point = src_node->AnchorPoint();
218 const auto& dest_point = dest_node->AnchorPoint();
219 double distance = std::fabs(src_point.x() - dest_point.x()) +
220 std::fabs(src_point.y() - dest_point.y());
221 return distance;
222}
223
225 const SubTopoGraph* sub_graph,
226 const TopoNode* src_node, const TopoNode* dest_node,
227 std::vector<NodeWithRange>* const result_nodes) {
228 Clear();
229 AINFO << "Start A* search algorithm.";
230
231 std::priority_queue<SearchNode> open_set_detail;
232
233 SearchNode src_search_node(src_node);
234 src_search_node.f = HeuristicCost(src_node, dest_node);
235 open_set_detail.push(src_search_node);
236
237 open_set_.insert(src_node);
238 g_score_[src_node] = 0.0;
239 enter_s_[src_node] = src_node->StartS();
240
241 SearchNode current_node;
242 std::unordered_set<const TopoEdge*> next_edge_set;
243 std::unordered_set<const TopoEdge*> sub_edge_set;
244 while (!open_set_detail.empty()) {
245 current_node = open_set_detail.top();
246 const auto* from_node = current_node.topo_node;
247 if (current_node.topo_node == dest_node) {
248 if (!Reconstruct(came_from_, from_node, result_nodes)) {
249 AERROR << "Failed to reconstruct route.";
250 return false;
251 }
252 return true;
253 }
254 open_set_.erase(from_node);
255 open_set_detail.pop();
256
257 if (closed_set_.count(from_node) != 0) {
258 // if showed before, just skip...
259 continue;
260 }
261 closed_set_.emplace(from_node);
262
263 // if residual_s is less than FLAGS_min_length_for_lane_change, only move
264 // forward
265 const auto& neighbor_edges =
266 (GetResidualS(from_node) > FLAGS_min_length_for_lane_change &&
267 change_lane_enabled_)
268 ? from_node->OutToAllEdge()
269 : from_node->OutToSucEdge();
270 double tentative_g_score = 0.0;
271 next_edge_set.clear();
272 for (const auto* edge : neighbor_edges) {
273 sub_edge_set.clear();
274 sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set);
275 next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());
276 }
277
278 for (const auto* edge : next_edge_set) {
279 const auto* to_node = edge->ToNode();
280 if (closed_set_.count(to_node) == 1) {
281 continue;
282 }
283 if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) {
284 continue;
285 }
286 tentative_g_score =
287 g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
288 if (edge->Type() != TopoEdgeType::TET_FORWARD) {
289 tentative_g_score -=
290 (edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
291 }
292 double f = tentative_g_score + HeuristicCost(to_node, dest_node);
293 if (open_set_.count(to_node) != 0 && f >= g_score_[to_node]) {
294 continue;
295 }
296 // if to_node is reached by forward, reset enter_s to start_s
297 if (edge->Type() == TopoEdgeType::TET_FORWARD) {
298 enter_s_[to_node] = to_node->StartS();
299 } else {
300 // else, add enter_s with FLAGS_min_length_for_lane_change
301 double to_node_enter_s =
302 (enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
303 from_node->Length() * to_node->Length();
304 // enter s could be larger than end_s but should be less than length
305 to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
306 // if enter_s is larger than end_s and to_node is dest_node
307 if (to_node_enter_s > to_node->EndS() && to_node == dest_node) {
308 continue;
309 }
310 enter_s_[to_node] = to_node_enter_s;
311 }
312
313 g_score_[to_node] = f;
314 SearchNode next_node(to_node);
315 next_node.f = f;
316 open_set_detail.push(next_node);
317 came_from_[to_node] = from_node;
318 if (open_set_.count(to_node) == 0) {
319 open_set_.insert(to_node);
320 }
321 }
322 }
323 AERROR << "Failed to find goal lane with id: " << dest_node->LaneId();
324 return false;
325}
326
327double AStarStrategy::GetResidualS(const TopoNode* node) {
328 double start_s = node->StartS();
329 const auto iter = enter_s_.find(node);
330 if (iter != enter_s_.end()) {
331 if (iter->second > node->EndS()) {
332 return 0.0;
333 }
334 start_s = iter->second;
335 } else {
336 AWARN << "lane " << node->LaneId() << "(" << node->StartS() << ", "
337 << node->EndS() << "not found in enter_s map";
338 }
339 double end_s = node->EndS();
340 const TopoNode* succ_node = nullptr;
341 for (const auto* edge : node->OutToAllEdge()) {
342 if (edge->ToNode()->LaneId() == node->LaneId()) {
343 succ_node = edge->ToNode();
344 break;
345 }
346 }
347 if (succ_node != nullptr) {
348 end_s = succ_node->EndS();
349 }
350 return (end_s - start_s);
351}
352
353double AStarStrategy::GetResidualS(const TopoEdge* edge,
354 const TopoNode* to_node) {
355 if (edge->Type() == TopoEdgeType::TET_FORWARD) {
356 return std::numeric_limits<double>::max();
357 }
358 double start_s = to_node->StartS();
359 const auto* from_node = edge->FromNode();
360 const auto iter = enter_s_.find(from_node);
361 if (iter != enter_s_.end()) {
362 double temp_s = iter->second / from_node->Length() * to_node->Length();
363 start_s = std::max(start_s, temp_s);
364 } else {
365 AWARN << "lane " << from_node->LaneId() << "(" << from_node->StartS()
366 << ", " << from_node->EndS() << "not found in enter_s map";
367 }
368 double end_s = to_node->EndS();
369 const TopoNode* succ_node = nullptr;
370 for (const auto* edge : to_node->OutToAllEdge()) {
371 if (edge->ToNode()->LaneId() == to_node->LaneId()) {
372 succ_node = edge->ToNode();
373 break;
374 }
375 }
376 if (succ_node != nullptr) {
377 end_s = succ_node->EndS();
378 }
379 return (end_s - start_s);
380}
381
382} // namespace routing
383} // namespace apollo
double f
const TopoNode * topo_node
virtual bool Search(const TopoGraph *graph, const SubTopoGraph *sub_graph, const TopoNode *src_node, const TopoNode *dest_node, std::vector< NodeWithRange > *const result_nodes)
void GetSubInEdgesIntoSubGraph(const TopoEdge *edge, std::unordered_set< const TopoEdge * > *const sub_edges) const
const std::string & LaneId() const
Definition topo_node.cc:162
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
#define AWARN
Definition log.h:43
bool operator==(Angle< T > lhs, Angle< T > rhs)
Tests two Angle objects for equality
Definition angle.h:255
bool operator<(const MatchCost &m1, const MatchCost &m2)
class register implement
Definition arena_queue.h:37
Definition future.h:29