TrajCost(current_node, next_node)); //节点搜索的代价值 // evaluate heuristic cost double optimal_path_cost = 0.0; optimal_path_cost += HoloObstacleHeuristic(next_node); //栅格搜索的路径代价值,最短栅格路径 next_node->SetHeuCos