A*算法基本过程

1.         把起点加入 open list 。

2.         重复如下过程：

a.         遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。

b.         把这个节点移到 close list 。

c.         对当前方格的 8 个相邻方格的每一个方格？

◆     如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。

◆     如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。

◆     如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。

d.         停止，当你

◆     把终点加入到了 open list 中，此时路径已经找到了，或者

◆     查找终点失败，并且 open list 是空的，此时没有路径。

3.         保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。

 Astar_searcher.cpp文件 

//AstarGetSucc函数
inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{   
    neighborPtrSets.clear();
    edgeCostSets.clear();
    /*
    *
    STEP 4: finish AstarPathFinder::AstarGetSucc yourself 
    please write your code below
    *
    *
    */
    //if GLX_SIZE
    GridNodePtr nodePtr;
    Vector3i center = currentPtr->index;
    for(int dev_X = -1;dev_X <= 1;dev_X++){
        if(center[0]+dev_X>=0 && center[0]+dev_X<= GLX_SIZE){
            for(int dev_Y = -1;dev_Y <= 1;dev_Y++){
                if(center[1]+dev_Y>=0 && center[1]+dev_Y<= GLY_SIZE){
                    for(int dev_Z = -1;dev_Z <= 1;dev_Z++){
                        if(center[2]+dev_Z>=0 && center[2]+dev_Z<= GLZ_SIZE){
                            nodePtr = GridNodeMap[center[0]+dev_X][center[1]+dev_Y][center[2]+dev_Z];
                            if(isOccupied(nodePtr->index) || nodePtr->id == -1)
                                continue;
                            else{
                                neighborPtrSets.push_back(nodePtr);
                                edgeCostSets.push_back(
                                    sqrt(
                                    (nodePtr->index(0) - currentPtr->index(0)) * (nodePtr->index(0) - currentPtr->index(0)) +
                                    (nodePtr->index(1) - currentPtr->index(1)) * (nodePtr->index(1) - currentPtr->index(1)) +
                                    (nodePtr->index(2) - currentPtr->index(2)) * (nodePtr->index(2) - currentPtr->index(2))   ) 
                                    );
                            }
                        }
                    }
                }
            }
        }
    }
}

//启发函数 getHeu
double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)
{
    /* 
    choose possible heuristic function you want
    Manhattan, Euclidean, Diagonal, or 0 (Dijkstra)
    Remember tie_breaker learned in lecture, add it here ?
    *
    *
    *
    STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
    please write your code below
    *
    *
    */
    double Euclidean_dis = 0;
    Euclidean_dis = sqrt(
            (node1->index(0) - node2->index(0)) * (node1->index[0] - node2->index(0)) +
            (node1->index(1) - node2->index(1)) * (node1->index(1) - node2->index(1)) +
            (node1->index(2) - node2->index(2)) * (node1->index(2) - node2->index(2)) 
            );

    return Euclidean_dis;
}

//寻找路径函数
void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)
{   
    ros::Time time_1 = ros::Time::now();    

    //index of start_point and end_point
    Vector3i start_idx = coord2gridIndex(start_pt);
    Vector3i end_idx   = coord2gridIndex(end_pt);
    goalIdx = end_idx;

    //position of start_point and end_point
    start_pt = gridIndex2coord(start_idx);
    end_pt   = gridIndex2coord(end_idx);

    //Initialize the pointers of struct GridNode which represent start node and goal node
    GridNodePtr startPtr = new GridNode(start_idx, start_pt);
    GridNodePtr endPtr   = new GridNode(end_idx,   end_pt);

    //openSet is the open_list implemented through multimap in STL library
    openSet.clear();
    closedSet.clear();
    // currentPtr represents the node with lowest f(n) in the open_list
    GridNodePtr currentPtr  = NULL;
    GridNodePtr neighborPtr = NULL;

    //put start node in open set
    startPtr -> gScore = 0;
    startPtr -> fScore = getHeu(startPtr,endPtr);   
    //STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
    startPtr -> id = 1; 
    startPtr -> coord = start_pt;
    openSet.insert( make_pair(startPtr -> fScore, startPtr) );
    /*
    *
    STEP 2 :  some else preparatory works which should be done before while loop
    please write your code below
    *
    *
    */
    vector<GridNodePtr> neighborPtrSets;
    vector<double> edgeCostSets;
    int i =0;
    // this is the main loop
    while ( !openSet.empty() ){
        /*
        *
        *
        step 3: Remove the node with lowest cost function from open set to closed set
        please write your code below
        
        IMPORTANT NOTE!!!
        This part you should use the C++ STL: multimap, more details can be find in Homework description
        *
        *
        */
        //从openset取出一个代价最小的节点
        std::cout<<"time1   "<<i<<endl;
        if(i == 32766)
            i=0;
        i++;
        currentPtr = openSet.begin()->second;
        //id = -1 表示已经访问过
        currentPtr->id = -1;
        //将节点从openset里面删除
         openSet.erase(openSet.begin());
        // if the current node is the goal 
        if( currentPtr->index == goalIdx ){
            ros::Time time_2 = ros::Time::now();
            terminatePtr = currentPtr;
            ROS_WARN("[A*]{sucess}  Time in A*  is %f ms, path cost if %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution );            
            return;
        }
        //get the succetion
        AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets);  //STEP 4: finish AstarPathFinder::AstarGetSucc yourself     

        /*
        *
        *
        STEP 5:  For all unexpanded neigbors "m" of node "n", please finish this for loop
        please write your code below
        *        
        */         
        for(int i = 0; i < (int)neighborPtrSets.size(); i++){
            /*
            *
            *
            Judge if the neigbors have been expanded
            please write your code below
            
            IMPORTANT NOTE!!!
            neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close set
            neighborPtrSets[i]->id = 1 : unexpanded, equal to this node is in open set
            *        
            */
            neighborPtr = neighborPtrSets[i];
            if(neighborPtr -> id == 0){ //discover a new node, which is not in the closed set and open set
                /*
                *
                *
                STEP 6:  As for a new node, do what you need do ,and then put neighbor in open set and record it
                please write your code below
                *        
                */
                neighborPtr->cameFrom = currentPtr;
                
                neighborPtr -> gScore = getHeu(neighborPtr,currentPtr) + currentPtr->gScore;
                neighborPtr -> fScore = getHeu(neighborPtr,endPtr)+neighborPtr -> gScore;
        
                neighborPtr -> id = 1;
                neighborPtr -> coord = gridIndex2coord(neighborPtr->index);
                openSet.insert( make_pair(neighborPtr -> fScore, neighborPtr));
                continue;
            }
            else if(neighborPtr -> id == 1){ //this node is in open set and need to judge if it needs to update, the "0" should be deleted when you are coding
                /*
                *
                *
                STEP 7:  As for a node in open set, update it , maintain the openset ,and then put neighbor in open set and record it
                please write your code below
                *        
                */
                int temp_gSore = getHeu(neighborPtr,currentPtr);
                if((temp_gSore+currentPtr->gScore)<neighborPtr->gScore){
                    auto pr = openSet.equal_range(neighborPtr->fScore);
                    neighborPtr->fScore = neighborPtr->fScore+temp_gSore+currentPtr->gScore-neighborPtr->gScore;
                    neighborPtr->gScore = temp_gSore+currentPtr->gScore;
                    neighborPtr->cameFrom = currentPtr;
                    if(pr.first != end(openSet)){
                        for(auto iter = pr.first;iter != pr.second;++iter){
                            if(iter->second->index == neighborPtr->index){
                                 openSet.erase(iter);
                                 openSet.insert( make_pair(neighborPtr -> fScore, neighborPtr));

                            }
                        }
                    }
                }
                 

                continue;
            }
            else{//this node is in closed set
                /*
                *
                please write your code below
                *        
                */
                continue;
            }
        }      
    }
    //if search fails
    ros::Time time_2 = ros::Time::now();
    if((time_2 - time_1).toSec() > 0.1)
        ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() );
}

//反向追溯得到路径
vector<Vector3d> AstarPathFinder::getPath() 
{   
    vector<Vector3d> path;
    vector<GridNodePtr> gridPath;
    /*
    *
    *
    STEP 8:  trace back from the curretnt nodePtr to get all nodes along the path
    please write your code below
    *      
    */
    GridNodePtr tempPtr = terminatePtr;
    while(tempPtr != NULL){
         gridPath.push_back(tempPtr);
         tempPtr = tempPtr->cameFrom;
    }

    for (auto ptr: gridPath)
        path.push_back(ptr->coord);
        
    reverse(path.begin(),path.end());

    return path;
}