#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; } } } grid_map_ = occ_map; }

这段代码是用 C++ 编写的 A* 寻路算法。它的作用是初始化一个三维地图，并在地图上创建一个节点列表。在使用完毕后，析构函数会释放节点的内存。其中，GridMap 代表地图，GridNodeMap 表示节点列表，POOL_SIZE_ 存储节点列表的大小，CENTER_IDX_ 存储节点列表中心的索引。在 initGridMap 函数中，先设置了节点列表的大小和中心索引，然后使用 new 运算符创建节点列表，并将每个节点初始化为 GridNode 类型的对象。

相关问题

#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对象。

cmake_minimum_required(VERSION 2.8.3) project(ego_planner) set(CMAKE_BUILD_TYPE "Release") ADD_COMPILE_OPTIONS(-std=c++11 ) ADD_COMPILE_OPTIONS(-std=c++14 ) set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") find_package(Eigen3 REQUIRED) find_package(PCL 1.7 REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs quadrotor_msgs plan_env path_searching bspline_opt traj_utils message_generation cv_bridge ) # catkin_package(CATKIN_DEPENDS message_runtime) catkin_package( INCLUDE_DIRS include LIBRARIES ego_planner CATKIN_DEPENDS plan_env path_searching bspline_opt traj_utils # DEPENDS system_lib ) include_directories( include SYSTEM ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ) add_executable(ego_planner_node src/ego_planner_node.cpp src/ego_replan_fsm.cpp src/planner_manager.cpp ) target_link_libraries(ego_planner_node ${catkin_LIBRARIES} ) #add_dependencies(ego_planner_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) add_executable(traj_server src/traj_server.cpp) target_link_libraries(traj_server ${catkin_LIBRARIES}) #add_dependencies(traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS})

这段代码是一个 CMakeLists.txt 文件，用于编译一个名为 ego_planner 的工程。该工程依赖于 Eigen3 和 PCL 库，同时也依赖于一些 ROS 的组件，如 roscpp、std_msgs、geometry_msgs、quadrotor_msgs、plan_env、path_searching、bspline_opt、traj_utils 和 message_generation。该工程包含两个可执行文件，一个是 ego_planner_node，另一个是 traj_server。其中 ego_planner_node.cpp 是主程序，它包含了 ego_replan_fsm.cpp 和 planner_manager.cpp 两个文件。traj_server.cpp 是用于生成轨迹的服务器程序。在编译过程中，使用了 C++11 和 C++14 标准，同时开启了 Release 模式。