global_planner源码阅读笔记

ROS的navigation stack中nav_core包定义了机器人的全局规划、局部规划和恢复控制三种plugin接口，navigation并给出了相应的plugin。全局路径规划BaseGlobalPlanner的plugin有三种: navfn/NavfnROS，global_planner/GlobalPlanner, carrot_planner/CarrotPlanner. 其中常用的为global_planner/GlobalPlanner，它是navfn/NavfnROS的改进版本，包含Dijskstra和A*算法进行全局路径规划，本文将对其源码包global_planner进行分析。
全局规划器的重要函数主要是初始化函数initialize和路径规划函数makePlan， 在planner_core文件中。

初始化函数initialize

使用全局规划器必须要首先进行初始化，代码及注释如下：

 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
    if (!initialized_) {
        ros::NodeHandle private_nh("~/" + name);
        costmap_ = costmap;
        frame_id_ = frame_id;
        unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
        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)  // 使用二次差值近似的potential
            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);   // A*算法
        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);
        double costmap_pub_freq;
        private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);
        make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
        dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
        dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
                &GlobalPlanner::reconfigureCB, this, _1, _2);
        dsrv_->setCallback(cb);
        initialized_ = true;
    } else
        ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}

规划函数makePlan

路径规划的主要函数是GlobalPlanner::makePlan，简化代码及注释如下：

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_);
    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)) {
        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_){   // 兼容navfn
        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
    clearRobotCell(start, start_x_i, start_y_i);
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
    //make sure to resize the underlying array that Navfn uses
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    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_);
    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
        // 通过代价用path_maker_->getPath得到路径
        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();
}

A*算法

A*算法是启发式规划算法，比Dijskstra算法速度快，其算法思想可参考该博文，文中采用了堆排序对开启列表进行处理，具体代码及注释如下：

// 计算规划代价函数：
//  costs为地图指针，potential为代价数组，cycles为循环次数，代码里值为2*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();     // queue_为启发式搜索到的向量队列：<i , cost>
    int start_i = toIndex(start_x, start_y);   // 起点对应的序号
    queue_.push_back(Index(start_i, 0));
    std::fill(potential, potential + ns_, POT_HIGH);  // 代价数组值全设为∞
    potential[start_i] = 0;
    int goal_i = toIndex(end_x, end_y);   // 终点对应的序号
    int cycle = 0;
    while (queue_.size() > 0 && cycle < cycles) {
        Index top = queue_[0];   // 最小代价点
        std::pop_heap(queue_.begin(), queue_.end(), greater1()); // 将向量front最小的代价Index放到放到最后
        queue_.pop_back();  // 删除最小代价的点
        int i = top.i;
        if (i == goal_i) // 若是目标点则终止搜索，搜索成功
            return true;
        // 将代价最小点i周围点加入搜索队里并更新代价值
        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;
}
// 添加点并更新代价函数
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_)  // 超出范围， ns_为栅格总数
        return;
    if (potential[next_i] < POT_HIGH)  // 已经搜索过
        return;
    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))  // 障碍物
        return;
     // p_calc_->calculatePotential() 采用简单方法计算值为costs[next_i] + neutral_cost_+ prev_potentia  地图代价+单格距离代价(初始化为50)+之前路径代价 为G
    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_;
    float distance = abs(end_x - x) + abs(end_y - y);
    // potential[next_i] + distance * neutral_cost_   为总代价 F = G + H,  H为到目标点距离代价
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));  // 加入搜索向量
    std::push_heap(queue_.begin(), queue_.end(), greater1()); // 对加入的再进行堆排序， 把最小代价点放到front队头queue_[0]
}