source devel/setup.bash
roslaunch grid\_path\_searcher demo.launch

int main(int argc, char** argv)
{
    ......
    // 订阅到地图信息的回调函数
    _map_sub  = nh.subscribe( "map",       1, rcvPointCloudCallBack );
    // 订阅到终点信息的回调函数
    _pts_sub  = nh.subscribe( "waypoints", 1, rcvWaypointsCallback );
    ......
    // 定义了结构体AstarPathFinder变量_astar_path_finder，该结构体存储、实现了Astar路径规划所需的所有信息和功能
    _astar_path_finder  = new AstarPathFinder();
    _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);
}

void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map)
{
    ......
    // 将障碍物信息设置进入栅格化地图，为后续路径规划做准备
    _astar_path_finder->setObs(pt.x, pt.y, pt.z);
    _jps_path_finder->setObs(pt.x, pt.y, pt.z);

    // 可视化地图部分
    ......
    map_vis.header.frame_id = "/world";
    _grid_map_vis_pub.publish(map_vis);
    ......
}

void rcvWaypointsCallback(const nav_msgs::Path & wp)
{
    // 获取交互式界面给出的终点坐标
    Vector3d target_pt;
    target_pt << wp.poses[0].pose.position.x,
                 wp.poses[0].pose.position.y,
                 wp.poses[0].pose.position.z;
    ......

    // 输入起点、终点、调用pathFind函数
    pathFinding(_start_pt, target_pt); 
}

void pathFinding(const Vector3d start_pt, const Vector3d target_pt)
{
    // 使用A*进行路径搜索
    _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();

    // 进行JPS路径规划编写时，将_use_jps的值置为1即可
#define _use_jps 0
#if _use_jps
    {
        // 使用JPS进行路径搜索
        _jps_path_finder -> JPSGraphSearch(start_pt, target_pt);
        ......
    }
#endif
}

struct GridNode
{     
    int id;        // 1--> open set, -1 --> closed set
    Eigen::Vector3d coord; 
    Eigen::Vector3i dir;   // direction of expanding
    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(){};
};

class AstarPathFinder
{
    private:

	protected:
        ......
        // open set实现：用C++ STL中的multimap
        std::multimap<double, GridNodePtr> openSet;

        // 启发式函数（待完成）
        double getHeu(GridNodePtr node1, GridNodePtr node2);

        // 拓展节点函数（待完成）
        void AstarGetSucc(GridNodePtr currentPtr, std::vector<GridNodePtr> & neighborPtrSets, std::vector<double> & edgeCostSets);
    public:
        ......
        // A*搜索算法函数（待完成）
        void AstarGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
        ......
}

class JPSPathFinder: public AstarPathFinder
{
    ......
    // JPS 的拓展节点函数，已经完成
    void JPSGetSucc(GridNodePtr currentPtr, std::vector<GridNodePtr> & neighborPtrSets, std::vector<double> & edgeCostSets);

    //JPS 搜索算法函数，主体框架和 A\*一致，只要用心对照修改，在完成了A*的基础，使用提供的函数接口完成JPS难度不打
    void JPSGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
}

void AstarPathFinder::AstarGetSucc(...);
double AstarPathFinder::getHeu(...);
void AstarPathFinder::AstarGraphSearch(...);
vector<Vector3d> AstarPathFinder::getPath(...);

sudo chmod +x install-ompl-ubuntu.sh
./install-ompl-ubuntu.sh

find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
# add your code here: find_package(xxx REQUIRED)

#include <ompl/config.h>
#include <ompl/base/StateSpace.h>
#include <ompl/base/Path.h>
#include <ompl/base/spaces/RealVectorBounds.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/StateValidityChecker.h>
#include <ompl/base/OptimizationObjective.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/SimpleSetup.h>

// Our collision checker. For this demo, our robot's state space
class ValidityChecker : public ob::StateValidityChecker
{
public:
    ValidityChecker(const ob::SpaceInformationPtr& si) :
        ob::StateValidityChecker(si) {}
    // Returns whether the given state's position overlaps the
    // circular obstacle
    bool isValid(const ob::State* state) const
    {   
        // We know we're working with a RealVectorStateSpace in this
        // example, so we downcast state into the specific type.
        const ob::RealVectorStateSpace::StateType* state3D =
            state->as<ob::RealVectorStateSpace::StateType>();
        /**
        *
        *
        STEP 1: Extract the robot's (x,y,z) position from its state
        *
        *
        */

        return _RRTstar_preparatory->isObsFree(x, y, z);
    }
};