33 double f = std::numeric_limits<double>::max();
35 SearchNode() =
default;
36 explicit SearchNode(
const TopoNode* node)
38 SearchNode(
const SearchNode& search_node) =
default;
40 bool operator<(
const SearchNode& node)
const {
45 bool operator==(
const SearchNode& node)
const {
50double GetCostToNeighbor(
const TopoEdge* edge) {
51 return (edge->Cost() + edge->ToNode()->Cost());
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;
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) {
77 from_to_edge = to_node->GetInEdgeFrom(from_node);
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() <<
")";
87 if (base_node->EndS() - base_node->StartS() <
88 from_node->EndS() - from_node->StartS()) {
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) {
99 if (candidate_node->GetOutEdgeTo(to_node) !=
nullptr) {
100 candidate_set.push_back(candidate_node);
103 const auto* largest_node = GetLargestNode(candidate_set);
104 if (largest_node ==
nullptr) {
107 if (largest_node != from_node) {
108 result_node_vec->at(i) = largest_node;
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) {
125 from_to_edge = to_node->GetInEdgeFrom(from_node);
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() <<
")";
135 if (base_node->EndS() - base_node->StartS() <
136 to_node->EndS() - to_node->StartS()) {
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) {
147 if (candidate_node->GetInEdgeFrom(from_node) !=
nullptr) {
148 candidate_set.push_back(candidate_node);
151 const auto* largest_node = GetLargestNode(candidate_set);
152 if (largest_node ==
nullptr) {
155 if (largest_node != to_node) {
156 result_node_vec->at(i) = largest_node;
163bool AdjustLaneChange(std::vector<const TopoNode*>*
const result_node_vec) {
164 if (result_node_vec->size() < 3) {
167 if (!AdjustLaneChangeBackward(result_node_vec)) {
168 AERROR <<
"Failed to adjust lane change backward";
171 if (!AdjustLaneChangeForward(result_node_vec)) {
172 AERROR <<
"Failed to adjust lane change backward";
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);
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);
189 std::reverse(result_node_vec.begin(), result_node_vec.end());
190 if (!AdjustLaneChange(&result_node_vec)) {
191 AERROR <<
"Failed to adjust lane change";
194 result_nodes->clear();
195 for (
const auto* node : result_node_vec) {
196 result_nodes->emplace_back(node->OriginNode(), node->StartS(),
205 : change_lane_enabled_(enable_change) {}
207void AStarStrategy::Clear() {
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());
227 std::vector<NodeWithRange>*
const result_nodes) {
229 AINFO <<
"Start A* search algorithm.";
231 std::priority_queue<SearchNode> open_set_detail;
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);
237 open_set_.insert(src_node);
238 g_score_[src_node] = 0.0;
239 enter_s_[src_node] = src_node->
StartS();
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.";
254 open_set_.erase(from_node);
255 open_set_detail.pop();
257 if (closed_set_.count(from_node) != 0) {
261 closed_set_.emplace(from_node);
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();
275 next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());
278 for (
const auto* edge : next_edge_set) {
279 const auto* to_node = edge->ToNode();
280 if (closed_set_.count(to_node) == 1) {
283 if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) {
287 g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
290 (edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
292 double f = tentative_g_score + HeuristicCost(to_node, dest_node);
293 if (open_set_.count(to_node) != 0 &&
f >= g_score_[to_node]) {
298 enter_s_[to_node] = to_node->StartS();
301 double to_node_enter_s =
302 (enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
303 from_node->Length() * to_node->Length();
305 to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
307 if (to_node_enter_s > to_node->EndS() && to_node == dest_node) {
310 enter_s_[to_node] = to_node_enter_s;
313 g_score_[to_node] =
f;
314 SearchNode next_node(to_node);
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);
323 AERROR <<
"Failed to find goal lane with id: " << dest_node->
LaneId();
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()) {
334 start_s = iter->second;
337 << node->
EndS() <<
"not found in enter_s map";
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();
347 if (succ_node !=
nullptr) {
348 end_s = succ_node->EndS();
350 return (end_s - start_s);
353double AStarStrategy::GetResidualS(
const TopoEdge* edge,
354 const TopoNode* to_node) {
356 return std::numeric_limits<double>::max();
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);
365 AWARN <<
"lane " << from_node->LaneId() <<
"(" << from_node->StartS()
366 <<
", " << from_node->EndS() <<
"not found in enter_s map";
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();
376 if (succ_node !=
nullptr) {
377 end_s = succ_node->EndS();
379 return (end_s - start_s);
const TopoNode * topo_node
AStarStrategy(bool enable_change)
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
bool operator==(Angle< T > lhs, Angle< T > rhs)
Tests two Angle objects for equality
bool operator<(const MatchCost &m1, const MatchCost &m2)