Implementation of A* algorithm in C++

Question

I have implemented the A-Star path finding algorithm for a 2D grid. The function returns a list of positions of an acceptable path.

main.cpp :

#include <cmath>
#include <cstdio>
#include <vector>
#include <iostream>
#include <algorithm>
#include <set>
#include <unordered_set>
#include <boost/circular_buffer.hpp>
#include <cstdlib>
#include <ctime>
#include <motion_planner.h>

using namespace std;

//extern int max_step_number;

/* **********************************************************
 * Test Input
 * Enter max_step_number for the random planner: 25 
 * Enter start positon: e.g: 2 0
 * 2 0 
 * Enter goal positon: e.g: 5 5
 * 5 5 
 * Enter world size: e.g: 6 6
 * 6 6
 *  0 0 1 0 0 0
    0 0 1 0 0 0
    0 0 0 0 1 0
    0 0 0 0 1 0
    1 0 1 1 1 0
    0 0 0 0 0 0
 * *************************************************************/

int main(void)
{
    motion_planner mp;
    int x,y,gx,gy,r,c;
    int val;
    //string vals;
    cout<<"Enter max_step_number for the random planner: ";
    cin>> mp.max_step_number;
    cout<<"\nEnter start positon: e.g: 2 1 \n";
    cin>>x>>y;
    cout<<"\nEnter goal positon: e.g: 5 4 \n";
    cin>>gx>>gy;
    cout<<"\nEnter world size: e.g: 6 6 \n";
    cin>>r>>c;
    pair<int,int> start = make_pair(x,y);
    pair<int,int> goal = make_pair(gx,gy);
    vector<vector<int> > world(r);
    //vector<vector<Node> > grid(r);
    for (int i = 0;i<r;i++)
    {
        for(int j = 0; j<c; j++)
        {
            cin>>val;
            world[i].push_back(val);
        }
    }    
    //cout<< world[4][0]<<"haaaaa"<<endl;

    //cout<< "Grid size"<< world.size()<<endl;

    vector<pair<int,int> >resRan = mp.randPlan(world,start,goal);
    //cout<<res.size()-1<<endl;

    pair<int,int> out;
    cout<< "Random Planner Path Traversed: (Showing position after start) "<<endl;
    for(int i= 0; i< resRan.size(); i++)
    {
        pair<int,int> out = resRan[i];
        cout<<out.first<<","<<out.second<<endl;
    }

    cout<< "\nOptimal Planner Path Traversed: (Showing position from start)"<<endl; 
    vector<pair<int,int> >resOpt = mp.optPlan(world,start,goal);
    //pair<int,int> out;
    for(int i=0; i < resOpt.size(); i++)
    {
        pair<int,int> out = resOpt[i];
        cout<<out.first<<","<<out.second<<endl;
    }
    //cout<<world[4][5]<<endl;
    //cout<<world.size();
    return 0;
}

motion_planner.h ; there are two functions, one is a random path search and one for astar

#ifndef MOTION_PLANNER_H
#define MOTION_PLANNER_H

#include <cmath>
#include <cstdio>
#include <vector>
#include <iostream>
#include <algorithm>
#include <set>
#include <unordered_set>
#include <boost/circular_buffer.hpp>
#include <cstdlib>
#include <ctime>


using namespace std;

class motion_planner{

    public:

    int max_step_number;                                                        //max steps after which random planner should stop
    typedef boost::circular_buffer<pair<int,int> > circular_buffer;

    /* 
     * Class for optimal planner (A* search). Each postion in the grid is a object of this class
     * */
    class Node
    {
      public:
        int g= 0, h=0;
        char val;                                                                //Char value in the grid
        pair<int,int> pos;
        Node *parent = 0; 

        Node(pair<int,int>nodePos,int value)
        {
            pos=nodePos;
            val=value;
        }
        //Node() : parent( make_unique<Node>()) {}
        int move_cost(Node other)
        {
            return 1;
        }

        pair<int,int> get_pos() const
        {
            return pos;
        }    
    };

    /* 
     * Custom Hasher for Class Node, to be used for Unordered_set. 
     * (Can even use priority_queues for storing the obejcts of class Node instead of hashset)
     * */
    struct NodeHasher
    {
      //template <typename T, typename U>
      size_t
      operator()( const Node &obj) const
      {
        pair<int,int> position;
        position = obj.get_pos();
        return 3* std::hash<int>()(position.first) + std::hash<int>()(position.second) ; //custom hasher key when we using pair
      }
    };

    /*
     *  Custom Comparator for Class Node, to be used for Unordered_set
     * */
    struct NodeComparator
    {
      bool
      operator()(const Node  &obj1, const  Node  &obj2) const
      {
        if (obj1.get_pos() == obj2.get_pos())
          return true;
        return false;
      }
    };

    /*
     *  Comparing the objects based on the f = h+g cost
     * */
    struct MinFValue
    {
      bool
      operator() (const Node &obj1, const Node &obj2) const
      {
          return obj1.h+obj1.g < obj2.h+obj2.g;
      }
    }minObj;

    /*
     * Return type for the random planner (not using it currently)
     * */
    struct returnType
    {
        bool t;
        vector<pair<int,int> > randPath;
    };

    vector<bool> foundInVisited (pair<int,int> nod, circular_buffer cb);
    vector<pair<int,int> > adjRandom(pair<int,int> node, vector<vector<int > > &world, circular_buffer visited);
    vector<pair<int,int> > randPlan(vector<vector<int> > &grid, pair<int,int> start, pair<int,int> goal);

    int heuris(pair<int,int> goal, pair<int,int> node);
    vector<pair<int,int> > adjAstar(pair<int,int> node, vector<vector<Node> > &world);
    vector<Node> aStar(vector<vector<Node> > &grid, pair<int,int> start, pair<int,int> goal);
    vector<pair<int,int> > optPlan(vector<vector<int> > &world, pair<int,int> start, pair<int,int> goal);
};

#endif

motion_planner.cpp

#include <cmath>
#include <cstdio>
#include <vector>
#include <iostream>
#include <algorithm>
#include <set>
#include <unordered_set>
#include <boost/circular_buffer.hpp>
#include <cstdlib>
#include <ctime>
#include <motion_planner.h>


using namespace std;

/* ***************************************************************************************************************************
 * Function to check which of the 4 adjacent cells of the current cell are in the visited list
 * 
 * @output: returns a boolean vector of size 4
 * @input: current cell, buffer of visited nodes( size- square root of max_step_number) 
 * ***************************************************************************************************************************/
vector<bool> motion_planner::foundInVisited (pair<int,int> nod, circular_buffer cb)
{
    vector<bool> boolAdj {true, true, true, true};
    int x = nod.first;
    int y = nod.second;

    for (int i = 0 ; i< cb.size(); i++)
    {
        //int r = cb[i].first(), c = cb[i].second();
        if(make_pair(x+1,y) == cb[i])
        {
            boolAdj[0] = false ;
        }
        if(make_pair(x,y+1) == cb[i])
        {
            boolAdj[1] = false;
        }
        if(make_pair(x-1,y) == cb[i])
        {
            boolAdj[2] = false;
        }
        if(make_pair(x,y-1) == cb[i])
        {
            boolAdj[3] = false;
        }

    }
    return boolAdj;
}

/* ***************************************************************************************************************************
 * Function to find which adjacent nodes of the current nodes are passable
 * 
 * @output: vector of passable nodes
 * @input: current node, the world state, buffer of visited nodes( size- square root of max_step_number) 
 * ***************************************************************************************************************************/
vector<pair<int,int> > motion_planner::adjRandom(pair<int,int> node, vector<vector<int > > &world, circular_buffer visited)
{
    vector<pair<int,int> > resNodes;
    //fromVisited = foundInVisited (node, visited);
    int x = node.first, y = node.second;
    bool t1 = false,t2=false,t3=false,t4 = false;
    vector<bool> resAdj = foundInVisited(node, visited);
    if ( (x+1) <= world.size()-1 && world[x+1][y] != 1 )
    {
        t1 = true;
        if(resAdj[0])
            resNodes.push_back(make_pair(x+1,y));
    }
    if ( (y+1) <= world[0].size()-1 && world[x][y+1] != 1 )
    {
        t2 = true;
        if(resAdj[1])
            resNodes.push_back(make_pair(x,y+1));
    }
    if ( (x-1) >= 0 && world[x-1][y] != 1 )
    {
        t3 = true;
        if(resAdj[2])
            resNodes.push_back(make_pair(x-1,y));
    }
    if ( (y-1) >= 0 && world[x][y-1] != 1 )
    {
        t4 = true;
        if(resAdj[3])
            resNodes.push_back(make_pair(x,y-1));
    }
    if(t1 == false && t2 ==t1 && t3 == t2 && t4 ==t1)
    {
        return resNodes;
    }
    else if (resNodes.size()==0)
    {
        vector<pair<int,int> > ts;
        if(t1 == true)  ts.push_back(make_pair(x+1,y));
        if(t2 == true)  ts.push_back(make_pair(x,y+1));
        if(t3 == true)  ts.push_back(make_pair(x-1,y));
        if(t4 == true)  ts.push_back(make_pair(x,y-1));
        random_shuffle(ts.begin(),ts.end());
        resNodes.push_back(*ts.begin());
        return resNodes;
    }
    else
        return resNodes;

}


/* *************************************************************************************************************************
 * Function implements random planner logic, uses a circular buffer to store the last visited nodes, random_shuffle to select next node from the vector of adjacent passable nodes, random)shuffle has no bias as opposed to rand, and size of this vector is always less than or equal to 4, time efficient. For a bigger vector, a random generator should be created to get a random element.
 * 
 * @output: vector of nodes traversed (size max_step_number or less)
 * @input: world state, start state, goal state
 * ************************************************************************************************************************/
vector<pair<int,int> > motion_planner::randPlan(vector<vector<int> > &grid, pair<int,int> start, pair<int,int> goal)
{
    vector<pair<int,int> > path;
    unsigned int squarert = abs(sqrt(max_step_number));
    circular_buffer visited(squarert);
    int sx=start.first, sy=start.second, gx=goal.first, gy=goal.second;
    int minF = 10000;
    pair<int,int> current = start;
    //path.push_back(current);
    visited.push_back(current);
    vector<pair<int,int> > childNodes;
    srand ( unsigned ( std::time(0) ) );
    if (current == goal)
    {
        return {current};
    }
    while(max_step_number-- > 0)
    {
        if (current == goal)
        {   
            cout<< "\n Path found with random planner!!!\n"<<endl;
            return path;
        }
        childNodes = adjRandom(current,grid,visited);
        random_shuffle(childNodes.begin(),childNodes.end()); 
        current = *childNodes.begin();
        visited.push_back(current);
        path.push_back(current);
    } 
    if(path[path.size()-1]!=goal) cout<<"\nGoal not reached using Random Planner(Max steps reached)!!!\n"<<endl;
    return path;
}


/* *************************************************************************************************************************
 * Function calculates heuristic of each node. Euclidean distance is used to calcualte the cost from current node to goal
 * 
 * @output: integer value heuristic
 * @input: goal state, current state
 * ************************************************************************************************************************/

int motion_planner::heuris(pair<int,int> goal, pair<int,int> node)
{
    int h = abs(goal.first-node.first) + abs(goal.second-node.second);
    return h;
}


/* *************************************************************************************************************************
 * Function returns the adjacent node positions of the current node that are passable for the A* search 
 * 
 * @output: vector of pairs(positions) of adjacent passable nodes 
 * @input: current position, world state
 * ************************************************************************************************************************/

vector<pair<int,int> > motion_planner::adjAstar(pair<int,int> node, vector<vector<motion_planner::Node> > &world)
{
    vector<pair<int,int> > adjNodes;
    int x = node.first, y = node.second;
    if ( (x+1) <= world.size()-1 && world[x+1][y].val != 1)
    {

        adjNodes.push_back(make_pair(x+1,y));

    }
    if ( (y+1) <= world[0].size()-1 && world[x][y+1].val != 1)
    {
        adjNodes.push_back(make_pair(x,y+1));
    }
    if ( (x-1) >= 0 && world[x-1][y].val != 1 )
    {

        adjNodes.push_back(make_pair(x-1,y));
    }
    if ( (y-1) >= 0 && world[x][y-1].val != 1 )
    {
        adjNodes.push_back(make_pair(x,y-1));
    }
    return adjNodes;
}


/* *************************************************************************************************************************
 * Function implements A* search logic, returns a vector of Node objects that are traversed.
 * 
 * @output: vector of nodes traversed
 * @input: 2D list of Node objects, start state, goal state
 * ************************************************************************************************************************/
vector<motion_planner::Node> motion_planner::aStar(vector<vector<motion_planner::Node> > &grid, pair<int,int> start, pair<int,int> goal)
{
    int sx=start.first, sy=start.second, gx=goal.first, gy=goal.second;
    int minF = 10000;

    unordered_set<motion_planner::Node,motion_planner::NodeHasher,motion_planner::NodeComparator> openList, closedList;
    //cout<< "grid size"<< grid[0].size()<<endl;
    openList.insert(grid[sx][sy]);
    //cout<<gx<<" "<<gy<<endl;

    vector<motion_planner::Node > path;
    int hihi=0;
    int i =0;
    while (!openList.empty())
    {
        i++;
        //cout<<"its here"<<endl;
        Node current = *min_element(openList.begin(), openList.end(), minObj);
        /*cout<<i<<endl;
        for (Node x : openList)
        {
            cout<<"h="<< x.h<< "g=" <<x.g << " "<< x.get_pos().first << "," << x.get_pos().second << "\t";

        }
        cout<< endl;
        cout<< current.get_pos().first<<" "<<current.get_pos().second<<endl; */
        int currX=current.get_pos().first, currY = current.get_pos().second;
        if (current.get_pos() == goal)
        {
            //cout<<"here";
            while(current.parent != NULL )
            {

                //cout<< current.get_pos().first<<" "<<current.get_pos().second<<endl;
                path.push_back(current);
                current = *current.parent;              
            }
            path.push_back(current);
            return path;
        }
        openList.erase(current);
        closedList.insert(current);
        //openList.erase(grid[currX][currY]);

        for (pair<int,int> nod : adjAstar(current.pos,grid))
        {
            hihi++;
            int r=nod.first, c = nod.second;
            //auto searchCL = find(closedList.begin(),closedList.end(),nod);
            auto searchCL = closedList.find(grid[r][c]);
            if(searchCL!=closedList.end())
                continue;

            auto searchOL = openList.find(grid[r][c]);
            if(searchOL!=openList.end())
            {
                //cout<<"hoorah"<<endl;
                int n = current.g+current.move_cost(grid[r][c]);
                if(grid[r][c].g>n)
                {
                    grid[r][c].g=n;
                    int a = current.get_pos().first; int b = current.get_pos().second;
                    grid[r][c].parent = &grid[a][b];
                }
            }

            else
            {         
                grid[r][c].g = current.g + current.move_cost(grid[r][c]);
                grid[r][c].h = heuris(goal,grid[r][c].pos);
                //cout<<&current<<endl;
                int a = current.get_pos().first; int b = current.get_pos().second;
                //cout<<"a "<<grid[r][c].get_pos().first<<" b "<<grid[r][c].get_pos().second<<endl;
                //cout<<"position of parent "<<grid[a][b].get_pos().first<<","<<grid[a][b].get_pos().second<<endl;
                grid[r][c].parent = &grid[a][b];
                openList.insert(grid[r][c]);          
            }

         }

     }
    //cout<<hihi;
    return path;

}


/* *************************************************************************************************************************
 * Function to get the positions of the nodes returned by the function aStar.
 * 
 * @output: vector of positions(pairs) of nodes traversed 
 * @input: world state, start state, goal state
 * ************************************************************************************************************************/
vector<pair<int,int> > motion_planner::optPlan(vector<vector<int> > &world, pair<int,int> start, pair<int,int> goal)
{
    vector<vector<motion_planner::Node> > grid(world.size());
    vector<pair<int,int> > res;
    vector<motion_planner::Node > path;
    for (int i = 0; i<world.size() ; i++)
    {
        for(int j =0; j<world[0].size(); j++)
        { 
            //Node* a = new Node(make_pair(i,j), world[i][j]);
            motion_planner::Node a = motion_planner::Node(make_pair(i,j),world[i][j]);
            grid[i].push_back(a);
        }
    }

    path = motion_planner::aStar(grid,start,goal);
    for(int i= path.size()-1; i>=0; i--)
    {
        res.push_back(path[i].get_pos());
    } 
    return res;

}

I have a long way to go in learning C++ and there must be many areas in which this function could have been optimized, e.g. the data structures used, code practices, C++ tricks, memory management, etc. to name a few.

Answer 1

Euclidean distance is used to calcualte the cost from current node to goal

The code implements Manhattan distance, so the comment is wrong, or perhaps the code is wrong, but in any case it doesn't match. You can use this page to review heuristics for grid worlds, I don't recommend Euclidean distance because it's either too optimistic (causing unnecessary node exploration) or even wrong (when the actual movement cost for a diagonal move is less than sqrt(2)).

pair<int,int>

Not wrong but most of these are actually 2D coordinates, which has more meaning than just "pair", so you could make an alias or your own class.

vector<pair<int,int> >

The >>-problem has been officially fixed since C++11 (compiler support predates the update of the standard) and you tagged this question C++14, so you don't need to do this. It's not wrong to continue doing it, just unnecessary.

unordered_set<..> openList: unordered_set does not support finding the minimum element in a reasonable way. You can get it out anyway, but it will happen by brute force. It is a bit tricky to do this efficiently, because finding the minimum element and finding a given element (or finding the element based on coordinates) back in order to change its parent and G and F, both need to be supported.

It can be done by maintaining a binary heap, and letting the nodes know their index in the heap, so that the node can be moved to its proper location in the heap after modifying its parent and G/F score. An unfortunate consequence of that construct is that it all needs to be manual, the standard library does have pop_heap and push_heap but those functions do not have any option to update some secondary data structure every time an element in the heap is moved to a different index.

That's all a bit complicated, a sneaky alternative is keeping only a heap, and just re-inserting a node when its G and parent are changed, so that it will be popped off the heap before the other version of that node that is already in the heap. It can happen then that you pop a node off the Open List and that node has actually been already closed, just skip it. The downside is filling up the Open List with dead weight, also if you implement an other suggestion from further down (to limit the use of nodes to only the open list) then the additional coordinates-to-index-in-the-heap map needs to exist anyway, which defeats the point of this dirty trick.

Repeatedly scanning the entire open list is a significant performance problem, easily leading to quadratic time complexity (in terms of number of nodes explored), though it depends on the actual exploration.

motion_planner::Node everywhere: in so many places that multiple independent copies of the same node are in circulation, which is brittle - there are multiple places in the code where you had to take the coordinate of a node and then look it up back in the grid, to ensure you were referring to the "right version" of a node. Various things don't need to be a full Node. For example in the closed list, all you care about are the coordinates, but a Node is used. That then causes Node to have a comparison and hash written solely for the application of "using Node in the closed list", with odd semantics as a result. That could have been avoided, for example:

1. Make the closed list be in terms of coordinates, or
2. Put a boolean closed on the Node so you don't even need a closed list.

But having a vector<vector<motion_planner::Node> > grid in the first place is a bit overkill. It meant you had to touch every cell of the grid, no matter how big the world or how small the area needed by the current query, so it does not scale well. That can be avoided:

• Let the node be a concept that only lives in the Open List.
• Create nodes on-demand, when generating the neighbours of the current node.
• Keep the parent-references in an unordered_map<coordinate, coordinate>, allowing them to outlive the nodes.
• Rather than having nodes know their index-in-the-heap, maintain a separate map from coordinate to index-in-the-heap, which would be how a Node object is found back from a given coordinate.

vector<vector<..> > for a grid: it gets the job done, but ideally there would be only one vector with everything in it, wrapped in a class that converts the indexing scheme. It's a bit of extra work compared to just letting nested vectors work it out, but you can save a level of indirection, and coalesce allocations, and also make the types more meaningful.

Credit where it's due: no list of neighbours in the node, that's good. A common mistake I see that typically goes hand in hand with making a grid of nodes, is turning that grid into a graph with explicit edges. That involves lots of set-up work, potentially-fragile pointer manipulation, and needlessly balloons the memory consumption, so it's good that you avoided that.

Answer 2

Just off the cuff, before I fully code review this, I would tell you:

1) There are not enough comments. You add comments like this:

//string vals;
cout<<"Enter max_step_number for the random planner: ";

...these are obviously string vals, but what are these?

motion_planner mp;
int x,y,gx,gy,r,c;
int val;

2) You need to initialize your values.

3) You need to have variable names that describe the function/purpose of the variable, i and j are fine for indexers, but outside of that one letter var names are a no-no.

4) You are not checking the input of your std::cin and expecting the values to be convertible.