C++ A* algorithm not always having the target node in path

Question

I implemented an A* algorithm in C++ but the vector doesn't always have the target node in it resulting in an endless loop. The library I'm using is: https://nullptr.club/libkmint/index.html. The function shortestPathToDist() should do the following: find the shortest path from the source node to the target node using A* ad return that path. Is this a correct implementation of A* or did I do something wrong and what?

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 node\n";
                //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;
        }
    }
}

Answer 1

queue.erase(queue.begin());. This is a bug. You are erasing the oldest added object, while you should pop the current shortest path node.

You should also remove the shortest path node from the visited set!

The real issue is that you are not using the correct data structures.

std::vector<const map::map_node*> queue;

should become

using scored_node = std::pair<double, const map::map_node*>
std::priority_queue<scored_node, std::vector<scored_node>, std::greater<scored_node>> queue;

With this change, you don't need smallestDistance anymore, and you should use

const auto shortest_path_scored_node = queue.top();
const auto shortest_path_node = shortest_path_scored_node.second;
queue.pop();
visited.erase(shortest_path_node);

Rather than use a vector

std::vector<const map::map_node*> visited;

you should use an unordered set, given you just care whether the element was visited. You are guaranteed uniqueness and fast retrieval without effort.

std::unodered_set<const map::map_node*> visited;