參考部落格：

導航實際流程為：

進行全域性路徑規劃，在進行區域性路徑規劃，然後釋出速度

全域性路徑規劃在makePlan函式中，該函式中呼叫了planner_的makePlan和empty介面。
planner_為繼承於BaseGlobalPlanner的例項，由pluginlib通過具體類的名字進行裝載。
之後，呼叫tc_的setPlan介面，對區域性路徑規劃器進行全域性路徑設定，然後，呼叫tc_的isReached介面進行判斷，然後呼叫tc_的computeVelocityCommands介面，進行速度計算，然後進行速度下發。

tc_為繼承於BaseLocalPlanner的例項，也是由pluginlinb通過具體類的名字進行裝載。

下面帶來兩個問題，planner_怎麼進行路徑規劃，以及tc_如何計算速度。
planner_在初始化時候，被塞入了planner_costmap_ros_
tc_在初始化時，被塞入了controller_costmap_ros_



在global planner的包中，註冊了外掛：global planner::GlobalPlanner

程式碼閱讀：

global_planne

1、plan_node.cpp  

plan_node.cpp是全域性規劃程式碼的入口(程式碼註釋都是自己理解然後新增，也許會有錯誤)

#include <global_planner/planner_core.h>
#include <navfn/MakeNavPlan.h>
#include <boost/shared_ptr.hpp>
#include <costmap_2d/costmap_2d_ros.h>

namespace cm = costmap_2d;
namespace rm = geometry_msgs;

using std::vector;
using rm::PoseStamped;
using std::string;
using cm::Costmap2D;
using cm::Costmap2DROS;

namespace global_planner {

class PlannerWithCostmap : public GlobalPlanner {
    public:
        PlannerWithCostmap(string name, Costmap2DROS* cmap);
        bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp);

    private:
        void poseCallback(const rm::PoseStamped::ConstPtr& goal);
        Costmap2DROS* cmap_;
        ros::ServiceServer make_plan_service_;
        ros::Subscriber pose_sub_;
};
//Publish a path for visualization purposes
bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {
    vector<PoseStamped> path;

    req.start.header.frame_id = "/map";
    req.goal.header.frame_id = "/map";
    bool success = makePlan
 
(req.start, req.goal, path);
    resp.plan_found = success;
    if (success) {
        resp.path = path;
    }

    return true;
}

void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
    tf::Stamped<tf::Pose> global_pose;
    cmap_->getRobotPose(global_pose);//獲取機器人起始位姿
    vector<PoseStamped> path;
    rm::PoseStamped start;
    start.header.stamp = global_pose.stamp_;
    start.header.frame_id = global_pose.frame_id_;
    start.pose.position.x = global_pose.getOrigin().x();
    start.pose.position.y = global_pose.getOrigin().y();
    start.pose.position.z = global_pose.getOrigin().z();
    start.pose.orientation.x = global_pose.getRotation().x();
    start.pose.orientation.y = global_pose.getRotation().y();
    start.pose.orientation.z = global_pose.getRotation().z();
    start.pose.orientation.w = global_pose.getRotation().w();
    makePlan
 
(start, *goal, path);//路徑規劃
}

PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :
        GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) {
    ros::NodeHandle private_nh("~");
    cmap_ = cmap;
    make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
    pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}

} // namespace

int main(int argc, char** argv) {
    ros::init(argc, argv, "global_planner");
//設定tf監聽時間間隔
    tf::TransformListener tf(ros::Duration(10));
//costmap_2d::Costmap2D 類是儲存和訪問2D代價地圖的的基本資料結構，下面程式碼作用是初始化
    costmap_2d::Costmap2DROS lcr("costmap", tf);
//兩個執行緒：1、提供planservice   2、訂閱goal，當得到goal則啟動makeplan
    global_planner::PlannerWithCostmap pppp("planner", &lcr);

    ros::spin();
    return 0;
}

接下來分析makeplan函式

2、makeplan

GlobalPlanner::makePlan類的使用介面有多種，例如：

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           std::vector<geometry_msgs::PoseStamped>& plan) {
    return makePlan(start, goal, default_tolerance_, plan);
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){.....}

但最終程式的主體是：

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
    boost::mutex::scoped_lock lock(mutex_);//給執行緒加鎖直到被銷燬
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }

    //clear the plan, just in case
    plan.clear();

    ros::NodeHandle n;
    std::string global_frame = frame_id_;

    //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
    if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
        ROS_ERROR(
                "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
        return false;
    }

    if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
        ROS_ERROR(
                "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
        return false;
    }
//記錄開始位姿
    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

    unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;//map
    double start_x, start_y, goal_x, goal_y;
//下面將世界座標系下的start和goal轉化為map形式
    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
        ROS_WARN(
                "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
        return false;
    }
    if(old_navfn_behavior_){
        start_x = start_x_i;
        start_y = start_y_i;
    }else{
        worldToMap(wx, wy, start_x, start_y);
    }

    wx = goal.pose.position.x;
    wy = goal.pose.position.y;

    if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
        ROS_WARN_THROTTLE(1.0,
                "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
        return false;
    }
    if(old_navfn_behavior_){
        goal_x = goal_x_i;
        goal_y = goal_y_i;
    }else{
        worldToMap(wx, wy, goal_x, goal_y);
    }

    //clear the starting cell within the costmap because we know it can't be an obstacle
    tf::Stamped<tf::Pose> start_pose;
    tf::poseStampedMsgToTF(start, start_pose);//map下資訊轉化為tf類的資料
    clearRobotCell(start_pose, start_x_i, start_y_i);//清除開始點，因為開始位置不可能是障礙
//計算costmap的xsize和ysize，賦值給nx ，ny
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();

    //make sure to resize the underlying array that Navfn uses，（分配空間，大小和costmap一樣大）
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    potential_array_ = new float[nx * ny];
//呼叫以下函式將costmap的四個邊的全部cell都設定為LETHAL_OBSTACLE(佔用)
    outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
//計算potential
  bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                    nx * ny * 2, potential_array_);

    if(!old_navfn_behavior_)
        planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
    if(publish_potential_)
        publishPotential(potential_array_);

    if (found_legal) {
        //extract the plan
        if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
            //make sure the goal we push on has the same timestamp as the rest of the plan
            geometry_msgs::PoseStamped goal_copy = goal;
            goal_copy.header.stamp = ros::Time::now();
            plan.push_back(goal_copy);
        } else {
            ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
        }
    }else{
        ROS_ERROR("Failed to get a plan.");
    }

    // add orientations if needed
    orientation_filter_->processPath(start, plan);
    
    //publish the plan for visualization purposes
    publishPlan(plan);
    delete potential_array_;
    return !plan.empty();
}

值得注意的是，在GlobalPlanner::initialize()這個初始化函式中有一段程式碼，決定了使用A*還是D*亦或是其他演算法計算：

  bool use_quadratic;
        private_nh.param("use_quadratic", use_quadratic, true);
        if (use_quadratic)
            p_calc_ = new QuadraticCalculator(cx, cy);
        else
            p_calc_ = new PotentialCalculator(cx, cy);

        bool use_dijkstra;
        private_nh.param("use_dijkstra", use_dijkstra, true);
        if (use_dijkstra)
        {
            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);//決定使用的演算法

        bool use_grid_path;
        private_nh.param("use_grid_path", use_grid_path, false);
        if (use_grid_path)
            path_maker_ = new GridPath(p_calc_);
        else
            path_maker_ = new GradientPath(p_calc_);

從makeplan程式碼中分析，最關鍵的語句有兩句：

1、計算potential

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

這裡的planner_的定義由GlobalPlanner::initialize()中的引數決定（程式見上）

2、提取plan

if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
            //make sure the goal we push on has the same timestamp as the rest of the plan
            geometry_msgs::PoseStamped goal_copy = goal;
            goal_copy.header.stamp = ros::Time::now();
            plan.push_back(goal_copy);
        }

astar.cpp檔案

計算potential時，假設 引數檔案中 use_dijkstra = fause  ，那麼使用的就是astar演算法，即

planner_ = new AStarExpansion(p_calc_, cx, cy)

因此首先需要分析astar.cpp內的函式：這篇部落格這部分寫的不錯點選開啟連結

輸入引數 為指向概率地圖的指標 其實位置地座標 目標座標  一個指向大小為nx*ny的陣列

bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
                                        int cycles, float* potential) {
    queue_.clear();
    int start_i = toIndex(start_x, start_y);
    queue_.push_back(Index(start_i, 0));//push the start point into OPEN  queue_

    std::fill(potential, potential + ns_, POT_HIGH); //initial all the potential as very large value 1e10
    potential[start_i] = 0;//set start_i為0

    int goal_i = toIndex(end_x, end_y);
    int cycle = 0;

   while (queue_.size() > 0 && cycle < cycles) {
        Index top = queue_[0];//get the Index with lowest cost (set to current)
        std::pop_heap(queue_.begin(), queue_.end(), greater1());//build the heap sort
        queue_.pop_back();//remove the Index with mini cost (remove from OPEN)

        int i = top.i;//target node the Index's i from (i,cost)
        if (i == goal_i)
            return true;
//for each neighbour node (*) of the current node(0),
        //  + * +          i-nx
        //  * 0 *      i-1, 0 , i+1
        //  + * +          i+nx
        add(costs, potential, potential[i], i + 1, end_x, end_y);
        add(costs, potential, potential[i], i - 1, end_x, end_y);
        add(costs, potential, potential[i], i + nx_, end_x, end_y);
        add(costs, potential, potential[i], i - nx_, end_x, end_y);

        cycle++;
    }

    return false;
} 

//接下來add函式的定義

/*f(n)=g(n)+h(n)
其中， f(n) 是從初始狀態經由狀態n到目標狀態的代價估計，g(n) 是在狀態空間中從初始狀態到狀態n的實際代價，
h(n) 是從狀態n到目標狀態的最佳路徑的估計代價。
（對於路徑搜尋問題，狀態就是圖中的節點，代價就是距離）*/

void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
                         int end_y) {
    if (next_i < 0 || next_i >= ns_)
        return;

    if (potential[next_i] < POT_HIGH)//it means the potential cell has been explored
        return;

    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))//it means this cell is obstaclereturn;
        return;
//計算next_i的potential值，calculatePotential函式返回next_i周圍的節點到next_i的最小值
    potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
    int x = next_i % nx_, y = next_i / nx_;//x mean column ，y means row
    float distance = abs(end_x - x) + abs(end_y - y);//calculate h（n）
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
    std::push_heap(queue_.begin(), queue_.end(), greater1());
}
} //end namespace global_planner

3、A*演算法程式碼總結：

1、bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential) 

輸入引數：* costs                               即：   costmap_->getCharMap()

                        start_x,start_y               起始點

                        end_x,end_y                  目標點

                        cycles                               即：nx * ny * 2

                        *potential                       用於儲存代價,陣列大小為nx * ny 

首先  start_x,start_y,end_x,end_y轉化為 start_i,end_i;

將start_i加入到open 中  

        queue_.push_back(Index(start_i, 0));   //Index包括  i  和 cost  ，程式碼將cost清零

進入迴圈,迴圈條件：堆的size大於0 且 迴圈次數小於 2*nx*ny

              迴圈中先定義   Index top = queue_[0];    即取最小堆的根，包含序號i，代價 cost

             然後通過    std::pop_heap(queue_.begin(), queue_.end(), greater1()); queue_.pop_back(); 將樹根放到末端並刪除

            取i=top.i, 計算代價地圖中要到達目標點 該點(i)的鄰點 所消耗代價的最小值

接下來分析如何計算：

2、void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,

                         int end_y)

代價計算：f(n)=g(n)+h(n)

呼叫形式是：        add(costs, potential, potential[i], i + 1, end_x, end_y);

        add(costs, potential, potential[i], i - 1, end_x, end_y);
        add(costs, potential, potential[i], i + nx_, end_x, end_y);
        add(costs, potential, potential[i], i - nx_, end_x, end_y);

輸入引數：  costs地圖    potential陣列   當前i點的potential值   鄰點的代號   目標點的行、列資訊

先計算start_i到 鄰點(i+1,i-1,i+nx,i-nx) 的最小代價g(n)，使用函式：

potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);

然後計算  鄰點(i+1,i-1,i+nx,i-nx) 到目標點的估值代價h (n)，與前面的最小代價g(n)相加,並放到樹根

 queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
    std::push_heap(queue_.begin(), queue_.end(), greater1());

3、 calculatePotential函式

 virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){
            if(prev_potential < 0){
                // get min of neighbors
                float min_h = std::min( potential[n - 1], potential[n + 1] ),
                      min_v = std::min( potential[n - nx_], potential[n + nx_]);
                prev_potential = std::min(min_h, min_v);
            }
            
            return prev_potential + cost;
        }

4、使用getPlanFromPotential函式得到最終路徑