怕什么真理无穷，进一寸有一寸的欢喜。——读《胡适谈读书》
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_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;
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[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]);
}
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);
}
std::cout << "getVisitedNodes函数结束" << std::endl;
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 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));
}
inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)//获取当前节点的相邻节点
{
std::cout << "进入AstarGetSucc函数，开始获取当前节点的相邻节点" << std::endl;
neighborPtrSets.clear(); //清空neighborPtrSets
// Note: the pointers in this set copy pointers to GridNodeMap
edgeCostSets.clear();//清空edgeCostSets
/*
*
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];//获取当前节点的x坐标
int this_y = thisNode[1];
int this_z = thisNode[2];
auto this_coord = currentPtr -> coord;//获取当前节点的坐标
int n_x, n_y, n_z;//相邻节点的x,y,z坐标
double dist;//当前节点到相邻节点的距离
GridNodePtr temp_ptr = nullptr;//临时指针
Eigen::Vector3d n_coord;//相邻节点的坐标
for(int i = -1;i <= 1;++i ){ // search in 3D space
for(int j = -1;j <= 1;++j ){//遍历相邻节点
for(int k = -1;k <= 1;++k){//遍历相邻节点,i,j,k分别表示相邻节点在x,y,z方向上的偏移
if( i == 0 && j == 0 && k == 0)//如果i,j,k都为0，跳过当前节点
continue; // to avoid this node,中文：跳过当前节点
n_x = this_x + i;//计算相邻节点的x坐标
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)))//如果相邻节点的坐标超出地图范围，跳过
//GLX_SIZE - 1为什么要减1？因为数组的下标是从0开始的，所以地图的范围是0到GLX_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);//将相邻节点的指针放入neighborPtrSets
// calculate the cost in edgeCostSets: inf means that is not unexpanded
edgeCostSets.push_back(dist); //将当前节点到相邻节点的距离放入edgeCostSets， vector<double> edgeCostSets;//存储当前节点到相邻节点的代价
// put the cost inot edgeCostSets
}
}
}
std::cout << "AstarGetSucc函数结束，结束获取相邻节点" << std::endl;
}
double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)//获取启发函数,这里使用的是对角距离,也就是三维空间中两点之间的最短距离
{ //进入这个函数，就生成了结构体变量node1和node2
std::cout << "进入getHeu函数,获取启发函数" << std::endl;
/*
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;//获取节点1的坐标
auto node2_coord = node2->coord;//获取节点2的坐标
// 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) );//计算两点在x轴上的距离
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
if(tie_break){
double p = 1.0 / 25.0;
h *= (1.0 + p);
//std::cout << "Tie Break!" << std::endl;
}
std::cout <<"getHeu函数结束,获取启发函数中的h is: "<< h << std::endl;
return h;
}
void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)//A*搜索算法,输入起点和终点
{
std::cout <<"开始AstarGraphSearch：A*搜索算法"<<std::endl;
ros::Time time_1 = ros::Time::now(); //记录开始时间，用于计算程序运行时间
//这里的time_1是一个ros::Time类型的变量，用于记录程序开始运行的时间
//index of start_point and end_point
Vector3i start_idx = coord2gridIndex(start_pt);//将起点坐标转换为栅格地图中的索引
Vector3i end_idx = coord2gridIndex(end_pt);
goalIdx = end_idx;//记录终点的索引
//Vector3i是Eigen库中的一个类，用于表示三维向量，这里用于表示三维索引
//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);//创建一个GridNode结构体指针，表示起点
GridNodePtr endPtr = new GridNode(end_idx, end_pt);
//openSet is the open_list implemented through multimap in STL library
openSet.clear();//清空openSet
// 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;//将起点的g值设置为0
startPtr -> fScore = getHeu(startPtr,endPtr); //获取启发函数，这里使用的是对角距离,也就是三维空间中两点之间的最短距离
//STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic function
startPtr -> id = 1; //把起点放入openSet中
startPtr -> coord = start_pt;//startPtr是一个GridNode结构体指针，表示起点，start_pt是起点的坐标，将起点的坐标赋值给startPtr的coord
openSet.insert( make_pair(startPtr -> fScore, startPtr) );//将起点放入openSet中
//这里的startPtr -> fScore是起点的f值，startPtr是一个GridNode结构体指针，表示起点，make_pair(startPtr -> fScore, startPtr)是一个pair对象，表示起点的f值和起点
//openSet是一个multimap对象，它的键是double类型，值是GridNodePtr类型，insert是multimap的一个函数，用于将pair对象插入到multimap中
//按照键的大小自动排序，所以openSet中的节点是按照f值从小到大排序的
// todo Note: modified, insert the pointer GridNodeMap[i][j][k] to the start node in grid map
/*
*
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;//将起点的id设置为1，表示起点在openSet中
vector<GridNodePtr> neighborPtrSets;//存储当前节点的相邻节点,这里的vector是容器，存储GridNodePtr类型的数据，gridNodePtr包含了节点的坐标，方向，索引，g值，f值，父节点，节点的迭代器
//所以这个vector的维数不止三维，而是六维，存储了相邻节点的坐标，方向，索引，g值，f值，父节点
vector<double> edgeCostSets;//存储当前节点到相邻节点的代价，这是一个一维的vector，存储了当前节点到相邻节点的代价
Eigen::Vector3i current_idx; // record the current index//记录当前节点的索引
// this is the main loop
while ( !openSet.empty() ){//当openSet不为空时，循环执行以下操作
/*
*
*
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
*
*
*/
currentPtr = openSet.begin() -> second;//这里的当前节点是openSet中f值最小的节点，second是multimap的一个函数，用于返回第二个元素的迭代器
//这里的当前节点虽然是指针，是不是对应一个新的结构体变量
//openSet是一个multimap对象，begin()是multimap的一个函数，用于返回指向第一个元素（这里的第一个元素是包含键值的迭代器）的迭代器,second是multimap的一个函数，用于返回第二个元素的迭代器
// first T1 键, second T2 值 ,pair<type 1,type 2>
openSet.erase(openSet.begin());//erase()是multimap的一个函数，用于删除指定位置的元素
// remove the node wistd::cout <<"h is: "<< h << std::endl;th minimal f value
current_idx = currentPtr->index;//获取当前节点的索引，currentPtr是一个GridNode结构体指针，表示当前节点
//currentPtr是一个GridNode结构体指针，表示当前节点，currentPtr->index是一个Vector3i类型的变量，表示当前节点的索引
GridNodeMap[current_idx[0]][current_idx[1]][current_idx[2]] -> id = -1;// update the id in grid node map,中文是更新栅格地图中的节点id，将当前节点的id设置为-1，表示当前节点在closeSet中
// 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 );
//(time_2 - time_1).toSec() * 1000.0具体解释：(time_2 - time_1).toSec()是程序运行的时间，toSec()是一个函数，用于返回时间的秒数，*1000.0是将秒转换为毫秒
//currentPtr->gScore * resolution是路径的代价，currentPtr是一个GridNode结构体指针，表示当前节点，gScore是当前节点的g值，resolution是地图的分辨率
std::cout <<"AstarGraphSearch：A*搜索算法结束"<<std::endl;
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
对于节点“n”的所有未扩展的邻居“m”，请完成这个for循环
*
*/
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];//更新相邻节点的g值
neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr,endPtr);//更新相邻节点的f值
std::cout<<"第"<<i<<"次进入getHeu函数，获取启发函数"<<std::endl;
neighborPtr->cameFrom = currentPtr;//更新相邻节点的父节点
// todo shallow copy or deep copy,翻译：浅拷贝还是深拷贝
// push node "m" into OPEN
openSet.insert(make_pair(neighborPtr -> fScore, neighborPtr));//将相邻节点放入openSet中
neighborPtr -> id = 1;//将相邻节点的id设置为1，表示相邻节点在openSet中
continue;
}
else if(neighborPtr -> id == 1){ //这个节点在openSet中，需要判断是否需要更新，当你编写代码时，应该删除“0”
//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])){//如果当前节点的g值加上当前节点到相邻节点的代价小于相邻节点的g值
neighborPtr -> gScore = currentPtr -> gScore + edgeCostSets[i];//更新相邻节点的g值
neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr,endPtr);//更新相邻节点的f值
neighborPtr -> cameFrom = currentPtr;//更新相邻节点的父节点
}
continue;//继续下一次循环
}
else{//this node is in closed set
/*
*
please write your code below
*
*/
// todo nothing to do here?
continue;//情况：这个节点在closeSet中，不需要做任何操作，继续下一次循环，在closeSet意味着这个节点已经被扩展过了
}
}
}
//if search fails
ros::Time time_2 = ros::Time::now();//记录结束时间，用于计算程序运行时间
if((time_2 - time_1).toSec() > 0.1)//如果程序运行时间大于0.1s
ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() );//输出程序运行时间
}
//获得路径，返回一个vector，存储了路径上的点，按照顺序存储，第一个点是起点，最后一个点是终点
vector<Vector3d> AstarPathFinder::getPath()
{
std::cout <<"开始getPath函数"<<std::endl;
vector<Vector3d> path;//存储路径上的点,按照顺序存储，第一个点是起点，最后一个点是终点，
vector<GridNodePtr> gridPath;//存储路径上的节点,区别：path存储的是路径上的点，gridPath存储的是路径上的节点
/*
*
*
STEP 8: trace back from the curretnt nodePtr to get all nodes along the path
please write your code below
*
*/
auto ptr = terminatePtr;//ptr是一个GridNode结构体指针，表示终点
while(ptr -> cameFrom != NULL){//当ptr的父节点不为空时，循环执行以下操作
gridPath.push_back(ptr);//将ptr放入gridPath中
ptr = ptr->cameFrom;//将ptr的父节点赋值给ptr
}
for (auto ptr: gridPath)//遍历gridPath,
//具体解释一下auto ptr: gridPath，ptr 是一个局部变量，其类型由 auto 自动推导得出，和 gridPath 中的数据类型相同。在每次循环中，ptr 都会被设置为 gridPath 中的当前元素。
path.push_back(ptr->coord);//将ptr的坐标放入path中
reverse(path.begin(),path.end());//将path中的元素逆序
std:: cout << "getPath函数结束,path size（节点数） is: " << path.size() << std::endl;//输出路径的长度
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) {//作用：如果f值的差异微不足道，则选择沿着从起点到终点的直线的路径
// todo do I have to update the f in openSet??
std::multimap<double, GridNodePtr> local_set;//存储当前节点的相邻节点
auto f_min = openSet.begin()->first;//获取openSet中f值最小的节点的f值
auto f_max = f_min + 1e-2;//f_max是f_min加上一个微不足道的差值
auto itlow = openSet.lower_bound (f_min);//获取openSet中f值最小的节点的迭代器
auto itup = openSet.upper_bound(f_max);//获取openSet中f值最大的节点的迭代器
double cross, f_new;//cross是当前节点到相邻节点的距离，f_new是相邻节点的f值
for (auto it=itlow; it!=itup; ++it){//遍历itlow到itup之间的节点
std::cout << "f value is:" << (*it).first << " pointer is: " << (*it).second << '\n';//输出f值和指针
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;//计算相邻节点的f值
local_set.insert( make_pair(f_new, (*it).second) );//将相邻节点的f值和指针放入local_set中 ，在 local_set 中插入 pair（f_new, (*it).second），这样一个 std::pair 对象，其中第一个元素是 f_new，第二个元素是 (*it).second。
// todo what is iterator, is this way correct?
}
return local_set.begin()->second;//返回local_set中f值最小的节点的指针，second是返回的是指针，表示local_set中f值最小的节点
}
Astar_searcher.h
#ifndef _ASTART_SEARCHER_H
#define _ASTART_SEARCHER_H
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#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;//地图的大小，区别于上面的地图大小，这个地图大小是一个数值，是三维地图大小的乘积
//有什么区别：GLX_SIZE, GLY_SIZE, GLZ_SIZE是地图的大小，GLXYZ_SIZE是地图大小的乘积
double resolution, inv_resolution;//分辨率和分辨率的倒数
double gl_xl, gl_yl, gl_zl;//地图的边界
double gl_xu, gl_yu, gl_zu;//地图的边界
//有什么区别：gl_xl, gl_yl, gl_zl是地图的下界，gl_xu, gl_yu, gl_zu是地图的上界
GridNodePtr terminatePtr;//终止节点,terminatePtr是一个GridNodePtr类型的指针
std::multimap<double, GridNodePtr> openSet;//multimap是一个关联容器，它的元素是一对键值对，键和值可以是任意类型，键值对是按照键的大小自动排序的
//std::multimap<double, GridNodePtr> openSet;具体解释每一部分：std是C++标准库的命名空间，multimap是一个关联容器，它的元素是一对键值对
//double是键的类型，GridNodePtr是值的类型，openSet是一个multimap对象，它的键是double类型，值是GridNodePtr类型
double getHeu(GridNodePtr node1, GridNodePtr node2);
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(){};//析构函数，释放内存
void AstarGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);//A*算法
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);//坐标取整,将坐标取整
std::vector<Eigen::Vector3d> getPath();
std::vector<Eigen::Vector3d> getVisitedNodes();
};
#endifdemo_node.cpp
#include <iostream>
#include <fstream>
#include <math.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#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;
double _x_size, _y_size, _z_size;
// useful global variables
bool _has_map = false;
Vector3d _start_pt;
Vector3d _map_lower, _map_upper;
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();//创建A*路径规划对象,AsatrPathFinder是一个类
JPSPathFinder * _jps_path_finder = new JPSPathFinder();//创建JPS路径规划对象
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)//接收路径规划的目标点,并调用路径规划函数,进行路径规划
{
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;
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_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(cloud_vis, map_vis);
map_vis.header.frame_id = "/world";
_grid_map_vis_pub.publish(map_vis);//发布地图,用于rviz显示,发布的是经过处理的地图
_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);//调用A*算法,进行路径规划
//Retrieve the path
auto grid_path = _astar_path_finder->getPath();//获取路径,获取A*算法的路径,并获取已经访问的节点
auto visited_nodes = _astar_path_finder->getVisitedNodes();//获取已经访问的节点,获取A*算法的已经访问的节点,用于可视化
//Visualize the result
visGridPath (grid_path, false);//可视化路径,可视化A*算法的路径
visVisitedNode(visited_nodes);//可视化已访问的节点,可视化A*算法的已经访问的节点
//Reset map for next call
_astar_path_finder->resetUsedGrids();//重置地图,重置A*算法的地图,用于下一次路径规划
//_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)
{
ros::init(argc, argv, "demo_node");
// 输出节点启动提示
ROS_INFO("Demo node has started.");
ros::NodeHandle nh("~");//创建节点句柄,用于与ROS系统通信,创建一个私有节点句柄,用于与ROS系统通信
std::cout << "Hold on, we'll win!" << std::endl;
_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);//发布地图,用于rviz显示,发布的是经过处理的地图
_grid_path_vis_pub = nh.advertise<visualization_msgs::Marker>("grid_path_vis", 1);//发布路径,用于rviz显示,发布的是路径
_visited_nodes_vis_pub = nh.advertise<visualization_msgs::Marker>("visited_nodes_vis",1);//发布已访问的节点,用于rviz显示,发布的是已经访问的节点
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();//创建A*路径规划对象
_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);
ros::Rate rate(100);//设置循环的频率,100Hz,具体解释一下：ros::Rate是一个类，用于控制循环的频率，这里的rate是对象还是构造函数？是对象
bool status = ros::ok();//节点是否正常运行
while(status) //节点是否正常运行
{
ros::spinOnce(); //检查是否有消息传入,处理回调函数
status = ros::ok();//节点是否正常运行
rate.sleep();//根据前面的设置,设置的频率是100Hz，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();
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;
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);
}node.h
#ifndef _NODE_H_
#define _NODE_H_
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
#define inf 1>>20//定义一个无穷大的值，1左移20位，1的二进制是0001，左移20位后变成1000000000000000000000，这是一个很大的数
struct GridNode;//声明一个节点结构体,这是一个结构体，不是一个类
typedef GridNode* GridNodePtr;//定义一个节点指针,GridNodePtr是一个指向GridNode的指针，这里typedef是一个关键字，用于给已有的数据类型定义一个新的名字
//完整解释一下：typedef是C++中的一个关键字，用于给已有的数据类型定义一个新的名字，GridNode是一个结构体，GridNodePtr是一个指向GridNode的指针
//这里已有的数据类型是什么：GridNode是一个结构体，GridNodePtr是一个指向GridNode的指针
struct GridNode//定义一个节点结构体，包含了节点的坐标，方向，索引，g值，f值，父节点，节点的迭代器，这不是一个类吧，这是一个结构体
{
int id; // 1--> open set, -1 --> closed set
Eigen::Vector3d coord; //世界坐标系下的坐标，节点的坐标，坐标中心是地图的中心,这是 一个三维向量
Eigen::Vector3i dir; // direction of expanding
Eigen::Vector3i index;//节点的索引，索引是节点在地图中的位置，和coord的区别是，索引是整数，而坐标是浮点数
//举例说明index:假设地图的分辨率是0.1m，地图的中心是(0,0,0)，那么地图的左下角的坐标是(-50,-50,-50)，那么地图的左下角的索引是(-500,-500,-500)
double gScore, fScore;
GridNodePtr cameFrom;//父节点，节点的父节点，这是一个指针，指向节点的父节点
std::multimap<double, GridNodePtr>::iterator nodeMapIt;
GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord){ //构造函数,初始化节点的各个参数，输入是节点的索引和坐标
id = 0;//节点的id
index = _index;//节点的索引
coord = _coord;//节点的坐标
dir = Eigen::Vector3i::Zero();//节点的方向
//Eigen::Vector3i::Zero()详细解释一下：Eigen::Vector3i是一个三维向量，Eigen::Vector3i::Zero()是一个三维向量，三个元素都是0
//分别介绍Eigen和Vector3i：Eigen是一个C++模板库，用于线性代数运算，Vector3i是Eigen库中的一个类，表示一个三维向量
//Zero()是Vector3i类中的一个函数，用于初始化一个三维向量，将向量的三个元素都初始化为0
gScore = inf;//节点的g值,inf是一个无穷大的值
fScore = inf;//节点的f值,inf是一个无穷大的值
cameFrom = NULL;//节点的父节点,初始化为NULL
}
GridNode(){};//构造函数,初始化节点的各个参数
~GridNode(){};//析构函数,释放节点的内存
};
//结构体什么时候调用构造函数，什么时候调用析构函数
//结构体的构造函数和析构函数是在结构体的变量被创建和销毁的时候调用的
//openSet.begin() -> second 返回的是一个 GridNodePtr，该指针已经指向了一个已经构造完成的 GridNode 对象。这个 GridNode 对象的创建和构造在之前的代码中已经完成。
#endif
知识点回顾
通常在编程中遇到的一些常见概念。为了便于理解，我将归类并简单描述。
指针：
指针是存储内存地址的变量。在C和C++编程中，我们可以使用指针访问不同的内存区域，从而获取或更改其内容。指针对于有效的内存管理和高效的编程任务（如动态内存分配和数据结构的操作）至关重要。数组：
数组是一个数据集合，其中所有数据都是相同类型的。在大多数编程语言中，数组中的元素可以通过其索引访问（通常从 0 开始）。对象和类：
对象和类是面向对象编程的两个基本概念。类定义了一组属性（数据元素）和方法（对数据执行操作的函数）。它是一个模板，我们可以根据这个模板创建对象（实例）。对象则是类的实例，具有定义在类中的属性和方法。结构体：
结构体是一种数据类型，在该类型中可以存储多种不同类型的数据。在C和C++中，我们可以使用struct关键字创建结构体。结构体通常用于存储相关联的数据项。模板：
在C++编程中，模板是一种功能强大的工具，允许我们编写独立于类型的代码。C++有两种模板，函数模板和类模板。这意味着我们可以创建一个可以处理多种数据类型的函数或类，并且数据类型可以在编译时确定。
结构体（Struct）并不是一个对象，而是一种用户定义的数据类型，用于组合并存储不同类型的数据。你可以将结构体看作是一个创建自定义数据类型的模板。在C和C++编程中，结构体通常用于存储一些逻辑上相关的信息。
创建结构体后，我们可以按照所定义的结构创建多个结构体变量（就像对象），并为这些变量的每个成员赋值。这些结构体变量存储的是结构体定义的数据类型。每个结构体变量都有自己的内存空间，它们的值相互独立，更改其中一个并不会影响其他变量。
例如，我们可以创建一个包含名字（字符串）和年龄（整数）的”Person”结构体，然后可以创建多个结构体变量（如tom、bob等），每个变量都会有自己特定的名字和年龄。
在C++和其他一些类似编程语言中，点（.）和箭头（->）都用于访问结构体或对象的成员，但它们的使用情景有所不同：
点操作符（.）：当你直接操作结构体或对象时，使用点操作符来访问其成员。比如有一个名为
person的结构体实例，我们可以用person.name来访问其name成员。箭头操作符（->）：当你有一个指向结构体或对象的指针时，使用箭头操作符来访问其成员。比如有一个指向
person的指针p，我们可以用p->name来访问person的name成员。
总结一下，如果你持有的是实体的话就使用点.，如果你持有的是指针就使用箭头->。
在C++中，定义一个结构体及其变量可以参照以下的步骤：
定义结构体：首先，我们需要定义结构体的数据结构，使用关键字
struct加上你想要的结构体名字，然后在大括号{}中定义你需要的成员变量，比如我们定义一个名为Person的结构体，它包含name和age两个成员：struct Person { std::string name; int age; };定义结构体变量：有了结构体的定义后，我们可以根据这个模板来创建结构体的变量。可以直接在定义结构体的后面加上变量名来定义，也可以在后面的代码中通过
struct 结构体名 变量名;来定义，比如：// 定义结构体时直接定义了一个名为 tom 的变量 struct Person { std::string name; int age; } tom; // 在代码中定义一个名为 bob 的变量 struct Person bob;给结构体变量赋值：定义好结构体变量后，我们可以通过点操作符
.来给变量的各个成员赋值：tom.name = "Tom"; tom.age = 20; bob.name = "Bob"; bob.age = 25;
现在， tom 和 bob 就都已经安全地存储了他们各自的 name 和 age 信息。你可以按需在程序中使用它们。
std::vector 是一个 C++ 标准库提供的动态数组，其大小可以根据需要进行增加或减小。vector<double> 则表示元素类型为 double 的 vector，即可以存储 double 类型元素的动态数组。
以下介绍 vector<double> 的一些基本使用方法：
初始化：可以在声明时直接初始化，也可以在声明后添加元素。
std::vector<double> v1; // 声明一个空的vector std::vector<double> v2(5, 1.0); // 声明一个含有5个元素的vector，所有元素值为1.0 std::vector<double> v3 = {1.0, 2.0, 3.0, 4.0, 5.0}; // 声明并初始化vector添加元素：
vector提供了push_back函数可以在尾部添加元素。v1.push_back(1.0); // 在v1的尾部添加一个元素1.0访问元素：可以使用索引操作符
[]或at函数访问元素，使用front和back分别获取首元素和尾元素。double d1 = v1[0]; // 使用索引访问元素 double d2 = v1.at(0); // 使用at函数访问元素 double d3 = v1.front(); // 获取首元素 double d4 = v1.back(); // 获取尾元素修改元素：可以使用索引操作符
[]或at函数修改元素。v1[0] = 2.0; // 使用索引修改元素 v1.at(0) = 3.0; // 使用at函数修改元素删除元素：使用
pop_back可以删除尾部元素，使用erase可以删除指定位置的元素。v1.pop_back(); // 删除尾部元素 v1.erase(v1.begin()); // 删除首元素查看容量和大小：使用
size函数可以查看vector中已有元素的数量，使用capacity可以查看vector当前的容量，使用empty可以判断vector是否为空。size_t s = v1.size(); // 获取vector中元素的数量 size_t c = v1.capacity(); // 获取vector的当前容量 bool b = v1.empty(); // 判断vector是否为空
此外，std::vector 还提供了很多其他的功能，比如排序，查找，复制等，可根据具体需要使用。
openSet.begin() 的确会返回一个迭代器，该迭代器指向 multimap 中的第一个元素（关键字最小的元素）。然而，需要注意的是，这个迭代器不是指容器本身，而是指向容器内的一个元素。
在这个上下文中，begin() -> second 是指迭代器指向的首个元素的值。如果 openSet 是一个 std::multimap<KeyType, ValueType>，那么 openSet.begin() -> second 就对应了首个元素的 ValueType 值。
这是因为 multimap 中每个元素都是一个键-值对，可以用 first 访问键，用 second 访问值。在这里，键对应迭代器的 first 属性，值对应迭代器的 second 属性。
因此，currentPtr = openSet.begin() -> second; 这行代码的作用是将 openSet 中第一个元素的值赋给 currentPtr。
openSet.begin()返回的是一个迭代器，这个迭代器指向openSet这个multimap容器中的第一个元素。迭代器可以理解为容器中元素的地址或者引用，是一个可以用于遍历和操作容器内元素的工具，而非容器本身。所以，openSet.begin()并不会返回容器本身。
名称 “ptr” 通常在编程语言中表示 “pointer”，指针是一种特殊的变量，其值为另一个变量的地址，即它”指向”另一个存在内存中的位置。具有指针特性的变量，我们就通常称它为 “指针”。
指针通常用于以下几种情况：
动态内存管理： 当你想要在运行时动态分配内存空间时，会使用指针。例如在 C++ 中，我们可以使用
new和delete来分配和释放内存，它们都操作指针。操作数组： 指针和数组有很密切的关系。实际上，数组名是一个指向数组首元素的指针。
传递函数参数： 当我们想要在函数内部修改外部变量的值，或者传递大量数据时，会使用指针或者引用。这比直接传值更有效率，因为我们只是传递了数据的地址，而不是复制了整份数据。
实现高级数据结构：如链接表、树、图等数据结构，都需要使用指针。
所以，“ptr”是一个非常常见的名称，它通常代表一个指向某个数据或者对象的指针。
只包含一个键-值对的对象也可以称之为容器。在编程中，容器是用来持有和管理一组元素的数据结构。这些元素可以是基本的数据类型，也可以是复杂的对象类型，甚至是键-值对。
例如在 C++ 中，std::map、std::unordered_map 或者 std::multimap 都是可以存放键-值对的容器。即使这些容器中只有一个键-值对，也是被认为是容器的实例。
这是因为，容器的定义并非由其包含元素的数量决定，而是由其提供的用于访问、插入和删除元素等操作的接口决定。因此，最小的容器是空容器，即不包含任何元素的容器。
vector<double> edgeCostSets; 这段代码是在声明一个名为 edgeCostSets 的 vector 容器，该容器用于存储 double 类型的数据。
std::vector 是 C++ 标准库中的一种序列容器，用于存储动态大小的元素集合，其大小可以根据需要自动调整。
举例说明 edgeCostSets 的一些常见用法：
添加元素：
我们可以使用 push_back() 方法向 vector 尾部添加元素：
edgeCostSets.push_back(1.0);
edgeCostSets.push_back(2.5);访问元素：
我们可以使用索引（跟数组类似）或者迭代器访问 vector 里的元素：
double firstElement = edgeCostSets[0]; // 使用索引访问
// 使用迭代器遍历并打印所有元素
for(vector<double>::iterator it = edgeCostSets.begin(); it != edgeCostSets.end(); ++it) {
cout << *it << " ";
}获取大小：
可以使用 size() 方法获取 vector 的大小（元素个数）：
std::size_t size = edgeCostSets.size();删除元素：
删除元素通常可以使用 erase() 方法：
edgeCostSets.erase(edgeCostSets.begin()); // 删除第一个元素综上，vector<double> edgeCostSets; 定义了一个可以存储 double 类型数值的动态数组 edgeCostSets。
当我们声明一个 std::vector<double> edgeCostSets; 时，我们实际上创建了一个动态数组 edgeCostSets，它可以持久化 double 类型的数据。初始化时，edgeCostSets 是空的，没有任何元素。
我们可以把 std::vector 容器想象成一条连续的内存存储线，对应到它的物理结构，它是在堆内存中连续分配的一片内存区域。每一个 double 类型的元素都按顺序放在这条线上的一个位置。
假设我们这样操作：
edgeCostSets.push_back(1.1);
edgeCostSets.push_back(2.2);
edgeCostSets.push_back(3.3);在这个情况下容器的内部结构如下：
index: 0 1 2
value: 1.1 2.2 3.3我们向 edgeCostSets 中追加了3个元素，每次调用 push_back 方法都会在 vector 的末尾添加一个新的元素。这些元素都存储在连续的内存地址上，我们可以通过索引（从0开始计数）来访问各个元素。
由于 std::vector 是动态数组，如果需要添加更多的元素，内存会自动重新分配并扩展以容纳新的元素。同时，由于是连续的内存空间，访问效率高，但插入删除效率低（尤其在中间位置插入删除）。使用 std::vector 需要根据实际需求判断其优劣。
vector<GridNodePtr> neighborPtrSets; 这段代码中，我们在创建一个名为 neighborPtrSets 的 vector 容器，该容器用于存储 GridNodePtr 类型的数据。这里 GridNodePtr 可能是一个指向 GridNode 对象的指针，或者是某种用户自定义类型。我会以 GridNodePtr 是一个指向对象的指针来进行解释。
std::vector 是 C++ 标准库中的一种序列容器，用于存储动态大小的元素集合。在物理结构上，它是在堆内存中连续分配的一片内存区域。
假设我们已经定义了 GridNode 类，并且已经初始化了一些 GridNode 对象和相应的指针 GridNodePtr。
举例来说：
GridNode node1, node2, node3;
GridNodePtr nodePtr1 = &node1;
GridNodePtr nodePtr2 = &node2;
GridNodePtr nodePtr3 = &node3;
neighborPtrSets.push_back(nodePtr1);
neighborPtrSets.push_back(nodePtr2);
neighborPtrSets.push_back(nodePtr3);在此例中，我们创建了3个 GridNode 对象，然后将它们的地址（即 GridNodePtr 指针）追加到了 neighborPtrSets 中。这样后，neighborPtrSets 的内部结构会如下所示：
index: 0 1 2
value: &node1 &node2 &node3这样，neighborPtrSets 就存储了三个指针，这些指针都是指向 GridNode 对象的指针。我们可以通过这些存储在 neighborPtrSets 中的指针来访问和操作原始对象。
请注意，容器中实际存储的是指针，而不是对象本身。因此，如果原始对象被修改，那么通过 neighborPtrSets 访问时得到的对象内容也会相应改变。同样，如果原始对象被销毁了，那么在 neighborPtrSets 中对应的指针就会成为悬空指针，此时再通过这个指针来访问对象就会出现未定义的行为。
当创建一个新的 GridNodePtr，比如 GridNodePtr currentPtr，并且你让它指向一个新的 GridNode 对象时，就会调用一次 GridNode 的构造函数。实例化 GridNode 对象的方式可能是使用 new 关键字等。
假设你有如下的代码：
GridNodePtr currentPtr = new GridNode();在这个例子中，一个新的 GridNode 对象被创建，同时 GridNode 的构造函数被执行。currentPtr 就是指向这个新创建的 GridNode 对象的指针。
确保当你不再需要这个 GridNode 对象时，你要使用 delete 关键字来释放它，防止内存泄漏。
delete currentPtr;
currentPtr = nullptr; // 防止成为野指针记住，对于在堆上动态分配的对象，你必须明确释放它们，否则这些对象将一直占据内存，直到程序结束，这可能会导致内存泄漏。
代码 currentPtr = openSet.begin() -> second; 中，实际上并没有创建一个新的 GridNode 对象，也就没有调用 GridNode 的构造函数。
这段代码的含义是，将 currentPtr 指向 openSet 容器的第一个元素的 second 成员。这里假设 openSet 是一个像 std::map<Key, GridNodePtr> 或 std::unordered_map<Key, GridNodePtr> 这样的容器，它的元素是由 Key 类型的键和 GridNodePtr 类型的值组成的键值对。
所以，openSet.begin() -> second 获取的是 openSet 的第一个元素（键值对）的值部分，这个值应该是一个 GridNodePtr，指向某个 GridNode 对象。然后，你将 currentPtr 指向了这个已经存在的 GridNode 对象。
请注意，这种情况下，你有责任确保 openSet.begin() -> second 指向的 GridNode 对象在使用 currentPtr 的整个生命周期中都是有效的。换句话说，如果在还在使用 currentPtr 的时候，openSet.begin() -> second 指向的对象被销毁了，那么 currentPtr 就会变成一个悬空指针，这将导致未定义的行为。
openSet.begin() -> second 返回的是一个 GridNodePtr，该指针已经指向了一个已经构造完成的 GridNode 对象。这个 GridNode 对象的创建和构造可能在之前的代码中已经完成，这可能包括调用构造函数初始化对象等操作。
举个例子，以下面这段代码为例：
// 假设 GridNode 实现了以下构造函数
GridNode::GridNode(int value) {
nodeValue = value;
}
// 在某处创建了一个新的 GridNode 对象，并加入到 openSet 中
int keyValue = 1;
GridNodePtr nodePtr = new GridNode(10);
openSet[keyValue] = nodePtr;
// 然后在其他地方，你可能会这样使用
GridNodePtr currentPtr = openSet.begin() -> second;在此例中，openSet.begin() -> second 就会返回之前创建的 GridNodePtr，它指向的 GridNode 的构造函数在前面的代码中就已经调用过了。所以 currentPtr 最终指向的是这个已经构造并初始化完成的 GridNode 对象。
continue 语句仅跳过当前循环，并立即开始下一个迭代。对于嵌套循环，continue 仅适用于最内层的循环。
在你给的这段代码中，如果 i == 0 && j == 0 && k == 0，continue 会导致立即开始下一个 k 循环的迭代，而不是跳过整个 k 循环或 j 或 i 循环。也就是说，如果 continue 语句被执行，那么它将跳过下面的代码（如果有的话），并立即在 k 循环中开始下一个迭代。如果 k 循环已经是最后一次迭代，那么它将跳到 j 循环的下一次迭代，对于 j 循环同样如此。
如果你想在某个条件下跳过所有的循环或者跳出全部循环，你可能需要使用更复杂的控制流，例如设置一个标志变量或使用 goto 语句。（但是注意，goto 语句在现代编程中的使用通常是不被推荐的，因为它可能会使代码难以理解和维护。）
在 C++ 中，for 循环有多种变体。这里使用的是 C++11 引入的基于范围的 for 循环。这种循环语句会依次迭代容器（如数组，向量，集合等）中的每个元素。
在你给出的代码 for (auto ptr: gridPath) 中：
gridPath是一个容器，如std::vector、std::list、std::array、std::set、std::map等。这个容器中应该含有你感兴趣的数据。ptr是一个局部变量，其类型由auto自动推导得出，和gridPath中的数据类型相同。在每次循环中，ptr都会被设置为gridPath中的当前元素。这样，你就可以在for循环的代码块中使用ptr来操作当前元素。
例如，如果 gridPath 是一个 std::vector<int>，ptr 则为 int 类型的元素；如果 gridPath 是一个 std::vector<GridNodePtr>，ptr 则为 GridNodePtr 类型的元素。
这种基于范围的 for 循环是一种非常便捷的方式，可以轻易遍历容器，而无需管理索引或迭代器。下面是一个具体的例子：
std::vector<int> numbers = {1, 2, 3, 4, 5};
for (auto num: numbers) {
std::cout << num << std::endl;
}这将输出：
1
2
3
4
5vector<Vector3d> path; 这行代码定义了一个名为 path 的 std::vector 容器，而这个容器中存储的元素类型是 Vector3d。Vector3d 通常代表一个包含三个数值的三维向量。
具体来说，Vector3d 可能是一个类或一个结构体，它通常包含三个浮点数成员（例如 x、y 和 z），用于描述三维空间中的一个点或者一个方向向量。例如，在某些图形编程库或者物理引擎中，Vector3d 可能被定义为：
struct Vector3d {
double x;
double y;
double z;
};在这种情况下，vector<Vector3d> path; 将存储一系列的三维坐标或向量。例如，你可以这样初始化和使用它：
vector<Vector3d> path;
Vector3d point1 {1.0, 2.0, 3.0}; // 创建一个 Vector3d 对象
Vector3d point2 {4.0, 5.0, 6.0}; // 创建另一个 Vector3d 对象
path.push_back(point1); // 添加到 vector 的末尾
path.push_back(point2);
// 遍历 vector 并打印出每个 Vector3d 对象
for (const auto& point : path) {
cout << "x: " << point.x
<< ", y: " << point.y
<< ", z: " << point.z << endl;
}在具体实际使用中，Vector3d 的定义可能略有不同，具体取决于你所使用的库或者实际需求。
push_back 是 C++ std::vector 类的一个成员函数，其主要用途是在容器的末尾插入一个新的元素。这个函数将其参数作为新元素添加到 vector 的末尾，vector 的大小会相应地增加。
下面是 push_back 的一些常见用法：
向空 vector 添加元素：
std::vector<int> v; v.push_back(1); // v 现在包含一个元素：1向已有元素的 vector 添加元素：
std::vector<int> v = {2, 3, 4}; v.push_back(5); // v 现在包含四个元素：2, 3, 4, 5使用循环添加多个元素：
std::vector<int> v; for(int i = 0; i < 10; ++i){ v.push_back(i); // v 现在包含十个元素：0, 1, 2, ... , 9 }添加自定义类型的元素：
struct Point { int x; int y; }; std::vector<Point> v; Point p = {1, 2}; v.push_back(p); // v 现在包含一个 Point 元素：{1, 2}或者直接使用
{}添加：v.push_back({3, 4}); // v 现在包含两个 Point 元素：{1, 2}, {3, 4}
注意：如果 vector 的容量不足以存储新的元素，push_back 操作可能会导致存储内容的内存区间被重新分配，从而使所有指向旧存储内容的迭代器、引用和指针失效。为了避免频繁的内存重新分配，你可以先用 reserve 函数预先分配足够的空间。
在 C++ 中，insert 是 std::set、std::map、std::multiset、std::multimap、std::unordered_set、std::unordered_map、std::unordered_multiset 和 std::unordered_multimap 容器的成员函数之一。这个函数用于插入元素到容器中。
在你的示例代码 local_set.insert( make_pair(f_new, (*it).second) ); 中：
local_set是一个集合或映射类型容器（比如std::map,std::set,std::unordered_map或者std::unordered_set）。具体类型需要根据具体上下文来确定。(*it).second表示迭代器it所指向的元素的第二组成部分，it通常是指向一个如std::pair类型元素的迭代器。结构类似std::pair<KeyType, ValueType>，其中.first是键（或叫第一组成部分），.second是值（或叫第二组成部分）。make_pair(f_new, (*it).second)会生成一个std::pair对象，其中第一个元素是f_new，第二个元素是(*it).second。local_set.insert( make_pair(f_new, (*it).second) );最终将一个包含两个元素f_new和(*it).second的 pair 插入到local_set容器中。
举个例子，如果 local_set 是 std::map<int, int> 类型，it 是另一个相同类型 map 的迭代器，并且 f_new 是 int 类型，则以下代码将将 f_new 和 it 所迭代到的值作为新的键值对插入到 local_set 中。
std::map<int, int> another_map = {{1, 2}, {3, 4}};
auto it = another_map.begin();
int f_new = 5;
std::map<int, int> local_set;
local_set.insert( make_pair(f_new, (*it).second) ); // 在 local_set 中插入 pair（5, 2）
