From a2ad8245b6457d300663ca680d31f273e489c2f5 Mon Sep 17 00:00:00 2001 From: NorahXiong Date: Wed, 3 Aug 2022 12:50:36 +0800 Subject: [PATCH] fix non-successive waypoints problem in hybrid A* algorithm Signed-off-by: NorahXiong --- planning/freespace_planning_algorithms/src/astar_search.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index f9dd179515200..3138190ceb705 100644 --- 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;