移动机器人gazebo仿真(5)—规划算法Ａ*

参考博客：

MoveBace.cpp阅读笔记

贪心算法的一个典型案例——Dijkstra算法:     浅谈路径规划算法之Dijkstra算法

A*:      A*寻路算法   

关于寻路算法的一些思考（1）——A*算法介绍

ROS的全局规划代码介绍（A*）

重要：ROS Navigation的global_planner类继承关系与实现算法

导航实际流程为：

进行全局路径规划，在进行局部路径规划，然后发布速度

全局路径规划在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内的函数：这篇博客这部分写的不错点击打开链接

代码中用了堆，这两篇博客对堆讲的比较详细：点击打开链接  堆相关算法详解与C++编程实现

输入参数 为指向概率地图的指针 其实位置地坐标 目标坐标  一个指向大小为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 obstacle        return;
        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函数得到最终路径