global_planner解析（4）

前面简单介绍了路径搜索的基类Expander，里面主要的就是calculatePotentials路径搜索的方法接口，还有路径搜索的扩展方式-普通扩展方式类PotentialCalculator，下面我们来讲一下地杰斯特拉算法
该算法主要在dijkstra.h及dijkstra.cpp中
我们首先来看一下头文件dijkstra.h

//迪杰斯特拉算法

#ifndef _DIJKSTRA_H
#define _DIJKSTRA_H

//优先 大小
#define PRIORITYBUFSIZE 10000
#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>

#include <nav_planner/planner_core.h>
#include <nav_planner/planner_core.h>

//在current buffer 添加当前点
#define push_cur(n) \
{\
    if(n>=0 && n<ns_ && !pending_[n] && getCost(costs, n) < lethal_cost_ && currentEnd_ < PRIORITYBUFSIZE)\
    {\
        currentBuffer_[currentEnd_++] = n;\
        pending_[n] = true;\
    }\
}
//在next buffer 添加当前点
#define push_next(n) \
{\
    if (n>=0 && n < ns_ && !pending_[n] && getCost(costs, n) < lethal_cost_ && nextEnd_ < PRIORITYBUFSIZE)\
    {\
        nextBuffer_[nextEnd_++] = n;\
        pending_[n] = true;\
    }\
}
//在end buffer添加当前点
//也就是把当前点压入到 end队列里面
#define push_over(n) \
{\
    if (n>=0 && n < ns_ && !pending_[n] && getCost(costs, n) < lethal_cost_ && overEnd < PRIORITYBUFSIZE)\
    {\
        overBuffer_[overEnd_++] = n;\
        pending_[n] = true;\
    }\
}
namespace nav_planner
{
class DijkstraExpansion : public Expander
{
public:
    DijkstraExpansion(PotentialCalculator* p_calc, int nx, int ny);
    ~DijkstraExpansion();

    //启发函数
    bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles,
                    float* potential);
    //设置地图大小
    void setSize(int nx,int ny);
    //设置中性成本
    void setNeutralCost(unsigned char neutral_cost){
        neutral_cost_ = neutral_cost;
        priorityIncrement_ = 2 * neu
    }
    //设置精确开始
    void setPreciseStart(bool precise){precise_ = precise; }

private:
    /* 方法 */
    //更新索引为n的单元格
    // costs为代价
    //potential 为正在计算的点位阵列
    //n为要更新的索引
    void updateCell(unsigned char* costs, float potential, int n);

    //获取某点的代价
    float getCost(unsigned char* costs, int n){
        float c = costs[n];
        //如果改点代价 小于 致命代价-1, 或者 未知参数为1, 并且 该点cost等于255
        if (c < lethal_cost_ - 1 || (unknown_ && c==255))
        {
            //代价等于  当前代价 * 因数 + 中立代价
            c = c * factor_ + neutral_cost_;
            //如果结果大于致命代价,则该点等于致命代价-1
            if (c >= lethal_cost_)
                c = lethal_cost_ - 1;
            return c;
        }
        //返回致命代价
        return lethal_cost_;
    }
    /* data */
    //块优先级缓冲区
    //优先级块的存储缓冲区
    int *buffer1_, *buffer2_, *buffer3_;
    //优先级缓冲区块
    //当前缓冲区/下一个缓冲区/停止缓冲区
    int *currentBuffer_, *nextBuffer_, *overBuffer_;
    //数组的终点
    int currentEnd_, nextEnd_, overEnd_;
    //传播过程中的未决细胞(估计是没有启发过的点)
    bool *pending_;
    //准确
    bool precise_;

    //终止扩展代价的临界值，刚开始为致命代价
    //如果搜索完毕未找到目标点则增大该临界值继续扩展直到扩展到目标点
    float threshold_;//阈
    //优先级阈值增量
    float priorityIncrement_; //优先级 递增
};
}//end namespace nav_planner
#endif

首先上来宏定义了优先级队列的大小为10000

#define PRIORITYBUFSIZE 10000

然后是宏定义往三个队列里面push点，分别有当前队列、下一个队列、关闭队列
currentBuffer_、nextBuffer_、overBuffer_

//在current buffer 添加当前点
#define push_cur(n) \
{\
    if(n>=0 && n<ns_ && !pending_[n] && getCost(costs, n) < lethal_cost_ && currentEnd_ < PRIORITYBUFSIZE)\
    {\
        currentBuffer_[currentEnd_++] = n;\
        pending_[n] = true;\
    }\
}

当然最重要的是地杰斯特拉算法类DijkstraExpansion，继承于Expander
主要的方法
启发函数实现方法（基于Expander）calculatePotentials
更新索引为n的单元格updateCell
获取某点代价值getCost

然后看一下cpp文件中具体实现方式

//迪杰斯特拉算法

#include <nav_planner/dijkstra.h>

//算法的头文件
#include <algorithm>

namespace nav_planner{

//构造函数
DijkstraExpansion::DijkstraExpansion(PotentialCalculator* p_calc, int nx, int ny) :
                    Expander(p_calc, nx, ny), pending_(NULL), precise_(false){
                        //初始化优先级 buf 长度为PRIORITYBUFSIZE = 10000;
                        buffer1_ = new int[PRIORITYBUFSIZE];
                        buffer2_ = new int[PRIORITYBUFSIZE];
                        buffer3_ = new int[PRIORITYBUFSIZE];

                        priorityIncrement_ = 2 * neutral_cost_;
}

//析构函数
DijkstraExpansion::~DijkstraExpansion(){
    delete[] buffer1_;
    delete[] buffer2_;
    delete[] buffer3_;

    if (pending_)
        delete[] pending_;
}

//设置或初始化地图的长度
void DijkstraExpansion::setSize(int xs, int ys){
    Expander::setSize(xs,ys);
    
    if (pending_)
        delete[] pending_;
    
    pending_ = new bool[ns_];
    //初始化未决全部设置为0
    memset(pending_, 0, ns_ * sizeof(bool));
}

//迪杰斯特拉主要的启发函数
//广度优先
//
bool DijkstraExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x,
                                    double end_y, int cycles, float* potential){

    //设置已经遍历过的栅格为0
    cells_visited_ = 0;
    //阈值设置为致命代价
    threshold_ = lethal_cost_;
    //将buffer1的地址传递给当前缓冲区
    currentBuffer_ = buffer1_;
    //当前缓冲区的长度设置为0
    currentEnd_ = 0;
    //把第二个缓冲区给下一个缓冲区
    nextBuffer_ = buffer2_;
    nextEnd_ = 0;
    overBuffer_ = buffer3_;
    overEnd_ = 0;
    //初始化未决全部设置为0
    memset(pending_, 0, ns_ * sizeof(bool));
    //初始化 potential里面的值全部为 POT_HIGH = 10e10 10的10次方
    std::fill(potential, potential + ns_, POT_HIGH);

    //返回开始点的一维数组下标
    int k = toIndex(start_x, start_y);

    //判断准确值
    //使用true新的方式 还是 false:老版navfn启发方式
    if (precise_)
    {
        double dx = start_x - (int)start_x;
        double dy = start_y - (int)start_y;

        //向下取整 //保留小数点后两位
        dx = floorf(dx * 100 + 0.5) / 100;
        dy = floorf(dy * 100 + 0.5) / 100;

        //起始点代价等于  中立代价 *2 * dx * dy  不太明白为啥乘dxdy
        potential[k] = neutral_cost_ * 2 * dx * dy;
        potential[k-1] = neutral_cost_ * 2 * (1-dx) * dy;
        potential[k+nx_] = neutral_cost_ * 2 * dx * (1-dy);
        potential[k+nx_+1] = neutral_cost_ * 2 * (1-dx) * (1-dy);

        //把k附近的点全部压入 current队列
        push_cur(k+2);
        push_cur(k-1);
        push_cur(k+nx_-1);
        push_cur(k+nx_+2);

        push_cur(k-nx_);
        push_cur(k-nx_+1);
        push_cur(k + nx_*2);
        push_cur(k + nx_*2 + 1);

    }else{
        //当前点的代价设为0
        potential[k] = 0;
        //把前后左右全部压入current队列
        push_cur[k+1];
        push_cur[k-1];
        push_cur[k-nx_];
        push_cur[k+nx_];
    }
    
    //最大优先级块大小
    int nwv = 0;
    //放入优先块的信元数量(放进去的栅格的数量)
    int nc = 0;

    //设置开始单元(分明是关闭好吗)//就是目标点
    int startCell = toIndex(end_x, end_y);

    //进行多次循环，除非被打断,或者循环
    int cycle = 0;
    for (; cycle < cycles; cycle++)
    {
        //优先权块为空
        if (currentEnd_ == 0 && nextEnd_ == 0)
            return false;
        
        //统计资料
        nc += currentEnd_;
        //最大优先级块大小 不能小于 当前队列大小
        if (currentEnd_ > nwv)
            nwv = currentEnd_;
        
        //在当前优先级缓冲区上重置未决标志
        //吧当前缓冲区指针 给pb
        int *pb = currentBuffer_;
        int i = currentEnd_;
        //把当前队列的未决全部设置为 false
        while (i-- > 0)
            pending_[*(pb++)] = false;

        //处理当前优先级缓冲区
        pb = currentBuffer_;
        i = currentEnd_;

        while(i-- > 0)
            updateCell(costs, potential, *pb++);
        
        //交换优先级块currentBuffer_ <=> nextBuffer_
        currentEnd_ = nextEnd_;
        nextEnd_ = 0;
        pb = currentBuffer_;
        currentBuffer_ = nextBuffer_;
        nextBuffer_ = pb;

        //查看是否已完成此优先级
        if (currentEnd_ == 0)
        {
            //递增优先级阈值
            threshold_ += priorityIncrement_;
            //将当前设置为溢出块
            currentEnd_ = overEnd_;
            overEnd_ = 0;
            pb = currentBuffer_;
            currentBuffer_ = overBuffer_;
            overBuffer_ = pb;
            //交换over 和当前队列
        }

        if (potential[startCell] < POT_HIGH)
            break;
    }
    if (cycle < cycles)
        return true;
    else
        return false;
}

//关键函数: 根据邻居的值计算单元更新后的潜在值
//从四个网格中的两个最低相邻点进行平面更新计算
//内插值的二次近似
//这里没有边界检查,函数应该很快

//二分之根号二
#define INVSQRT2 0.707106781
inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential, int n){
    //已经遍历过的栅格 自加1
    cells_visited_++;
    //做平面更新
    float c = getCost(costs, n);
    //大于致命代价,不要启发到障碍里面去
    if (c >= lethal_cost_)
        return;
    //pot  =  前后左右最小的potential + 当前的costs 
    float pot = p_calc_->calculatePotential(potential, c, n);
    //现在将受影响的邻居添加到优先级块
    if (pot < potential[n])
    {
        //获得上下左右的 cost值, 但是不明白为啥要乘以二分之根号二
        float le = INVSQRT2 * (float)getCost(costs, n-1);
        float re = INVSQRT2 * (float)getCost(costs, n+1);
        float ue = INVSQRT2 * (float)getCost(costs, n-nx_);
        float de = INVSQRT2 * (float)getCost(costs, n+nx_);

        potential[n] =pot;

        //低成本缓冲块,暂时不清楚干啥的
        //如果当前 pot小于阈值
        if (pot < threshold_)
        {
            //如果当前点的潜力  大于 潜力-1 + cost*根号二/2
            //将其push到next的队列
            if (potential[n-1] > pot + le) push_next(n-1);
            if (potential[n+1] > pot + re) push_next(n+1);
            if (potential[n-nx_] > pot + ue) push_next(n-nx_);
            if (potential[n+nx_] > pot + de) push_next(n+nx_);
        }else{
            //如果当前点的潜力  大于 潜力-1 + cost*根号二/2
            //将其push到over的队列
            if (potential[n-1] > pot + le) push_over(n-1);
            if (potential[n+1] > pot + re) push_over(n+1);
            if (potential[n-nx_] > pot + ue) push_over(n-nx_);
            if (potential[n+nx_] > pot + de) push_over(n+nx_);
        }
    }
}
}

在构造函数中创建了三个优先级队列，析构函数中释放了他们
设置地图大小的创建了一个pending_的bool数组
主要的启发函数calculatePotentials继承自类Expander，函数流程如下

主要流程是首先初始化，将起始点四周的点加入优先级队列，然后进入主循环
主循环先判断当前优先级队列是否为空，如果为空则代表没办法扩展了直接退出return false
记录一下最大优先级队列大小
将当前优先级队列的pending全部设为false
重新获取当前队列的指针和长度
对当前队列每个点调用updatecell函数
调用完成之后，交换当前队列与下一队列
查看当前队列是否为空，空的话代表没有什么可以扩展的了
如果为空则调大临界点的阈值（最开始是致命代价），将关闭列表与当前列表互换，进行下一次扩展（这里表明了无论是否大于致命代价，地杰斯特拉算法总会扩展到目标点）
判断目标点是否被扩展到，如果扩展到了就退出

下面来讲updateCell函数
该函数主要是将输入点四周加入下一次扩展队列，或者是大于临界点加入到关闭列表

地杰斯特拉算法已经讲解完毕，后续会完善流程和每个变量的讲解