Navigation（六）global_planner源码解析之planner_core

 

一、入口

找入口就找main函数，定位到plan_node.cpp这个文件，可以看到main函数写了节点名为global_planner：

ros::init(argc, argv, "global_planner")

 

继续读，后面分别声明了costmap_2d::Costmap2DROS的对象，以及global_planner::PlannerWithCostmap的对象：

    costmap_2d::Costmap2DROS lcr("costmap", buffer);
    global_planner::PlannerWithCostmap pppp("planner", &lcr);

因此我们要分别去看这两个类的定义。本文主要讲global_planner，所以暂且忽略costmap_2d::Costmap2DROS。

 

二、声明

跳转到global_planner::PlannerWithCostmap，看到PlannerWithCostmap类继承了GlobalPlanner类（该类是nav_core中写的基类）：

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_;

 

三、Planner_core

承接上面所讲的声明，分别对几个成员函数进行解析。

函数最先进入的是构造函数PlannerWithCostmap(string name, Costmap2DROS* cmap)，所以进入该函数的定义看一下：

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);
}

 

这个类继承了GlobalPlanner()类，所以要先去看GlobalPlanner()类的构造函数：

GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
        costmap_(NULL), initialized_(false), allow_unknown_(true) {
    //initialize the planner
    initialize(name, costmap, frame_id);
}

 

其中，调用了初始化函数initialize（），其中选取了规划器的各个参数：

        private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
        if(!old_navfn_behavior_)
            convert_offset_ = 0.5;
        else
            convert_offset_ = 0.0;

        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);  //p_calc_实例：计算“一个点”的可行性

        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);  //planner_实例：计算“所有”的可行点potential array

        bool use_grid_path;
        private_nh.param("use_grid_path", use_grid_path, false);
        if (use_grid_path)
            path_maker_ = new GridPath(p_calc_);//栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点
        else
            path_maker_ = new GradientPath(p_calc_);  //梯度路径，从周围八个栅格中找到下降梯度最大的点

 

接着用了滤波器，然后定义了两个发布器，配置了一些参数：

        orientation_filter_ = new OrientationFilter();  //
        //定义两个发布器
        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
        potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);

        private_nh.param("allow_unknown", allow_unknown_, true);
        planner_->setHasUnknown(allow_unknown_);
        private_nh.param("planner_window_x", planner_window_x_, 0.0);
        private_nh.param("planner_window_y", planner_window_y_, 0.0);
        private_nh.param("default_tolerance", default_tolerance_, 0.0);
        private_nh.param("publish_scale", publish_scale_, 100);

 

发布一个make_plan的Service，make_plan_srv_：

        make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

调用了回调函数GlobalPlanner::makePlanService，跳转过去看一下，其实也没什么，主要是调用了makeplan（），这个在下面会再细讲：

bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
    makePlan(req.start, req.goal, resp.plan.poses);

    resp.plan.header.stamp = ros::Time::now();
    resp.plan.header.frame_id = frame_id_;

    return true;
}

 

接着加载动态调参：

void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
    planner_->setLethalCost(config.lethal_cost);
    path_maker_->setLethalCost(config.lethal_cost);
    planner_->setNeutralCost(config.neutral_cost);
    planner_->setFactor(config.cost_factor);
    publish_potential_ = config.publish_potential;
    orientation_filter_->setMode(config.orientation_mode);
    orientation_filter_->setWindowSize(config.orientation_window_size);
}

 

 

然后回来看PlannerWithCostmap(string name, Costmap2DROS* cmap)的构造函数：

里面调用了回调函数PlannerWithCostmap::makePlanService和PlannerWithCostmap::poseCallback，我们分别看一下：

（一）首先是PlannerWithCostmap::makePlanService的定义：

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;
}

 

其中，调用了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);//调用下面的makeplan
}

 

上面这一步，相当于调用了另外一个makePlan（）函数，跳转过去，传入的参数多了一个容忍值：

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) 

 

首先把原来的规划清除，说白了就是清空一下vector：

    plan.clear();

 

然后判断目标点、起点是不是在全局坐标系下：

    if (goal.header.frame_id != global_frame) {
        ROS_ERROR(
                "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), goal.header.frame_id.c_str());
        return false;
    }

    if (start.header.frame_id != global_frame) {
        ROS_ERROR(
                "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", global_frame.c_str(), start.header.frame_id.c_str());
        return false;
    }

 

把起始点坐标从世界地图转为map坐标系：

    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;
    double start_x, start_y, goal_x, goal_y;
    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i))。。。

我们来看一下worldToMap函数，其实就是通过坐标和地图的origin原点相减 然后除以分辨率得出：

bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
  if (wx < origin_x_ || wy < origin_y_)
    return false;

  mx = (int)((wx - origin_x_) / resolution_);
  my = (int)((wy - origin_y_) / resolution_);


  if (mx < size_x_ && my < size_y_)
    return true;

  return false;
}

 

ok，返回makePlan（）函数继续看：

清除costmap中的起始单元格，因为它在的地方肯定不是障碍：

    clearRobotCell(start, start_x_i, start_y_i);

我们来看一下这个函数的定义，主要是把起始点周围的costmap栅格设置为free：

void GlobalPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }

    //把关联的cost设置为free
    costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
}

其中，setcost（）函数定义如下，主要是把x y坐标下的costmap置为costmap_2d::FREE_SPACE：

void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
  costmap_[getIndex(mx, my)] = cost;
}

其中，getIndex(mx, my)函数就是把x y坐标转化为索引值。

 

ok，返回makeplan（）函数继续看：

分配空间，和costmap一样大，其中这几个变量有什么用？？保持疑问，potential_array_二维数组，存储potential可行点。

    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    potential_array_ = new float[nx * ny]; 

 

把costmap的四周(边界)变为LETHAL_OBSTACLE：

  outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE); 

来看下定义，主要是设置规划地图的边框，传入的costmap_->getCharMap()的返回值就是costmap_，这个函数将

costmap的四周(边界)变为LETHAL_OBSTACLE：

void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
    unsigned char* pc = costarr;
    for (int i = 0; i < nx; i++)
        *pc++ = value;
    pc = costarr + (ny - 1) * nx;
    for (int i = 0; i < nx; i++)
        *pc++ = value;
    pc = costarr;
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;
    pc = costarr + nx - 1;
    for (int i = 0; i < ny; i++, pc += nx)
        *pc = value;
}

 

ok，返回makeplan（）函数继续看：

主要步骤一、计算potential_array，寻找所有可行点：

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

其中，调用了calculatePotentials函数计算代价，这个函数有两种方法：A*和Dij，由use_dijkstra参数决定，这两个方法的类都继承了Expander。

 

然后判断使用的是navfn包还是，old_navfn_behavior_这个参数默认为false，用来兼容navfn；然后调用clearEndpoint（）函数，该函数把终点周围的点更新了一下：

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

其中，clearEndpoint函数如下，首先调用了序列号转换函数 toIndex(gx, gy)，costs[n]+neutral_cost_是什么？？

        void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s){
            int startCell = toIndex(gx, gy);
            for(int i=-s;i<=s;i++){
            for(int j=-s;j<=s;j++){
                int n = startCell+i+nx_*j;
                if(potential[n]<POT_HIGH)
                    continue;
                float c = costs[n]+neutral_cost_;
                float pot = p_calc_->calculatePotential(potential, c, n);
                potential[n] = pot;
            }
            }
        }

 

然后发布可行点：

publishPotential(potential_array_)

调用的是本文件中定义的函数：

void GlobalPlanner::publishPotential(float* potential)
{
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
    double resolution = costmap_->getResolution();
    nav_msgs::OccupancyGrid grid;
    // Publish Whole Grid
    grid.header.frame_id = frame_id_;
    grid.header.stamp = ros::Time::now();
    grid.info.resolution = resolution;

    grid.info.width = nx;
    grid.info.height = ny;

    double wx, wy;
    costmap_->mapToWorld(0, 0, wx, wy);
    grid.info.origin.position.x = wx - resolution / 2;
    grid.info.origin.position.y = wy - resolution / 2;
    grid.info.origin.position.z = 0.0;
    grid.info.origin.orientation.w = 1.0;

    grid.data.resize(nx * ny);

    float max = 0.0;
    for (unsigned int i = 0; i < grid.data.size(); i++) {
        float potential = potential_array_[i];
        if (potential < POT_HIGH) {
            if (potential > max) {
                max = potential;
            }
        }
    }

    for (unsigned int i = 0; i < grid.data.size(); i++) {
        if (potential_array_[i] >= POT_HIGH) {
            grid.data[i] = -1;
        } else
            grid.data[i] = potential_array_[i] * publish_scale_ / max;
    }
    potential_pub_.publish(grid);
}

 

主要步骤二：从所有可行点中获取路径plan，调用path_maker_->getPath()实例，从所有的可行点中获取路径plan。调用的是：Traceback::virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) = 0;getPath的方法有两种（GradientPath、GridPath），由use_grid_path参数决定：class GradientPath : public Traceback、class GridPath : public Traceback如果成功找到一个路径，从potential这个数组中得到路径，getPlanFromPotential。potential这个数组是潜力，距离起始栅格越远的栅格潜力越高，距离障碍物越近的栅格潜力越低。每个栅格可以根据附近的栅格求出其梯度信息，且梯度方向指向起始栅格或远离障碍物栅格。

        if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {  2             //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.");
        }

其中，getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan) 调用了path_maker_ -> getPath()，路径存储在plan中：

bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
                                      const geometry_msgs::PoseStamped& goal,
                                       std::vector<geometry_msgs::PoseStamped>& plan) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }

    std::string global_frame = frame_id_;

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

    std::vector<std::pair<float, float> > path;

    ////use_grid_path   调用path_maker_->getPath()实例，提取路径
    if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {
        ROS_ERROR("NO PATH!");
        return false;
    }

    ros::Time plan_time = ros::Time::now();
    for (int i = path.size() -1; i>=0; i--) {
        std::pair<float, float> point = path[i];
        //convert the plan to world coordinates
        double world_x, world_y;
        mapToWorld(point.first, point.second, world_x, world_y);

        geometry_msgs::PoseStamped pose;
        pose.header.stamp = plan_time;
        pose.header.frame_id = global_frame;
        pose.pose.position.x = world_x;
        pose.pose.position.y = world_y;
        pose.pose.position.z = 0.0;
        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = 0.0;
        pose.pose.orientation.w = 1.0;
        plan.push_back(pose);
    }
    if(old_navfn_behavior_){
            plan.push_back(goal);
    }
    return !plan.empty();
}

 

然后，添加方向信息 ，发布可视化路径：

    orientation_filter_->processPath(start, plan);

    publishPlan(plan);//发布可视化路径
    delete potential_array_;
    return !plan.empty();

其中，publishPlan(plan)如下：

void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }

    //create a message for the plan
    nav_msgs::Path gui_path;
    gui_path.poses.resize(path.size());

    gui_path.header.frame_id = frame_id_;
    gui_path.header.stamp = ros::Time::now();

    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    for (unsigned int i = 0; i < path.size(); i++) {
        gui_path.poses[i] = path[i];
    }

    plan_pub_.publish(gui_path);
}

 

（二）终于到了第二个回调函数PlannerWithCostmap::poseCallback：

 其实功能和上面的回调函数一样，都是调用了makeplan（）函数，这里就不再赘述了。