#include "path_searching/dyn_a_star.h" using namespace std; using namespace Eigen; AStar::~AStar() { for (int i = 0; i < POOL_SIZE_(0); i++) for (int j = 0; j < POOL_SIZE_(1); j++) for (int k = 0; k < POOL_SIZE_(2); k++) delete GridNodeMap_[i][j][k]; } void AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size) { POOL_SIZE_ = pool_size; CENTER_IDX_ = pool_size / 2; GridNodeMap_ = new GridNodePtr **[POOL_SIZE_(0)]; for (int i = 0; i < POOL_SIZE_(0); i++) { GridNodeMap_[i] = new GridNodePtr *[POOL_SIZE_(1)]; for (int j = 0; j < POOL_SIZE_(1); j++) { GridNodeMap_[i][j] = new GridNodePtr[POOL_SIZE_(2)]; for (int k = 0; k < POOL_SIZE_(2); k++) { GridNodeMap_[i][j][k] = new GridNode; } } }

这段代码实现了A*算法中的地图初始化部分。首先定义了一个AStar类，其中定义了一个析构函数，在析构函数中释放了GridNodeMap_中的每个节点。

接着，定义了一个initGridMap函数，用于初始化地图。该函数接受一个occ_map指针，表示输入的障碍物地图，和一个pool_size向量，表示地图节点池的大小。函数首先将pool_size赋值给类中的POOL_SIZE_和CENTER_IDX_变量，然后根据POOL_SIZE_的大小分配GridNodeMap_的内存空间。接着，使用三重循环遍历GridNodeMap_中的每个节点，并将其初始化为一个GridNode类型的对象，该对象包含了该节点的坐标、状态等信息。

总之，这段代码的作用是初始化A*算法中的地图，将障碍物地图转化为一个三维的节点池，并将每个节点初始化为一个GridNode对象。