Skip to content

Commit

Permalink
adding looping fix to foxy (ros-navigation#2075)
Browse files Browse the repository at this point in the history
  • Loading branch information
SteveMacenski authored Nov 5, 2020
1 parent 0ed83f0 commit 3d166b7
Showing 1 changed file with 13 additions and 3 deletions.
16 changes: 13 additions & 3 deletions smac_planner/src/a_star.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -411,10 +411,20 @@ AStarAlgorithm<NodeSE2>::NodePtr AStarAlgorithm<NodeSE2>::getAnalyticPath(
prev = node;
for (const auto & node_pose : possible_nodes) {
const auto & n = node_pose.first;
n->parent = prev;
prev = n;
if (!n->wasVisited()) {
// Make sure this node has not been visited by the regular algorithm.
// If it has been, there is the (slight) chance that it is in the path we are expanding
// from, so we should skip it.
// Skipping to the next node will still create a kinematically feasible path.
n->parent = prev;
n->visited();
prev = n;
}
}
if (_goal != prev) {
_goal->parent = prev;
_goal->visited();
}
_goal->parent = prev;
return _goal;
}

Expand Down

0 comments on commit 3d166b7

Please sign in to comment.