C++A*算法并不总是在路径中具有目标节点

我在C++中实现了一个A*算法，但向量中并不总是有目标节点，这导致了一个无休止的循环。我正在使用的库是：https://nullptr.club/libkmint/index.html.函数shortestPathToDist((应该执行以下操作：使用A*查找从源节点到目标节点的最短路径并返回该路径。这是a*的正确实现吗？还是我做错了什么？

Helper.hpp:

#ifndef UFO_HELPER_HPP
#define UFO_HELPER_HPP
#include <map>
#include <iostream>
#include "kmint/map/map.hpp"
namespace kmint {
namespace ufo {
class helper {
public:
helper();
std::vector<const map::map_node*> shortestPathToDist(const kmint::map::map_node& source_node, const kmint::map::map_node& target_node);
private:
const map::map_node* helper::smallestDistance(std::map<const map::map_node*, float>& actualCost, std::map<const map::map_node*, float>& heuristicCost, std::vector<const kmint::map::map_node*>& queue);
float heuristic(const map::map_node& source_node, const map::map_node& target_node);
};
}
}
#endif

Helper.cpp:

#include "kmint/ufo/helper.hpp"
namespace kmint {
namespace ufo {
helper::helper() {
}
std::vector<const map::map_node*> helper::shortestPathToDist(const map::map_node& source_node, const map::map_node& target_node)
{
std::vector<const map::map_node*> path;
std::vector<const map::map_node*> visited;
std::vector<const map::map_node*> queue;
std::map<const map::map_node*, const map::map_node*> previous;
std::map<const map::map_node*, float> cost;
std::map<const map::map_node*, float> heuristic_cost;
queue.push_back(&source_node);
cost[&source_node] = 0;
heuristic_cost[&source_node] = heuristic(source_node, target_node);
while (queue.size() > 0) {
const map::map_node* shortest_path_node = smallestDistance(cost, heuristic_cost, queue);
for (int i = 0; i < shortest_path_node->num_edges(); i++) {
map::map_edge edge = (*shortest_path_node)[i];
const map::map_node *node_to = &edge.to();
if (std::find(visited.begin(), visited.end(), node_to) == visited.end()
&& std::find(queue.begin(), queue.end(), node_to) == queue.end()) {
queue.push_back(node_to);
}
if (cost.find(node_to) == cost.end() || cost[node_to] > cost[shortest_path_node] + edge.weight())
{
cost[node_to] = cost[shortest_path_node] + edge.weight();
heuristic_cost[node_to] = heuristic(*shortest_path_node, target_node);
previous[node_to] = shortest_path_node;
}
if (node_to->node_id() == target_node.node_id())
{
cost[node_to] = cost[shortest_path_node] + edge.weight();
heuristic_cost[node_to] = heuristic(*shortest_path_node, target_node);
previous[node_to] = shortest_path_node;
break;
}
}
queue.erase(queue.begin());
visited.push_back(shortest_path_node);
}
// shortest path to target_node
const map::map_node* current_node = nullptr;
for (auto const&[key, val] : previous) {
if (key != nullptr && key != NULL) {
if (key->node_id() == target_node.node_id()) {
current_node = val;
break;
}
}
}
path.clear();
if (current_node == nullptr || current_node == NULL) {
std::cout << "could not find target noden";
//this->shortest_path_to_target(source_node, target_node);
return path;
}
while (current_node != &source_node) {
if (current_node != nullptr && current_node != NULL) {
if (path.size() > 0) {
bool found = false;
for (auto &p : path) {
if (p != NULL && p != nullptr && p->node_id() == current_node->node_id()) {
found = true;
break;
}
}
if (!found) {
path.insert(path.begin(), current_node);
}
}
else {
path.insert(path.begin(), current_node);
}
}
for (auto const&[key, val] : previous) {
if (key != nullptr && key != NULL && current_node != nullptr && current_node != NULL) {
if (key->node_id() == current_node->node_id()) {
current_node = val;
break;
}
}
}
}
return path;
}
// manhatan heuristic
float helper::heuristic(const map::map_node& fNode, const map::map_node& sNode)
{
return std::abs(fNode.location().x() - sNode.location().x()) + std::abs(fNode.location().y() - sNode.location().y());
}
const map::map_node* helper::smallestDistance(std::map<const map::map_node*, float>& actualCost, std::map<const map::map_node*, float>& heuristicCost, std::vector<const map::map_node*>& queue)
{
const map::map_node* sDN = nullptr;
for (int i = 0; i < queue.size(); i++)
{
if (sDN == nullptr || actualCost[queue[i]] + heuristicCost[queue[i]] < actualCost[sDN] + heuristicCost[sDN]) {
sDN = queue[i];
}
}
return sDN;
}
}
}