global_planner解析（3）

这章我们对路径搜索的基类Expander进行讲解：
该部分代码在expander.h文件中，

#ifndef EXPANDER_H
#define EXPANDER_H
#include <nav_planner/planner_core.h>
#include <nav_planner/potenrial.h>
namespace nav_planner
{
//扩展器
class Expander
{
public:
    //构造函数
    Expander(PotentialCalculator* p_calc, int nx, int ny):
            unknown_(true),lethal_cost_(253),neutral_cost_(50),factor_(3.0),p_calc_(p_calc){
                setSize(nx,ny);
            }
    
    //纯虚函数
    virtual bool calculatePotentials(unsigned char* costs,
                            double start_x, double start_y,
                            double end_x, double end_y,
                            int cycles,
                            float* potential) = 0;

    //设置或者重置地图的大小  , x,y
    virtual void setSize(int nx,int ny){
        nx_ = nx;
        ny_ = ny;
        ns_ = nx * ny;
    }

    //设置致命代价大小
    void setLethalCost(unsigned char lethal_cost){
        lethal_cost_ = lethal_cost;
    }
    //设置中立代价大小
    void setNeutralCost(unsigned char neutral_cost){
        neutral_cost_ = neutral_cost;
    }
    //设置因子大小
    void setFactor(float factor){
        factor_ = factor;
    }
    //设置未知
    void setHasuUnknown(bool unknown){
        unknown_ = unknown;
    }

    //清空端点
    void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s){
        //返回一维数组下标
        int startCell = toIndex(gx,gy);
        //i代表x,j代表y;
        for(int i = -s, i =< s; i++){
            for (int j = -s; j <= s; j++){
                //n是该次循环地图数组的下标
                int n = startCell + i + nx_ * j;
                //POT_HIGH 是 未分配的栅格值
                if (potential[n] < POT_HIGH)
                    continue;
                //
                float c = costs[n] + neutral_cost_;
                float pot = p_calc_->PotentialCalculator(potential, c, n);
                potenrial[n] = pot;
            }
            
        }
    }
protected:
    //返回一维数组下标
    inline int toIndex(int x, int y){
        return x+nx_*y;
    }
    int nx_,ny_,ns_;    //地图的长宽和面积
    bool unknown_;      //未知
    unsigned char lethal_cost_;     //致命代价
    unsigned char neutral_cost_;    //中立/中性 代价
    int cells_visited_;             //已经遍历过的栅格
    float factor_;                  //因子,因素
    PotentialCalculator* p_calc_;             //计算器
private:
    /* data */
};
}
#endif

该类主要有清空端点、设置地图大小还有最重要的启发函数的接口*
最主要的就是启发函数的接口

    virtual bool calculatePotentials(unsigned char* costs,
                            double start_x, double start_y,
                            double end_x, double end_y,
                            int cycles,
                            float* potential) = 0;

这个在地杰斯特拉算法和astar算法等具体算法中实现

接下来我们讲解启发函数的计算方式，分别有直接使用前后左右四个空格的代价值与使用二次逼近函数来计算的方式，我们这次就来讲普通的
该部分代码在potential_calculator.h文件中，

#ifndef POTENTIAL_H
#define POTENTIAL_H
//
//潜力计算器
class PotentialCalculator
{
public:
    PotentialCalculator(int nx,int ny){
    	//设置地图大小
        setSize(nx,ny);
    }
    //计算方式
    virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential = -1){
        if (prev_potential < 0)
        {
        	//分别求出前后、左右最小的代价值
            float min_h = std::min(potential[n-1] , potential[n+1]);
            float min_v = std::min(potential[n-nx_] , potential[n+nx]);
            //从最小值中再取最小，也就是四个最小的
            prev_potential = std::min(min_h , min_v);
        }
        //返回最小的代价值+当前已经累计的cost
        return prev_potential + cost;
    }
    //设置或重置地图的大小
    virtual void setSize(int x,int y){
        nx_ = nx;
        ny_ = ny;
        ns_ = nx*ny;
    }
protected:
        //求出地图坐标点 x,y 在一维数组里面的下标
        inline int toIndex(int x,int y){
            return x + nx_*y;
        }
        int nx_ , ny_ , ns_ ; //地图的长度
};
#endif

PotentialCalculator类主要的函数就是calculatePotential，返回当前点四周最小的代价值及累加前面累加的代价值。
其子类QuadraticCalculator在文件quadratic_calculator.h及quadratic_calculator.cpp中，里面将calculatePotential方法重写使用二次逼近方式，这里不进行细讲