基础实现

对照文档完成src/grid_path_searcher/src/Astar_seracher.cpp

在工作空间下，执行

source devel/setup.bash
roslaunch grid_path_searcher demo.launch

使用3D Nav Goal指定目标点，可见规划出的Path（下图中下部绿色Path）

点云数据的保存与加载

参考：

PCL：读写pcd点云的两种方式_孙 悟 空的博客-CSDN博客_pcl 读取pcd

Point Cloud Library (PCL): pcl Namespace Reference

在src/grid_path_searcher/src/random_complex_generator.cpp中修改

保存点云：

pcl::PCDWriter pcdWriter;
if (!cloudMap.empty())
{
    string file_name = "/home/chen/ros_ws/mobile_robots_motion_planning/hw2/src/grid_path_searcher/src/point_cloud.pcd";
    pcdWriter.writeBinary(file_name, cloudMap);
    cout << "Point cloud saved：" << cloudMap.points.size() << endl;
}
else
{
    PCL_ERROR("\\a->NO point cloud！\\n");
}

TODO：不知为何，保存点云的路径必须是绝对路径才能生效，相对路径不报错，但没有点云文件生成。

加载点云：

// Load point cloud from disk
pcl::PCDReader pcdReader;
string file_name = "point_cloud.pcd";
pcdReader.read(file_name, cloudMap);
cout << "Loaded point cloud：" << cloudMap.points.size() << endl;

不同启发函数对A*运行效率的影响

为统一变量，在对比实验中需将地图与目标点固定。此处将一次实验的点云数据保存，之后直接加载固定的点云数据。

固定发布目标点参考：