I am trying to implement Join State A* for multiple agents.
It returns the shortest path possible but it do not removes the conflicts any help to correct this code will be highly appreciated.

To avoid conflict I am using the following algorithm:

AStar::CoordinateList AStar::Generator::findPath(const std::vector<Vec2i>& source_, const std::vector<Vec2i>& target_)
{
    struct JointStateNode
    {
        uint G, H;
        std::vector<Vec2i> coordinates;
        JointStateNode* parent;

        JointStateNode(const std::vector<Vec2i>& coord_, JointStateNode* parent_ = nullptr)
            : coordinates(coord_), parent(parent_), G(0), H(0) {}

        uint getScore() const {
            return G + H;
        }
    };

    using JointStateNodeSet = std::vector<JointStateNode*>;

    auto findNodeOnList = [](JointStateNodeSet& nodes_, const std::vector<Vec2i>& coordinates_) -> JointStateNode* {
        for (auto node : nodes_) {
            if (node->coordinates == coordinates_) {
                return node;
            }
        }
        return nullptr;
    };

    auto releaseNodes = [](JointStateNodeSet& nodes_) {
        for (auto it = nodes_.begin(); it != nodes_.end();) {
            delete *it;
            it = nodes_.erase(it);
        }
    };

    JointStateNode* current = nullptr;
    JointStateNodeSet openSet, closedSet;
    openSet.reserve(100);
    closedSet.reserve(100);
    openSet.push_back(new JointStateNode(source_));

    while (!openSet.empty()) {
        auto current_it = openSet.begin();
        current = *current_it;

        for (auto it = openSet.begin(); it != openSet.end(); it++) {
            auto node = *it;
            if (node->getScore() <= current->getScore()) {
                current = node;
                current_it = it;
            }
        }

        if (current->coordinates == target_) {
            break;
        }

        closedSet.push_back(current);
        openSet.erase(current_it);

        for (uint i = 0; i < directions; ++i) {
            std::vector<Vec2i> newCoordinates;
            for (const auto& coord : current->coordinates) {
                newCoordinates.push_back(coord + direction[i]);
            }

            if (detectVertexCollision(newCoordinates) || detectEdgeCollision(current->coordinates, newCoordinates) ||
                std::any_of(newCoordinates.begin(), newCoordinates.end(), 
                [&](const Vec2i& coord) { return detectCollision(coord); }) || 
                findNodeOnList(closedSet, newCoordinates)) {
                std::cout<<"Collision detected can't set this vertex {"<<newCoordinates[0].x<<", "<<newCoordinates[0].y<<"}n";
                continue;
            }

            uint totalCost = current->G + 1;

            JointStateNode* successor = findNodeOnList(openSet, newCoordinates);
            if (successor == nullptr) {
                successor = new JointStateNode(newCoordinates, current);
                successor->G = totalCost;
                successor->H = heuristic(successor->coordinates, target_);
                openSet.push_back(successor);
            } else if (totalCost < successor->G) {
                successor->parent = current;
                successor->G = totalCost;
            }
        }
    }

    CoordinateList path;
    while (current != nullptr) {
        path.push_back(current->coordinates);
        current = current->parent;
    }

    releaseNodes(openSet);
    releaseNodes(closedSet);

    std::reverse(path.begin(), path.end());
    return path;
}

After testing on 5×5 grid for three agents with starting points and target points:

    std::vector<AStar::Vec2i> start1 = {{4, 0}};
    std::vector<AStar::Vec2i> target1 = {{2, 2}};
    std::vector<AStar::Vec2i> start2 = {{4, 1}};
    std::vector<AStar::Vec2i> target2 = {{0, 0}};
    std::vector<AStar::Vec2i> start3 = {{2, 3}};
    std::vector<AStar::Vec2i> target3 = {{1, 1}};

I get the following output:
Path 1: (4, 0) (3, 0) (2, 0) (2, 1) (2, 2)
Path 2: (4, 1) (3, 1) (2, 1) (1, 1) (0, 1) (0, 0)
Path 3: (2, 3) (1, 3) (1, 2) (1, 1)

As it can be seen that (1,1) in path 2 and (1,1) in path are at the same position which is indeed a conflict. I want it to remove the conflict according functions logic and choose some different path.