在navugation中添加自己的算法（2）

1.在哪里修改全局路径算法

传统的Ａ＊算法虽然也很好用，但是我们应该致力于不停地寻找更优化的路径算法，使机器人的路径规划速度更快，行走的更流畅，行走的距离和时间更短

全局路径的规划是在planner_core.cpp中，首先应该对该文件熟悉

（１）初始化我们添加的算法

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) 
｛
　...
        if (use_dijkstra)
        {
            ROS_INFO("------------------dijkstra start!");
            DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
            if(!old_navfn_behavior_)
                de->setPreciseStart(true);
            planner_ = de;
        }
        else
        {
            //planner_ = new AStarExpansion(p_calc_, cx, cy); 
            //planner_ = new JPSPlusExpansion(p_calc_, cx, cy); 
            planner_ = new JPSExpansion(p_calc_, cx, cy); 
        }
...
｝

在初始化函数中，根据配置文件的参数，选择dijkstra和Ａ*算法，我们可以添加自己的cpp文件

这里可以偷懒一下。。。直接拷贝astar.h和astar.cpp文件，然后将ｃｌａｓｓ修改成自己，在planner_core.cpp中加上我们的头文件。

其他的参数没什么好介绍的，都是不重要的参数，直接默认就行了

（２）调用我们的算法

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector& plan) 
｛
...
    potential_array_ = new float[nx * ny];
    outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);

    bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,nx * ny * 2, potential_array_,nx,ny);

...

｝

这里是我们算法真正开始调用的地方，也是最重要的地方

calculatePotentials函数有几个参数：

costmap_->getCharMap()　：　地图的代价地图，包括静态、膨胀层和雷达临时障碍物的重叠地图，具体可以参考我上一篇文章，有对这三种ｌａｙｅｒ介绍。

start_x,start_y : 起点的位置，float类型

goal_x,goal_y:　终点的位置，float类型

nx * ny * 2,地图的长　＊　宽　＊　２,两倍地图，在Ａ＊中有用

potential_array_　大小为nx * ny 的一位数组，在算法中会初始化为一串无穷大的数组，计算过一个节点就将对应的位置数据赋值，所以如果无穷大就说明这点没有计算过，如果不是则是计算过的节点，相当于Ａ＊中的closeList，后面重新加载路径也用得上

nx,ny　：　地图的长宽

（３）算法模板

对比Astar修改对应的参数和类名

#ifndef _AOC_H
#define _AOC_H

#include 
#include 
#include 
#include 
#include "ant.h"

namespace global_planner {
class Index_Aoc {
    public:
        Index_Aoc(int a, float b) {
            i = a;
            cost = b;
        }
        int i;
        float cost;
};

struct greater1_Aoc {
        bool operator()(const Index_Aoc& a, const Index_Aoc& b) const {
            return a.cost > b.cost;
        }
};

class AocExpansion : public Expander {
    public:
        AocExpansion(PotentialCalculator* p_calc, int nx, int ny);
        bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles,
                                float* potential, int m_height, int m_width);
    private:
        
        clock_t startTime_astar,endTime_astar;
};

} //end namespace global_planner
#endif

 

// 计算规划代价函数：
//  costs为地图指针，potential为代价数组，cycles为循环次数，代码里值为2*nx*ny为地图栅格数的两倍
#include
#include
#include 

namespace global_planner {

AocExpansion::AocExpansion(PotentialCalculator* p_calc, int xs, int ys) :
        Expander(p_calc, xs, ys) {
             ROS_WARN("init AOC!");
}

bool AocExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
                                        int cycles, float* potential, int m_height, int m_width) {
    

    return false;
}

} //end namespace global_planner

２．算法中的重要部分

当调用算法中的calculatePotentials函数

重点介绍一下unsigned char* costs,

这个一位数组将整个代价地图包含在内，但是其是一列一列计算的，相当于是（０，０）是第一个，（１，０）是第二个，（０，１）是第ｎｘ　＋　１个，这个我们要特别注意，不然规划不了路径的

未完待续。。。