基于ROS的A*算法代码学习

算法小白一枚,本文是在学习路径规划算法中的一篇学习记录,由于对c++掌握粗浅,所以边查边学,将整个代码进行了详细的梳理,当然里面也还有些问题受限于相关知识不能解决,希望该文章能解决您的一些问题,同时也希望本人的遗留问题能得到您的解答。
(代码注释中英夹杂,英文注释是原来作者的注释,中文注释是本人后来添加的)

过程演示

1.编译功能包.

2.Ctrl+Shift+t新建窗口打开roscore.

3.Ctrl+Shift+t新建窗口打开rviz.
记得要source devel/setup.sh,告诉这个窗口你功能包的位置,否则3d Nav Goal插件会找不到.

4.在rviz中打开demo.rviz(路径src/grid_path_searcher/launch/rviz_config).

5.Ctrl+Shift+t新建窗口加载地图.
记得source.

6.A*路径搜索

下面是结果

这是终端里的信息

ROS包

├── grid_path_searcher--------------------------------------------------------------路径搜索包名称
│ ├── CMakeLists.txt
│ ├── include
│ │ ├── Astar_searcher.h------------------------------------------------------------A代码头文件
│ │ ├── backward.hpp
│ │ ├── JPS_searcher.h
│ │ ├── JPS_utils.h
│ │ └── node.h-------------------------------------------------------------------------A代码头文件
│ ├── launch
│ │ ├── demo.launch-----------------------------------------------------------------加载地图的launch文件
│ │ └── rviz_config
│ │ ├── demo.rviz---------------------------------------------------------------------rviz的环境文件
│ │ └── jps_demo.rviz
│ ├── package.xml
│ ├── README.md
│ └── src
│ ├── Astar_searcher.cpp-----------------------------------------------------------A*算法代码文件
│ ├── CMakeLists.txt
│ ├── demo_node.cpp---------------------------------------------------------------主函数文件
│ ├── graph_searcher.cpp
│ ├── random_complex_generator.cpp-----------------------------------------地图生成障碍物
│ └── read_only
│ ├── JPS_searcher.cpp
│ └── JPS_utils.cpp

---

│ └── waypoint_generator
│ ├── CMakeLists.txt
│ ├── package.xml
│ └── src
│ ├── sample_waypoints.h
│ └── waypoint_generator.cpp-----------------------------------------------------发布目标点信息

以上所有带注释的就是ROS下的A*算法代码实现相关文件.

node.h

#ifndef _NODE_H_
#define _NODE_H_

#include 
#include 
#include 
#include 
#include "backward.hpp"

#define inf 1>>20
struct GridNode;
typedef GridNode* GridNodePtr;

struct GridNode
{     
    // 1--> open set, -1 --> closed set, 0 --> 未被访问过
    int id;
    // 实际坐标
    Eigen::Vector3d coord;
    // direction of expanding
    Eigen::Vector3i dir;
    // 栅格地图坐标
    Eigen::Vector3i index;
	
    double gScore, fScore;
    // 父节点
    GridNodePtr cameFrom;
    // 这个不清楚干什么用的,感觉代码里没用到
    std::multimap<double, GridNodePtr>::iterator nodeMapIt;

    GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord){  
		id = 0;
		index = _index;
		coord = _coord;
    //这个dir感觉在代码中没用到,有什么用呢???
		dir   = Eigen::Vector3i::Zero();

		gScore = inf;
		fScore = inf;
		cameFrom = NULL;
    }

    GridNode(){};  
    ~GridNode(){};
};

#endif

Astar_searcher.h

#ifndef _ASTART_SEARCHER_H
#define _ASTART_SEARCHER_H

#include 
#include 
#include 
#include 
#include "backward.hpp"
#include "node.h"

class AstarPathFinder
{	
	private:

	protected:
		uint8_t * data;
		// 地图指针
		GridNodePtr *** GridNodeMap;
		// 网格地图坐标形式的目标点
		Eigen::Vector3i goalIdx;
		// 定义的地图的长宽高
		int GLX_SIZE, GLY_SIZE, GLZ_SIZE;
		int GLXYZ_SIZE, GLYZ_SIZE;

    	// resolution表示栅格地图精度,inv_resolution=1/resolution
		double resolution, inv_resolution;
		// gl_x表示地图边界,l->low(下边界),u->up(上边界)
		double gl_xl, gl_yl, gl_zl;
		double gl_xu, gl_yu, gl_zu;

		// 终点指针
		GridNodePtr terminatePtr;
		// openSet容器,用于存放规划中确定下来的路径点
		std::multimap<double, GridNodePtr> openSet;

    	// 返回两点之间的距离(曼哈顿距离/欧式距离/对角线距离)
		double getHeu(GridNodePtr node1, GridNodePtr node2);
		// 返回当前指针指向坐标的所有相邻节点,以及当前节点与相邻节点的cost
		void AstarGetSucc(GridNodePtr currentPtr, std::vector<GridNodePtr> & neighborPtrSets, std::vector<double> & edgeCostSets);		

		// 判断节点是否是障碍物
    	bool isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const;
		bool isOccupied(const Eigen::Vector3i & index) const;
		// 判断节点是不是为空
		bool isFree(const int & idx_x, const int & idx_y, const int & idx_z) const;
		bool isFree(const Eigen::Vector3i & index) const;
		
		//栅格地图坐标转实际坐标
		Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index);
		//实际坐标转栅格地图坐标
		Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt);

	public:
		AstarPathFinder(){};
		~AstarPathFinder(){};
		// A*路径搜索
		void AstarGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
		// 将所有点的属性设置为未访问过的状态下
		void resetGrid(GridNodePtr ptr);
		// 通过循环遍历重置每一个点
		void resetUsedGrids();

		// 初始化地图
		void initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id);
		// 设置障碍物
		void setObs(const double coord_x, const double coord_y, const double coord_z);

		Eigen::Vector3d coordRounding(const Eigen::Vector3d & coord);
		// 获得A*搜索到的完整路径
		std::vector<Eigen::Vector3d> getPath();
		// 获得访问过的所有节点
		std::vector<Eigen::Vector3d> getVisitedNodes();
};

#endif

Astar_searcher.cpp

#include "Astar_searcher.h"

using namespace std;
using namespace Eigen;

bool tie_break = false;

// 初始化地图
void AstarPathFinder::initGridMap(double _resolution, Vector3d global_xyz_l, Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id)
{   
    // gl_x表示地图边界,l->low(下边界),u->up(上边界)
    gl_xl = global_xyz_l(0);
    gl_yl = global_xyz_l(1);
    gl_zl = global_xyz_l(2);

    gl_xu = global_xyz_u(0);
    gl_yu = global_xyz_u(1);
    gl_zu = global_xyz_u(2);
    
    GLX_SIZE = max_x_id;
    GLY_SIZE = max_y_id;
    GLZ_SIZE = max_z_id;
    GLYZ_SIZE  = GLY_SIZE * GLZ_SIZE;
    GLXYZ_SIZE = GLX_SIZE * GLYZ_SIZE;

    resolution = _resolution;
    inv_resolution = 1.0 / _resolution;    

    // void *memset(void *s, int ch, size_t n);将s中当前位置后面的n个字节(typedef unsigned int size_t)用ch替换并返回 s 。
    data = new uint8_t[GLXYZ_SIZE];
    memset(data, 0, GLXYZ_SIZE * sizeof(uint8_t));

    GridNodeMap = new GridNodePtr ** [GLX_SIZE];
    for(int i = 0; i < GLX_SIZE; i++){
        GridNodeMap[i] = new GridNodePtr * [GLY_SIZE];
        for(int j = 0; j < GLY_SIZE; j++){
            GridNodeMap[i][j] = new GridNodePtr [GLZ_SIZE];
            for( int k = 0; k < GLZ_SIZE;k++){
                Vector3i tmpIdx(i,j,k);
                Vector3d pos = gridIndex2coord(tmpIdx);
                // GridNodeMap-三维指针;传入网格地图坐标和实际坐标,初始化每个节点的属性
                GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos);
            }
        }
    }
}

// 将所有点的属性设置为未访问过的状态下
void AstarPathFinder::resetGrid(GridNodePtr ptr)
{
    ptr->id = 0;
    ptr->cameFrom = NULL;
    ptr->gScore = inf;
    ptr->fScore = inf;
}

// 通过循环遍历重置每一个点
void AstarPathFinder::resetUsedGrids()
{   
    for(int i=0; i < GLX_SIZE ; i++)
        for(int j=0; j < GLY_SIZE ; j++)
            for(int k=0; k < GLZ_SIZE ; k++)
                resetGrid(GridNodeMap[i][j][k]);
}

// set obstalces into grid map for path planning
void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z)
{   
    if( coord_x < gl_xl  || coord_y < gl_yl  || coord_z <  gl_zl || 
        coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu )
        return;

    int idx_x = static_cast<int>( (coord_x - gl_xl) * inv_resolution);
    int idx_y = static_cast<int>( (coord_y - gl_yl) * inv_resolution);
    int idx_z = static_cast<int>( (coord_z - gl_zl) * inv_resolution);      

    data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1;
}

// 获得访问过的所有节点
vector<Vector3d> AstarPathFinder::getVisitedNodes()
{
    vector<Vector3d> visited_nodes;
    for(int i = 0; i < GLX_SIZE; i++)
        for(int j = 0; j < GLY_SIZE; j++)
            for(int k = 0; k < GLZ_SIZE; k++)
            {
                if(GridNodeMap[i][j][k]->id != 0) // visualize all nodes in open and close list
//                if(GridNodeMap[i][j][k]->id == -1)  // visualize nodes in close list only TODO: careful
                    visited_nodes.push_back(GridNodeMap[i][j][k]->coord);
            }

    ROS_WARN("visited_nodes size : %d", visited_nodes.size());
    return visited_nodes;
}

// 网格地图坐标转换为实际坐标
Vector3d AstarPathFinder::gridIndex2coord(const Vector3i & index) 
{
    Vector3d pt;

    pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl;
    pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl;
    pt(2) = ((double)index(2) + 0.5) * resolution + gl_zl;

    return pt;
}

// 实际坐标转换为网格地图坐标
Vector3i AstarPathFinder::coord2gridIndex(const Vector3d & pt) 
{
    Vector3i idx;
    idx <<  min( max( int( (pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1),
            min( max( int( (pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1),
            min( max( int( (pt(2) - gl_zl) * inv_resolution), 0), GLZ_SIZE - 1);                  
  
    return idx;
}

//----这个函数没看懂,为什么要把坐标转换为栅格地图坐标再转换为实际坐标?????
Eigen::Vector3d AstarPathFinder::coordRounding(const Eigen::Vector3d & coord)
{
    return gridIndex2coord(coord2gridIndex(coord));
}

// inline是C++关键字，在函数声明或定义中，函数返回类型前加上关键字inline，即可以把函数指定为内联函数。这样可以解决一些频繁调用的函数大量消耗栈空间（栈内存）的问题。
// 用网格地图坐标判断是否是障碍物点(在函数内部调用另一个用实际坐标判断的函数)
inline bool AstarPathFinder::isOccupied(const Eigen::Vector3i & index) const
{
    return isOccupied(index(0), index(1), index(2));
}

// 用网格地图坐标判断是否是空点(在函数内部调用另一个用实际坐标判断的函数)
inline bool AstarPathFinder::isFree(const Eigen::Vector3i & index) const
{
    return isFree(index(0), index(1), index(2));
}

inline bool AstarPathFinder::isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const 
{
    return  (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && 
            (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1));
}

inline bool AstarPathFinder::isFree(const int & idx_x, const int & idx_y, const int & idx_z) const 
{
    return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && 
           (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] < 1));
}

// 获取该点周围的所有节点和周围点的edgeCostSets(edgeCostSets:该点到目标的的距离)
inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{   
    neighborPtrSets.clear(); // Note: the pointers in this set copy pointers to GridNodeMap
    edgeCostSets.clear();
    /*
    *
    STEP 4: finish AstarPathFinder::AstarGetSucc yourself 
    please write your code below
    *
    */
    // idea index -> coordinate -> edgecost
    if(currentPtr == nullptr)
        std::cout << "Error: Current pointer is null!" << endl;


    Eigen::Vector3i thisNode = currentPtr -> index;
    int this_x = thisNode[0];
    int this_y = thisNode[1];
    int this_z = thisNode[2];
    auto this_coord = currentPtr -> coord;
    int  n_x, n_y, n_z;
    double dist;
    GridNodePtr temp_ptr = nullptr;
    Eigen::Vector3d n_coord;

    // 遍历周围点,获取周围点的edgeCostSets
    for(int i = -1;i <= 1;++i ){
        for(int j = -1;j <= 1;++j ){
            for(int k = -1;k <= 1;++k){
                if( i == 0 && j == 0 && k == 0)
                    continue; // to avoid this node

                n_x = this_x + i;
                n_y = this_y + j;
                n_z = this_z + k;

                if( (n_x < 0) || (n_x > (GLX_SIZE - 1)) || (n_y < 0) || (n_y > (GLY_SIZE - 1) ) || (n_z < 0) || (n_z > (GLZ_SIZE - 1)))
                    continue; // to avoid index problem

                if(isOccupied(n_x, n_y, n_z))
                    continue; // to avoid obstacles

                // put the pointer into neighborPtrSets
                temp_ptr = GridNodeMap[n_x][n_y][n_z];

                if(temp_ptr->id == -1) 
                    continue; // todo to check this; why the node can transversing the obstacles

                n_coord = temp_ptr->coord;

                if(temp_ptr == currentPtr){
                    std::cout << "Error: temp_ptr == currentPtr)" << std::endl;
                }

                if( (std::abs(n_coord[0] - this_coord[0]) < 1e-6) and (std::abs(n_coord[1] - this_coord[1]) < 1e-6) and (std::abs(n_coord[2] - this_coord[2]) < 1e-6 )){
                    std::cout << "Error: Not expanding correctly!" << std::endl;
                    std::cout << "n_coord:" << n_coord[0] << " "<<n_coord[1]<<" "<<n_coord[2] << std::endl;
                    std::cout << "this_coord:" << this_coord[0] << " "<<this_coord[1]<<" "<<this_coord[2] << std::endl;

                    std::cout << "current node index:" << this_x << " "<< this_y<<" "<< this_z << std::endl;
                    std::cout << "neighbor node index:" << n_x << " "<< n_y<<" "<< n_z << std::endl;
                }

                dist = std::sqrt( (n_coord[0] - this_coord[0]) * (n_coord[0] - this_coord[0])+
                        (n_coord[1] - this_coord[1]) * (n_coord[1] - this_coord[1])+
                        (n_coord[2] - this_coord[2]) * (n_coord[2] - this_coord[2]));
                
                neighborPtrSets.push_back(temp_ptr); // calculate the cost in edgeCostSets: inf means that is not unexpanded
                edgeCostSets.push_back(dist); // put the cost inot edgeCostSets

            }
        }
    }

}

// 启发试函数,获得h值
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 h;
    auto node1_coord = node1->coord;
    auto node2_coord = node2->coord;

    // Heuristics 1: Manhattan
    // h = std::abs(node1_coord(0) - node2_coord(0) ) +
    //     std::abs(node1_coord(1) - node2_coord(1) ) +
    //     std::abs(node1_coord(2) - node2_coord(2) );

    // Heuristics 2: Euclidean
    // h = std::sqrt(std::pow((node1_coord(0) - node2_coord(0)), 2 ) +
    //     std::pow((node1_coord(1) - node2_coord(1)), 2 ) +
    //     std::pow((node1_coord(2) - node2_coord(2)), 2 ));

    // Heuristics 3: Diagnol distance
    double dx = std::abs(node1_coord(0) - node2_coord(0) );
    double dy = std::abs(node1_coord(1) - node2_coord(1) );
    double dz = std::abs(node1_coord(2) - node2_coord(2) );
    double min_xyz = std::min({dx, dy, dz});
    h = dx + dy + dz + (std::sqrt(3.0) -3) * min_xyz; // idea: diagnol is a short-cut, find out how many short-cuts can be realized

    //这个tie_break目前还没搞懂什么意思
    if(tie_break){
        double p = 1.0 / 25.0;
        h *= (1.0 + p);
        //std::cout << "Tie Break!" << std::endl;
    }

    return h;
}

// A*路径搜索
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();
    //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;
    // make_pair的用法:无需写出型别,就可以生成一个pair对象;比如std::make_pair(42, '@'),而不必费力写成：std::pair(42, '@')
    //todo Note: modified, insert the pointer GridNodeMap[i][j][k] to the start node in grid map
    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
    *
    *
    */
    // three dimension pointer GridNodeMap[i][j][k] is pointed to a struct GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord);
    // assign g(xs) = 0, g(n) = inf (already done in initialzation of struct)
    // mark start point as visited(expanded) (id 0: no operation, id: 1 in OPEN, id -1: in CLOSE )

    // 这个到底需不需要???测试后发现不需要也可以实现功能
    GridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]] -> id = 1;

    vector<GridNodePtr> neighborPtrSets;
    vector<double> edgeCostSets;
    Eigen::Vector3i current_idx; // record the current index

    // 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:待访问节点容器;closed set:访问过节点容器
        int x = openSet.begin()->second->index(0); 
        int y = openSet.begin()->second->index(1); 
        int z = openSet.begin()->second->index(2);
        openSet.erase(openSet.begin());
        currentPtr = GridNodeMap[x][y][z];

        // 如果节点被访问过;则返回
        if(currentPtr->id == -1)
            continue;
        // 标记id为-1,表示节点已被扩展
        currentPtr->id = -1;

        // currentPtr = openSet.begin() -> second; // first T1, second T2
        // openSet.erase(openSet.begin()); // remove the node with minimal f value
        // current_idx = currentPtr->index;
        // GridNodeMap[current_idx[0]][current_idx[1]][current_idx[2]] -> id = -1;// update the id in grid node map
        
        // 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
                *        
                */
                // shall update: gScore = inf; fScore = inf; cameFrom = NULL, id, mayby direction

                neighborPtr->gScore = currentPtr->gScore + edgeCostSets[i];
                neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr,endPtr);
                neighborPtr->cameFrom = currentPtr; // todo shallow copy or deep copy
                // push node "m" into OPEN
                openSet.insert(make_pair(neighborPtr -> fScore, neighborPtr));
                neighborPtr -> id = 1;
                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
                *        
                */
                // shall update: gScore; fScore; cameFrom, mayby direction
                if( neighborPtr->gScore > (currentPtr->gScore+edgeCostSets[i]))
                {
                    neighborPtr -> gScore = currentPtr -> gScore + edgeCostSets[i];
                    neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr,endPtr);
                    neighborPtr -> cameFrom = currentPtr;
                }

                continue;
            }
            else{//this node is in closed set
                /*
                *
                please write your code below
                *        
                */
                // todo nothing to do here?
                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() );
}

// 获得A*搜索到的完整路径
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
    *      
    */
    auto ptr = terminatePtr;
    while(ptr -> cameFrom != NULL){
        gridPath.push_back(ptr);
        ptr = ptr->cameFrom;
    }

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

    return path;
}

// if the difference of f is trivial, then choose then prefer the path along the straight line from start to goal
// discared!!!
// 目前这个函数我没有用到.
GridNodePtr & TieBreaker(const std::multimap<double, GridNodePtr> &  openSet, const GridNodePtr & endPtr)
{
    // todo do I have to update the f in openSet??
    std::multimap<double, GridNodePtr> local_set;

    auto f_min = openSet.begin()->first;
    auto f_max = f_min + 1e-2;
    auto itlow = openSet.lower_bound (f_min);
    auto itup = openSet.upper_bound(f_max);
    double cross, f_new;

    for (auto it=itlow; it!=itup; ++it)
    {
        std::cout << "f value is:" << (*it).first << " pointer is: " << (*it).second << '\n';
        cross = std::abs(endPtr->coord(0) - (*it).second->coord(0)) +
                std::abs(endPtr->coord(1) - (*it).second->coord(1)) +
                std::abs(endPtr->coord(2) - (*it).second->coord(2));
        f_new = (*it).second->fScore + 0.001 * cross;
        local_set.insert( make_pair(f_new, (*it).second) ); // todo what is iterator, is this way correct?
    }


    return local_set.begin()->second;
}

demo_node.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 

#include "Astar_searcher.h"
#include "JPS_searcher.h"
#include "backward.hpp"

using namespace std;
using namespace Eigen;

namespace backward {
backward::SignalHandling sh;
}

// simulation param from launch file
// 精度,精度倒数,点云间隙
double _resolution, _inv_resolution, _cloud_margin;
// 地图x,y,z尺寸
double _x_size, _y_size, _z_size;    

// useful global variables
bool _has_map   = false;

// 起始点指针
Vector3d _start_pt;
// 地图上下界
Vector3d _map_lower, _map_upper;
// 地图x,y,z最大id
int _max_x_id, _max_y_id, _max_z_id;

// ros related
ros::Subscriber _map_sub, _pts_sub;
ros::Publisher  _grid_path_vis_pub, _visited_nodes_vis_pub, _grid_map_vis_pub;

AstarPathFinder * _astar_path_finder     = new AstarPathFinder();
JPSPathFinder   * _jps_path_finder       = new JPSPathFinder();

// 终点信息的回调函数
void rcvWaypointsCallback(const nav_msgs::Path & wp);
// 地图信息的回调函数
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map);

// 实体化搜索到的路径
void visGridPath( vector<Vector3d> nodes, bool is_use_jps );
// 实体化显示所有访问过的节点
void visVisitedNode( vector<Vector3d> nodes );
// 路径规划函数
void pathFinding(const Vector3d start_pt, const Vector3d target_pt);

// 终点信息的回调函数
void rcvWaypointsCallback(const nav_msgs::Path & wp)
{     
    // 如果终点信息的z值小于0,或者没有地图信息,则直接返回
    if( wp.poses[0].pose.position.z < 0.0 || _has_map == false )
        return;

    // 将终点信息传递给终点指针
    Vector3d target_pt;
    target_pt << wp.poses[0].pose.position.x,
                 wp.poses[0].pose.position.y,
                 wp.poses[0].pose.position.z;

    // 终端打印已经接收到终点信息
    ROS_INFO("[node] receive the planning target");
    // 路径规划
    pathFinding(_start_pt, target_pt); 
}

// 地图信息的回调函数
void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map)
{   
    // 如果已经有地图信息,则返回
    if( _has_map ) return;

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::PointCloud<pcl::PointXYZ> cloud_vis;
    sensor_msgs::PointCloud2 map_vis;

    // 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud(在RVIZ中显示的点云的数据格式sensor_msgs::PointCloud2)
    pcl::fromROSMsg(pointcloud_map, cloud);
    
    // 如果还没有点云数据,则返回
    if( (int)cloud.points.size() == 0 ) return;

    pcl::PointXYZ pt;
    for (int idx = 0; idx < (int)cloud.points.size(); idx++)
    {    
        pt = cloud.points[idx];

        // set obstalces into grid map for path planning
        _astar_path_finder->setObs(pt.x, pt.y, pt.z);
        // jps才能用得到,Astar永不到
        // _jps_path_finder->setObs(pt.x, pt.y, pt.z);

        // for visualize only
        Vector3d cor_round = _astar_path_finder->coordRounding(Vector3d(pt.x, pt.y, pt.z));
        pt.x = cor_round(0);
        pt.y = cor_round(1);
        pt.z = cor_round(2);
        // 坐标压入堆栈
        cloud_vis.points.push_back(pt);
    }

    cloud_vis.width    = cloud_vis.points.size();
    cloud_vis.height   = 1;
    cloud_vis.is_dense = true;

    // pcl::toROSMsg (pcl::PointCloud,sensor_msgs::PointCloud2);
    pcl::toROSMsg(cloud_vis, map_vis);

    map_vis.header.frame_id = "/world";
    //发布地图信息
    _grid_map_vis_pub.publish(map_vis);

    _has_map = true;
}

//路径规划函数
void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{
    //Call A* to search for a path
    _astar_path_finder->AstarGraphSearch(start_pt, target_pt);

    //Retrieve the path
    auto grid_path     = _astar_path_finder->getPath();
    auto visited_nodes = _astar_path_finder->getVisitedNodes();

    //Visualize the result
    visGridPath (grid_path, false);
    // 显示所有访问过的节点(函数没问题,但是显示不出来,没找到问题)
    visVisitedNode(visited_nodes);

    //Reset map for next call
    _astar_path_finder->resetUsedGrids();

    //_use_jps = 0 -> Do not use JPS
    //_use_jps = 1 -> Use JPS
    //you just need to change the #define value of _use_jps
#define _use_jps 0
#if _use_jps
    {
        //Call JPS to search for a path
        _jps_path_finder -> JPSGraphSearch(start_pt, target_pt);

        //Retrieve the path
        auto grid_path     = _jps_path_finder->getPath();
        auto visited_nodes = _jps_path_finder->getVisitedNodes();

        //Visualize the result
        visGridPath   (grid_path, _use_jps);
        visVisitedNode(visited_nodes);

        //Reset map for next call
        _jps_path_finder->resetUsedGrids();
    }
#endif
}

//主函数
int main(int argc, char** argv)
{
    // 初始化demo_node节点
    ros::init(argc, argv, "demo_node");
    ros::NodeHandle nh("~");

    _map_sub  = nh.subscribe( "map",       1, rcvPointCloudCallBack );
    _pts_sub  = nh.subscribe( "waypoints", 1, rcvWaypointsCallback );

    _grid_map_vis_pub             = nh.advertise<sensor_msgs::PointCloud2>("grid_map_vis", 1);
    _grid_path_vis_pub            = nh.advertise<visualization_msgs::Marker>("grid_path_vis", 1);
    _visited_nodes_vis_pub        = nh.advertise<visualization_msgs::Marker>("visited_nodes_vis",1);

    // param是从参数服务器中获取第一个参数,将值保存到第二个参数中,如果没获取到第一个参数的值,则使用默认值(第三个参数)
    nh.param("map/cloud_margin",  _cloud_margin, 0.0);
    nh.param("map/resolution",    _resolution,   0.2);
    
    nh.param("map/x_size",        _x_size, 50.0);
    nh.param("map/y_size",        _y_size, 50.0);
    nh.param("map/z_size",        _z_size, 5.0 );
    
    nh.param("planning/start_x",  _start_pt(0),  0.0);
    nh.param("planning/start_y",  _start_pt(1),  0.0);
    nh.param("planning/start_z",  _start_pt(2),  0.0);

    _map_lower << - _x_size/2.0, - _y_size/2.0,     0.0;
    _map_upper << + _x_size/2.0, + _y_size/2.0, _z_size;
    
    _inv_resolution = 1.0 / _resolution;
    
    _max_x_id = (int)(_x_size * _inv_resolution);
    _max_y_id = (int)(_y_size * _inv_resolution);
    _max_z_id = (int)(_z_size * _inv_resolution);

    _astar_path_finder  = new AstarPathFinder();
    _astar_path_finder  -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);

//    _jps_path_finder    = new JPSPathFinder();
//    _jps_path_finder    -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id);
    
    // rate中的参数100，代表在while循环中，每秒钟运行100次。
    ros::Rate rate(100);
    bool status = ros::ok();
    while(status) 
    {
        // 监听反馈函数（callback）。只能监听反馈，不能循环。
        ros::spinOnce();      
        status = ros::ok();
        rate.sleep();
    }

    delete _astar_path_finder;
    delete _jps_path_finder;
    return 0;
}

// 实体化搜索到的路径
void visGridPath( vector<Vector3d> nodes, bool is_use_jps )
{   
    visualization_msgs::Marker node_vis; 
    node_vis.header.frame_id = "world";
    node_vis.header.stamp = ros::Time::now();
    
    // is_use_jps是用于判断用的是不是jps算法
    if(is_use_jps)
        node_vis.ns = "demo_node/jps_path";
    else
        node_vis.ns = "demo_node/astar_path";

    node_vis.type = visualization_msgs::Marker::CUBE_LIST;
    node_vis.action = visualization_msgs::Marker::ADD;
    node_vis.id = 0;

    //----不明白这里的pose是什么作用???
    node_vis.pose.orientation.x = 0.0;
    node_vis.pose.orientation.y = 0.0;
    node_vis.pose.orientation.z = 0.0;
    node_vis.pose.orientation.w = 1.0;

    //路径的颜色
    if(is_use_jps){
        node_vis.color.a = 1.0;
        node_vis.color.r = 1.0;
        node_vis.color.g = 0.0;
        node_vis.color.b = 0.0;
    }
    else{
        node_vis.color.a = 1.0;
        node_vis.color.r = 0.0;
        node_vis.color.g = 1.0;
        node_vis.color.b = 0.0;
    }

    node_vis.scale.x = _resolution;
    node_vis.scale.y = _resolution;
    node_vis.scale.z = _resolution;

    // 将所有路径中的点压入堆栈
    geometry_msgs::Point pt;
    for(int i = 0; i < int(nodes.size()); i++)
    {
        Vector3d coord = nodes[i];
        pt.x = coord(0);
        pt.y = coord(1);
        pt.z = coord(2);

        node_vis.points.push_back(pt);
    }

    // 发布显示所有路径节点
    _grid_path_vis_pub.publish(node_vis);
}

//按理说这个函数是实体化显示所有访问过得点,但是在地图中却没有显示出来
void visVisitedNode( vector<Vector3d> nodes )
{   
    visualization_msgs::Marker node_vis; 
    node_vis.header.frame_id = "world";
    node_vis.header.stamp = ros::Time::now();
    node_vis.ns = "demo_node/expanded_nodes";
    node_vis.type = visualization_msgs::Marker::CUBE_LIST;
    node_vis.action = visualization_msgs::Marker::ADD;
    node_vis.id = 0;

    node_vis.pose.orientation.x = 0.0;
    node_vis.pose.orientation.y = 0.0;
    node_vis.pose.orientation.z = 0.0;
    node_vis.pose.orientation.w = 1.0;
    node_vis.color.a = 0.5;
    node_vis.color.r = 0.0;
    node_vis.color.g = 0.0;
    node_vis.color.b = 1.0;

    node_vis.scale.x = _resolution;
    node_vis.scale.y = _resolution;
    node_vis.scale.z = _resolution;

    geometry_msgs::Point pt;
    for(int i = 0; i < int(nodes.size()); i++)
    {
        Vector3d coord = nodes[i];
        pt.x = coord(0);
        pt.y = coord(1);
        pt.z = coord(2);

        node_vis.points.push_back(pt);
    }

    _visited_nodes_vis_pub.publish(node_vis);
}

waypoint_generator.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "sample_waypoints.h"
#include 
#include 
#include 
#include 

using namespace std;
using bfmt = boost::format;

ros::Publisher pub1;
ros::Publisher pub2;
ros::Publisher pub3;
string waypoint_type = string("manual");
bool is_odom_ready;
nav_msgs::Odometry odom;
nav_msgs::Path waypoints;

// series waypoint needed
std::deque<nav_msgs::Path> waypointSegments;
ros::Time trigged_time;

void load_seg(ros::NodeHandle& nh, int segid, const ros::Time& time_base) {
    std::string seg_str = boost::str(bfmt("seg%d/") % segid);
    double yaw;
    double time_from_start;
    ROS_INFO("Getting segment %d", segid);
    ROS_ASSERT(nh.getParam(seg_str + "yaw", yaw));
    ROS_ASSERT_MSG((yaw > -3.1499999) && (yaw < 3.14999999), "yaw=%.3f", yaw);
    ROS_ASSERT(nh.getParam(seg_str + "time_from_start", time_from_start));
    ROS_ASSERT(time_from_start >= 0.0);

    std::vector<double> ptx;
    std::vector<double> pty;
    std::vector<double> ptz;

    ROS_ASSERT(nh.getParam(seg_str + "x", ptx));
    ROS_ASSERT(nh.getParam(seg_str + "y", pty));
    ROS_ASSERT(nh.getParam(seg_str + "z", ptz));

    ROS_ASSERT(ptx.size());
    ROS_ASSERT(ptx.size() == pty.size() && ptx.size() == ptz.size());

    nav_msgs::Path path_msg;

    path_msg.header.stamp = time_base + ros::Duration(time_from_start);

    double baseyaw = tf::getYaw(odom.pose.pose.orientation);
    
    for (size_t k = 0; k < ptx.size(); ++k) {
        geometry_msgs::PoseStamped pt;
        pt.pose.orientation = tf::createQuaternionMsgFromYaw(baseyaw + yaw);
        Eigen::Vector2d dp(ptx.at(k), pty.at(k));
        Eigen::Vector2d rdp;
        rdp.x() = std::cos(-baseyaw-yaw)*dp.x() + std::sin(-baseyaw-yaw)*dp.y();
        rdp.y() =-std::sin(-baseyaw-yaw)*dp.x() + std::cos(-baseyaw-yaw)*dp.y();
        pt.pose.position.x = rdp.x() + odom.pose.pose.position.x;
        pt.pose.position.y = rdp.y() + odom.pose.pose.position.y;
        pt.pose.position.z = ptz.at(k) + odom.pose.pose.position.z;
        path_msg.poses.push_back(pt);
    }

    waypointSegments.push_back(path_msg);
}

void load_waypoints(ros::NodeHandle& nh, const ros::Time& time_base) {
    int seg_cnt = 0;
    waypointSegments.clear();
    ROS_ASSERT(nh.getParam("segment_cnt", seg_cnt));
    for (int i = 0; i < seg_cnt; ++i) {
        load_seg(nh, i, time_base);
        if (i > 0) {
            ROS_ASSERT(waypointSegments[i - 1].header.stamp < waypointSegments[i].header.stamp);
        }
    }
    ROS_INFO("Overall load %zu segments", waypointSegments.size());
}

// 发布目标点信息,并清空waypoints
void publish_waypoints() {
    waypoints.header.frame_id = std::string("world");
    waypoints.header.stamp = ros::Time::now();
    pub1.publish(waypoints);
    // 下面这四行代码无用,因为我们并没有odom坐标系,可能是原代码作者需要用到
    geometry_msgs::PoseStamped init_pose;
    init_pose.header = odom.header;
    init_pose.pose = odom.pose.pose;
    waypoints.poses.insert(waypoints.poses.begin(), init_pose);
    // pub2.publish(waypoints);
    waypoints.poses.clear();
}

void publish_waypoints_vis() {
    nav_msgs::Path wp_vis = waypoints;
    geometry_msgs::PoseArray poseArray;
    poseArray.header.frame_id = std::string("world");
    poseArray.header.stamp = ros::Time::now();

    {
        geometry_msgs::Pose init_pose;
        init_pose = odom.pose.pose;
        poseArray.poses.push_back(init_pose);
    }

    for (auto it = waypoints.poses.begin(); it != waypoints.poses.end(); ++it) {
        geometry_msgs::Pose p;
        p = it->pose;
        poseArray.poses.push_back(p);
    }
    pub2.publish(poseArray);
}

// odom回调函数,我们并没有odom坐标系,无用
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) {
    is_odom_ready = true;
    odom = *msg;

    if (waypointSegments.size()) {
        ros::Time expected_time = waypointSegments.front().header.stamp;
        if (odom.header.stamp >= expected_time) {
            waypoints = waypointSegments.front();

            std::stringstream ss;
            ss << bfmt("Series send %.3f from start:\n") % trigged_time.toSec();
            for (auto& pose_stamped : waypoints.poses) {
                ss << bfmt("P[%.2f, %.2f, %.2f] q(%.2f,%.2f,%.2f,%.2f)") %
                          pose_stamped.pose.position.x % pose_stamped.pose.position.y %
                          pose_stamped.pose.position.z % pose_stamped.pose.orientation.w %
                          pose_stamped.pose.orientation.x % pose_stamped.pose.orientation.y %
                          pose_stamped.pose.orientation.z << std::endl;
            }
            ROS_INFO_STREAM(ss.str());

            publish_waypoints_vis();
            publish_waypoints();

            waypointSegments.pop_front();
        }
    }
}

// 目标点回调函数
void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
// 原代码这部分就是被注释掉的,所以从这部分可以得出
/*    if (!is_odom_ready) {
        ROS_ERROR("[waypoint_generator] No odom!");
        return;
    }*/

    trigged_time = ros::Time::now(); //odom.header.stamp;
    //ROS_ASSERT(trigged_time > ros::Time(0));

    ros::NodeHandle n("~");
    n.param("waypoint_type", waypoint_type, string("manual"));
    
    if (waypoint_type == string("circle")) {
        waypoints = circle();
        publish_waypoints_vis();
        publish_waypoints();
    } else if (waypoint_type == string("eight")) {
        waypoints = eight();
        publish_waypoints_vis();
        publish_waypoints();
    } else if (waypoint_type == string("point")) {
        waypoints = point();
        publish_waypoints_vis();
        publish_waypoints();
    } else if (waypoint_type == string("series")) {
        load_waypoints(n, trigged_time);
    // 我们只用看这部分,这部分才是我们能用到的
    } else if (waypoint_type == string("manual-lonely-waypoint")) {
        // if height >= 0, it's a valid goal;
        if (msg->pose.position.z >= 0) {
            geometry_msgs::PoseStamped pt = *msg;
            waypoints.poses.clear();
            waypoints.poses.push_back(pt);
            // publish_waypoints_vis();
            publish_waypoints();
        } else {
            ROS_WARN("[waypoint_generator] invalid goal in manual-lonely-waypoint mode.");
        }
    } else {
        if (msg->pose.position.z > 0) {
            // if height > 0, it's a normal goal;
            geometry_msgs::PoseStamped pt = *msg;
            if (waypoint_type == string("noyaw")) {
                double yaw = tf::getYaw(odom.pose.pose.orientation);
                pt.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
            }
            waypoints.poses.push_back(pt);
            publish_waypoints_vis();
        } else if (msg->pose.position.z > -1.0) {
            // if 0 > height > -1.0, remove last goal;
            if (waypoints.poses.size() >= 1) {
                waypoints.poses.erase(std::prev(waypoints.poses.end()));
            }
            publish_waypoints_vis();
        } else {
            // if -1.0 > height, end of input
            if (waypoints.poses.size() >= 1) {
                publish_waypoints_vis();
                publish_waypoints();
            }
        }
    }
}

void traj_start_trigger_callback(const geometry_msgs::PoseStamped& msg) {
    if (!is_odom_ready) {
        ROS_ERROR("[waypoint_generator] No odom!");
        return;
    }

    ROS_WARN("[waypoint_generator] Trigger!");
    trigged_time = odom.header.stamp;
    ROS_ASSERT(trigged_time > ros::Time(0));

    ros::NodeHandle n("~");
    n.param("waypoint_type", waypoint_type, string("manual"));

    ROS_ERROR_STREAM("Pattern " << waypoint_type << " generated!");
    if (waypoint_type == string("free")) {
        waypoints = point();
        publish_waypoints_vis();
        publish_waypoints();
    } else if (waypoint_type == string("circle")) {
        waypoints = circle();
        publish_waypoints_vis();
        publish_waypoints();
    } else if (waypoint_type == string("eight")) {
        waypoints = eight();
        publish_waypoints_vis();
        publish_waypoints();
   } else if (waypoint_type == string("point")) {
        waypoints = point();
        publish_waypoints_vis();
        publish_waypoints();
    } else if (waypoint_type == string("series")) {
        load_waypoints(n, trigged_time);
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "waypoint_generator");
    ros::NodeHandle n("~");
    n.param("waypoint_type", waypoint_type, string("manual"));

    ros::Subscriber sub1 = n.subscribe("odom", 10, odom_callback);
    ros::Subscriber sub2 = n.subscribe("goal", 10, goal_callback);
    // 没用到
    ros::Subscriber sub3 = n.subscribe("traj_start_trigger", 10, traj_start_trigger_callback);
    
    pub1 = n.advertise<nav_msgs::Path>("waypoints", 50);
    pub2 = n.advertise<geometry_msgs::PoseArray>("waypoints_vis", 10);

    trigged_time = ros::Time(0);

    ros::spin();
    return 0;
}

参考引用

代码来源:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_2.
相关知识来源:深蓝学院<<移动机器人运动规划>>