ROS-Navigation学习(4)——global_planner学习

框架

global_planner规划器的整体框架如下
参考：https://zhuanlan.zhihu.com/p/46212318
A*算法的原理讲解，A*算法
这个是自己看到的网上很多的一篇。讲的也的确很棒。

Dijkstra算法原理：Dijkstra

由于之前A*和Dijkstra算法基于栅格的路线不会比较光滑，但是在navigation中有基于梯度势场的，这样会使得其比较光滑。所以想对比一下GridPath::getPath 和 GradientPath::getPath这两个程序中算法区别（之前接触较少）

GridPath

首先上代码，行数很少。。

bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
    std::pair<float, float> current;
    current.first = end_x;
    current.second = end_y;

    int start_index = getIndex(start_x, start_y);

    path.push_back(current);
    int c = 0;
    int ns = xs_ * ys_;
    
    while (getIndex(current.first, current.second) != start_index) {
        float min_val = 1e10;
        int min_x = 0, min_y = 0;
        for (int xd = -1; xd <= 1; xd++) {
            for (int yd = -1; yd <= 1; yd++) {
                if (xd == 0 && yd == 0)
                    continue;
                int x = current.first + xd, y = current.second + yd;
                int index = getIndex(x, y);
                if (potential[index] < min_val) {
                    min_val = potential[index];
                    min_x = x;
                    min_y = y;
                }
            }
        }
        if (min_x == 0 && min_y == 0)
            return false;
        current.first = min_x;
        current.second = min_y;
        path.push_back(current);
        
        if(c++>ns*4){
            return false;
        }

    }
    return true;
}

代码比较简单，主要是两个for循环那一块的内容。对应就是寻找当前点周围8个点位中potential最小的那个，作为该点位的前一个需要经过的点位。注意这个寻找是从目标点到起始点逐个寻找，并且逐个加入到向量最后，这样可能会在移动的时候更方便点位的提取。也是因此，while里的判断条件是:

//不等于起始点的索引，就继续寻找。
while (getIndex(current.first, current.second) != start_index) 

最后的一个判断是判断是否溢出的。

if(c++>ns*4){
            return false;
        }

Gradient_path

这个文件并没找到相关博客，是自己理解的，可能存在问题。若有不同理解可以联系

这个基于梯度的利用的是插补法使路径变得光滑，和ARA*那两种利用预先的光滑路径基元进行拟合不同。原理就是，在当前点和下一个点位的dx和dy方向上加上插值。
有三个主要的函数 setsize, getpath, gradcell.
设置大小的函数就是申请一个指定大小的内存。比较简单

void GradientPath::setSize(int xs, int ys) {
    Traceback::setSize(xs, ys);
    if (gradx_)
        delete[] gradx_;
    if (grady_)
        delete[] grady_;
    
    //申请gradx_和gray_;
    gradx_ = new float[xs * ys];
    grady_ = new float[xs * ys];
}

gradcell这个相对重要一些，是计算梯度以及切向信息的。

float GradientPath::gradCell(float* potential, int n) {
    //如果这个梯度是增加的，就直接返回。 实际上在getPtah函数调用这个函数的时候
    //并没有调用返回值，而是对gradx_和grady_进行使用。增加的梯度相当于向这个方向
    //移动会使得potential增加。但是我们应该是让它减小。
    if (gradx_[n] + grady_[n] > 0.0)    // check this cell
        return 1.0;
	//判断当前的n是不是越界，之前是否已经检测过了或者是超出了
    if (n < xs_ || n > xs_ * ys_ - xs_)    // would be out of bounds
        return 0.0;
    float cv = potential[n];
    float dx = 0.0;
    float dy = 0.0;
	
	//检测障碍物，如果当前的potential大于阈值，就判定有障碍，向小于阈值的方向移动。
	//因此dx 和dy就给定在没有障碍方向的一个固定值 lethal_cost
    // check for in an	 obstacle
    if (cv >= POT_HIGH) {
        if (potential[n - 1] < POT_HIGH)
            dx = -lethal_cost_;
        else if (potential[n + 1] < POT_HIGH)
            dx = lethal_cost_;

        if (potential[n - xs_] < POT_HIGH)
            dy = -lethal_cost_;
        else if (potential[n + xs_] < POT_HIGH)
            dy = lethal_cost_;
    }
	
	//如果没有遇到障碍，就计算dx和dy。
    else                // not in an obstacle
    {
        // dx calc, average to sides
        if (potential[n - 1] < POT_HIGH)
            dx += potential[n - 1] - cv;
        if (potential[n + 1] < POT_HIGH)
            dx += cv - potential[n + 1];

        // dy calc, average to sides
        if (potential[n - xs_] < POT_HIGH)
            dy += potential[n - xs_] - cv;
        if (potential[n + xs_] < POT_HIGH)
            dy += cv - potential[n + xs_];
    }

    // normalize  对dx和dy进行归一，并且赋值给gradx_和grady_
    float norm = hypot(dx, dy);
    if (norm > 0) {
        norm = 1.0 / norm;
        gradx_[n] = norm * dx;
        grady_[n] = norm * dy;
    }
    return norm;
}

这里有个细节（个人感觉），是在计算dx和dy时，都是先判断n-1的potential.
然后就是主要的getpath函数
首先是一些预处理，设置一些初始值等

 std::pair<float, float> current;
    int stc = getIndex(goal_x, goal_y);

    // set up offset
    float dx = goal_x - (int)goal_x;
    float dy = goal_y - (int)goal_y;
    int ns = xs_ * ys_;
    memset(gradx_, 0, ns * sizeof(float));
    memset(grady_, 0, ns * sizeof(float));

    int c = 0;

最主要的是while循环里的一组 if else
最开始是判断这个点位距离目标点是不是已经足够近，如果足够近，就将目标点压入path,然后返回true。（这里的目标点实际是起始点，因为vector的结构，可以有多种数据结构 vector讲解链接）其中nx和ny是路径中下一位置的点位，等待压入path.

double nx = stc % xs_ + dx, ny = stc / xs_ + dy;

if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) {
    current.first = start_x;
    current.second = start_y;
    path.push_back(current);
    return true;
}

接下来判断是否越界

if (stc < xs_ || stc > xs_ * ys_ - xs_) // would be out of bounds
        {
            printf("[PathCalc] Out of bounds\n");
            return false;
        }

然后将当前的nx和ny压入path，这个循环是每次开始的时候进行最后数据的存入，再开始下一步的操作，而不是操作后再进行存入。

        path.push_back(current);

        bool oscillation_detected = false;
        int npath = path.size();
        if (npath > 2 && path[npath - 1].first == path[npath - 3].first
                && path[npath - 1].second == path[npath - 3].second) {
            ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
            oscillation_detected = true;
        }

        int stcnx = stc + xs_;
        int stcpx = stc - xs_;

接下来的一组if else是最关键的gradient相关数据处理。
首先还是检测该点周围八个点的potential值，如果有一个超出阈值，就检测这八个值里最小的，并且不进行gradient,而是采用grid的。因此dx=dy=0。如果都超过了阈值，返回0;

        int stcnx = stc + xs_;
        int stcpx = stc - xs_;

        // check for potentials at eight positions near cell
        if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH
                || potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH
                || potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH
                || oscillation_detected) {
            ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size());
            // check eight neighbors to find the lowest
            int minc = stc;
            int minp = potential[stc];
            int st = stcpx - 1;
            //检测stcpx一行
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            st++;
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            st++;
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            //开始新一行检测
            st = stc - 1;
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            st = stc + 1;
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            //新一行检测
            st = stcnx - 1;
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            st++;
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            st++;
            if (potential[st] < minp) {
                minp = potential[st];
                minc = st;
            }
            stc = minc;
            dx = 0;
            dy = 0;

            //ROS_DEBUG("[Path] Pot: %0.1f  pos: %0.1f,%0.1f",
            //    potential[stc], path[npath-1].first, path[npath-1].second);

            if (potential[stc] >= POT_HIGH) {
                ROS_DEBUG("[PathCalc] No path found, high potential");
                //savemap("navfn_highpot");
                return 0;
            }
        }

如果八个方位都没有超过阈值，表示周围具有很好的梯度。此时采用梯度插补的方法计算dx dy.
首先是计算四个点的梯度方向，不过不太清楚为什么是4个不是8个。

 gradCell(potential, stc);
 gradCell(potential, stc + 1);
 gradCell(potential, stcnx);
 gradCell(potential, stcnx + 1);

接下来就是梯度插补值的计算

float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1];
float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1];
float x = (1.0 - dy) * x1 + dy * x2; // interpolated x
float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1];
float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1];
float y = (1.0 - dy) * y1 + dy * y2; // interpolated y

  // move in the right direction
float ss = pathStep_ / hypot(x, y);
dx += x * ss;
dy += y * ss;

最后检测dx和dy是否出界，如果大于1，就相当于是在下一个格子，这样还是会不光滑，跟grid一样。因此不能让其大于1 （个人理解）

// check for overflow
            if (dx > 1.0) {
                stc++;
                dx -= 1.0;
            }
            if (dx < -1.0) {
                stc--;
                dx += 1.0;
            }
            if (dy > 1.0) {
                stc += xs_;
                dy -= 1.0;
            }
            if (dy < -1.0) {
                stc -= xs_;
                dy += 1.0;
            }