fix non-successive waypoints problem in hybrid A* algorithm

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>
autowarefoundation · Aug 3, 2022 · a2ad824 · a2ad824
1 parent e7dfa19
commit a2ad824
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp
@@ -274,13 +274,12 @@ bool AstarSearch::search()
 
       // Compare cost
       AstarNode * next_node = getNodeRef(next_index);
-      const double next_gc = current_node->gc + move_cost;
-      if (next_node->status == NodeStatus::None || next_gc < next_node->gc) {
+      if (next_node->status == NodeStatus::None) {
         next_node->status = NodeStatus::Open;
         next_node->x = next_pose.position.x;
         next_node->y = next_pose.position.y;
         next_node->theta = tf2::getYaw(next_pose.orientation);
-        next_node->gc = next_gc;
+        next_node->gc = current_node->gc + move_cost;
         next_node->hc = estimateCost(next_pose);
         next_node->is_back = transition.is_back;
         next_node->parent = current_node;