怕什么真理无穷，进一寸有一寸的欢喜。——读《胡适谈读书》
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) 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(); edgeCostSets.clear(); 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; 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; 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; if(isOccupied(n_x, n_y, n_z)) continue; temp_ptr = GridNodeMap[n_x][n_y][n_z]; if(temp_ptr->id == -1) continue; 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); edgeCostSets.push_back(dist); } } }std::cout << "AstarGetSucc函数结束，结束获取相邻节点" << std::endl;}double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2){ std::cout << "进入getHeu函数,获取启发函数" << std::endl; double h; auto node1_coord = node1->coord; auto node2_coord = node2->coord; 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; if(tie_break){ double p = 1.0 / 25.0; h *= (1.0 + p); } std::cout <<"getHeu函数结束,获取启发函数中的h is: "<< h << std::endl; return h;}void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt){ std::cout <<"开始AstarGraphSearch：A*搜索算法"<<std::endl; ros::Time time_1 = ros::Time::now(); Vector3i start_idx = coord2gridIndex(start_pt); Vector3i end_idx = coord2gridIndex(end_pt); goalIdx = end_idx; start_pt = gridIndex2coord(start_idx); end_pt = gridIndex2coord(end_idx); GridNodePtr startPtr = new GridNode(start_idx, start_pt); GridNodePtr endPtr = new GridNode(end_idx, end_pt); openSet.clear(); GridNodePtr currentPtr = NULL; GridNodePtr neighborPtr = NULL; startPtr -> gScore = 0; startPtr -> fScore = getHeu(startPtr,endPtr); startPtr -> id = 1; startPtr -> coord = start_pt; openSet.insert( make_pair(startPtr -> fScore, startPtr) ); GridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]] -> id = 1; vector<GridNodePtr> neighborPtrSets; vector<double> edgeCostSets; Eigen::Vector3i current_idx; while ( !openSet.empty() ){ currentPtr = openSet.begin() -> second; openSet.erase(openSet.begin()); current_idx = currentPtr->index; GridNodeMap[current_idx[0]][current_idx[1]][current_idx[2]] -> id = -1; 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 ); std::cout <<"AstarGraphSearch：A*搜索算法结束"<<std::endl; return; } AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets); for(int i = 0; i < (int)neighborPtrSets.size(); i++){ neighborPtr = neighborPtrSets[i]; if(neighborPtr -> id == 0){ neighborPtr->gScore = currentPtr->gScore + edgeCostSets[i]; neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr,endPtr); std::cout<<"第"<<i<<"次进入getHeu函数，获取启发函数"<<std::endl; neighborPtr->cameFrom = currentPtr; openSet.insert(make_pair(neighborPtr -> fScore, neighborPtr)); neighborPtr -> id = 1; continue; } else if(neighborPtr -> id == 1){ 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{ continue; } } } 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() { std::cout <<"开始getPath函数"<<std::endl; vector<Vector3d> path; vector<GridNodePtr> gridPath; 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()); std:: cout << "getPath函数结束,path size（节点数） is: " << path.size() << std::endl; return path; }GridNodePtr & TieBreaker(const std::multimap<double, GridNodePtr> & openSet, const GridNodePtr & endPtr) { 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) ); } return local_set.begin()->second;}
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;double resolution, inv_resolution;double gl_xl, gl_yl, gl_zl;double gl_xu, gl_yu, gl_zu;GridNodePtr terminatePtr;std::multimap<double, GridNodePtr> openSet;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);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();};#endif
demo_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;}double _resolution, _inv_resolution, _cloud_margin;double _x_size, _y_size, _z_size; bool _has_map = false;Vector3d _start_pt;Vector3d _map_lower, _map_upper;int _max_x_id, _max_y_id, _max_z_id;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){ 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]; _astar_path_finder->setObs(pt.x, pt.y, pt.z); _jps_path_finder->setObs(pt.x, pt.y, pt.z); 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); _has_map = true;}void pathFinding(const Vector3d start_pt, const Vector3d target_pt){ _astar_path_finder->AstarGraphSearch(start_pt, target_pt); auto grid_path = _astar_path_finder->getPath(); auto visited_nodes = _astar_path_finder->getVisitedNodes(); visGridPath (grid_path, false); visVisitedNode(visited_nodes); _astar_path_finder->resetUsedGrids(); #define _use_jps 0#if _use_jps { _jps_path_finder -> JPSGraphSearch(start_pt, target_pt); auto grid_path = _jps_path_finder->getPath(); auto visited_nodes = _jps_path_finder->getVisitedNodes(); visGridPath (grid_path, _use_jps); visVisitedNode(visited_nodes); _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("~"); 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); _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); 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); ros::Rate rate(100); bool status = ros::ok(); while(status) { 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(); 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>>20struct GridNode;typedef GridNode* GridNodePtr;struct GridNode{ int id; Eigen::Vector3d coord; 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 = Eigen::Vector3i::Zero(); gScore = inf;fScore = inf;cameFrom = NULL; } GridNode(){}; ~GridNode(){};};#endif
知识点回顾
通常在编程中遇到的一些常见概念。为了便于理解，我将归类并简单描述。
指针：
指针是存储内存地址的变量。在C和C++编程中，我们可以使用指针访问不同的内存区域，从而获取或更改其内容。指针对于有效的内存管理和高效的编程任务（如动态内存分配和数据结构的操作）至关重要。
数组：
数组是一个数据集合，其中所有数据都是相同类型的。在大多数编程语言中，数组中的元素可以通过其索引访问（通常从 0 开始）。
对象和类：
对象和类是面向对象编程的两个基本概念。类定义了一组属性（数据元素）和方法（对数据执行操作的函数）。它是一个模板，我们可以根据这个模板创建对象（实例）。对象则是类的实例，具有定义在类中的属性和方法。
结构体：
结构体是一种数据类型，在该类型中可以存储多种不同类型的数据。在C和C++中，我们可以使用struct关键字创建结构体。结构体通常用于存储相关联的数据项。
模板：
在C++编程中，模板是一种功能强大的工具，允许我们编写独立于类型的代码。C++有两种模板，函数模板和类模板。这意味着我们可以创建一个可以处理多种数据类型的函数或类，并且数据类型可以在编译时确定。
结构体（Struct）并不是一个对象，而是一种用户定义的数据类型，用于组合并存储不同类型的数据。你可以将结构体看作是一个创建自定义数据类型的模板。在C和C++编程中，结构体通常用于存储一些逻辑上相关的信息。
创建结构体后，我们可以按照所定义的结构创建多个结构体变量（就像对象），并为这些变量的每个成员赋值。这些结构体变量存储的是结构体定义的数据类型。每个结构体变量都有自己的内存空间，它们的值相互独立，更改其中一个并不会影响其他变量。
例如，我们可以创建一个包含名字（字符串）和年龄（整数）的”Person”结构体，然后可以创建多个结构体变量（如tom、bob等），每个变量都会有自己特定的名字和年龄。
在C++和其他一些类似编程语言中，点（.）和箭头（->）都用于访问结构体或对象的成员，但它们的使用情景有所不同：
总结一下，如果你持有的是实体的话就使用点.，如果你持有的是指针就使用箭头->。
在C++中，定义一个结构体及其变量可以参照以下的步骤：
定义结构体：首先，我们需要定义结构体的数据结构，使用关键字 struct 加上你想要的结构体名字，然后在大括号 {} 中定义你需要的成员变量，比如我们定义一个名为 Person 的结构体，它包含 name 和 age 两个成员：
struct Person { std::string name; int age;};
定义结构体变量：有了结构体的定义后，我们可以根据这个模板来创建结构体的变量。可以直接在定义结构体的后面加上变量名来定义，也可以在后面的代码中通过 struct 结构体名 变量名; 来定义，比如：
struct Person { std::string name; int age;} tom;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; std::vector<double> v2(5, 1.0); std::vector<double> v3 = {1.0, 2.0, 3.0, 4.0, 5.0};
添加元素：vector 提供了 push_back 函数可以在尾部添加元素。
v1.push_back(1.0);
访问元素：可以使用索引操作符 [] 或 at 函数访问元素，使用 front 和 back 分别获取首元素和尾元素。
double d1 = v1[0]; double d2 = v1.at(0); double d3 = v1.front(); double d4 = v1.back();
修改元素：可以使用索引操作符 [] 或 at 函数修改元素。
v1[0] = 2.0; v1.at(0) = 3.0;
删除元素：使用 pop_back 可以删除尾部元素，使用 erase 可以删除指定位置的元素。
v1.pop_back(); v1.erase(v1.begin());
查看容量和大小：使用 size 函数可以查看 vector 中已有元素的数量，使用 capacity 可以查看 vector 当前的容量，使用 empty 可以判断 vector 是否为空。
size_t s = v1.size(); size_t c = v1.capacity(); bool b = v1.empty();
此外，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(int value) { nodeValue = value;}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;}
这将输出：
12345
vector<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 point2 {4.0, 5.0, 6.0}; path.push_back(point1); path.push_back(point2);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);
向已有元素的 vector 添加元素：
std::vector<int> v = {2, 3, 4};v.push_back(5);
使用循环添加多个元素：
std::vector<int> v;for(int i = 0; i < 10; ++i){ v.push_back(i); }
添加自定义类型的元素：
struct Point { int x; int y;};std::vector<Point> v;Point p = {1, 2};v.push_back(p);
或者直接使用 {} 添加：
v.push_back({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) );
]]>
