Robotic Path Planning - A* (Star)

Question

I'm implementing A* path planning algorithm for my main robots exploration behavior in C++. As the robot moves, it maps the environment around itself as a 2D graph. From this graph, I have set a Vector2D Tuple {x, y} which holds the location of this waypoint, where I want the robot to navigate too.

The first thing I do with A* is to have a Node class, which holds information about the current node;

double f; //  F, final score
double g; // Movement cost
double h; // Hueristic cost (Manhatten)
Node* parent;
Vector2d position;

As A* starts, I have my starting node as my Robots starting position (I also hold this position as a Vector for easy access). Then, I enter a while loop until the end goal is found. The first thing I do in this loop is to generate eight adjacent Nodes (Left, Bottom, Right, Top, Top-left, Top-Right, Bottom-Left, Bottom Right), I then return this in a OpenList vector.

// Open List is current nodes to check std::vector positions;

positions.push_back(Vector2d(current->position.getX() - gridSize, current->position.getY())); // Left of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() + gridSize, current->position.getY())); // right of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX(), current->position.getY() + gridSize)); // Top of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX(), current->position.getY() - gridSize)); // Bottom of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() + gridSize,current->position.getY() + gridSize)); // Top Right of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() - gridSize,current->position.getY() + gridSize)); // Top Left of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() + gridSize,current->position.getY() - gridSize)); // Bottom Right of my current grid space (parent node)
positions.push_back(Vector2d(current->position.getX() - gridSize,current->position.getY() - gridSize)); // Bottom Left of my current grid space (parent node)

// moving diagnolly has a bigger cost
int movementCost[8] = { 10, 10, 10, 10, 14, 14, 14, 14 };

// loop through all my positions and calculate their g, h and finally, f score.
for (int i = 0; i < positions.size(); i++)
{
    Node* node = new Node(positions[i]);

    node->parent = current;
    node->movementCost = movementCost[i];
    if (!presentInClosedList(node))
    {
        // if the probability value of the current node is less then 0.5 (not an obstacle) then add to the open list, else skip it as an obstacle
        // Set astar grid occupancy
        if (grid->getProbabilityValue(node->position) < 0.51)
        {
            node->g = current->g + movementCost[i];
            node->h = (abs(positions[i].getX() - wantedLocation.getX())) + (abs(positions[i].getY() - wantedLocation.getY()));
            node->f = node->g + node->h;

            openList.push_back(node);
        }
    }
}

This is the code to see if the current node is present in my closedList

bool exists = false;

for (int i = 0; i < closedList.size(); i++)
{
    if (closedList[i]->position == currentNode->position)
    {
        closedList[i]->f = currentNode->f;
        closedList[i]->g = currentNode->g;
        closedList[i]->h = currentNode->h;
        closedList[i]->parent = currentNode->parent;

        exists = true;
        break;
    }
}

return exists;

This returns an openlist of possible routes. Next, I select the one with the smallest F score, and add this to my closedList. I keep doing this routine until the end goal has been found. Finally, once found I go back down the list using the parent objects. Here is the rest of the code

    // If my parents location is the same as my wanted location, then we've found our position.
    if (locationFound(current, wantedLocation))
    {
        // Now we need to backtrack from our wantedNode looking at the parents of each node to reconstruct the AStar path
        Node* p = current->parent;
        rollingDist = p->g;

        while (!wantedFound)
        {
            if (p->position == startLocation)
            {
                wantedFound = true;
                wantedNodeFound = true;
                break;
            }

            path.push_back(p);
            p = p->parent;

        }

    }

Now this is my issue. On every attempt it always finds the wanted location, but never the shortest path. See figure one below.

Figure one. Where the yellow marker is the wanted location, and the red darts is the "Path" to my wanted location, and finally, the "Blue" marker is where A star began.

This is my issue. I can't seem to reconstruct this path.

Answer 1

To recap the comments, there are two important problems

• Manhattan distance is not admissible for your movement costs, since the actual shortest path can take a diagonal shortcut that Manhattan distance wouldn't take into account.
• Before adding a new node to the Open list, it not only necessary to check whether it is in the Closed list, but also whether it is already in the Open list. If it is already in the Open list, the G's have to be compared and the smallest must be chosen (together with the corresponding parent pointer).[1]

Since you have octile movement with 10/14 costs, your heuristic function could be (from http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html)

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

With D = 10, D2 = 14. Of course you can also use anything else admissible but this formula already reflects the actual distance on an open plain so it can't easily be improved.

Finding and updating nodes in the Open list is an annoying part of A* that I'm sure many people would like to pretend isn't necessary, since it means you can't reasonably use any pre-defined priority queue (they lack efficient lookup). It can be done by having a manually implemented binary heap and a hash table that maps coordinates to their corresponding indexes in the heap. The hash table has to be updated by the heap whenever a node is moved.

[1]: the relevant snippet of pseudo code from the wikipedia article is:

    tentative_gScore := gScore[current] + dist_between(current, neighbor)
    if neighbor not in openSet  // Discover a new node
        openSet.Add(neighbor)
    else if tentative_gScore >= gScore[neighbor]
        continue        // This is not a better path.

    // This path is the best until now. Record it!
    cameFrom[neighbor] := current
    gScore[neighbor] := tentative_gScore
    fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)