ROS运动规划学习五---global_planner

---

前言

本节将学习ROS中的全局规划期global_planner功能包，前面已经介绍了nav_core、move_base、costmap_2d功能包。全局规划大都基于静态地图进行规划，产生路径点，然后给到局部规划器进行局部规划，ROS中常见的全局规划器功能包有navfn、global_planner(Dijkstra、A*）、asr_navfn、Movelt!(常用于机械臂)等.

---

一、global_planner功能包结构

plan_node文件是全局规划的入口；planner_core是global_planner的核心，进行初始化，调用A*或者Dijkstra进行全局规划；quadratic_calculator（二次逼近方式，常用） 和potential_calculator（直接返回当前点代价最小值，累加前面的代价值）在生成搜索路径中用到；搜索到路径后使用回溯grid_path（栅格路径）、gradient_path（梯度路径）获得最终路径；之后orientation_filter进行方向滤波。
栅格路径：从终点开始找上下左右四个点中最小的栅格直到起点
梯度路径：从周围八个栅格中找到下降梯度最大的点

二、planner_core

1.执行过程

首先，注册全局路径插件，使其成为ros插件，在ros中使用

PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

进行初始化：如果没有初始化，就执行初始化操作，获取costmap的尺寸大小，初始化参数：old_navfn_behavior_、use_quadratic（是否使用二次逼近：是：调用QuadraticCalculator-推荐，否：调用PotentialCalculator）、use_dijkstra（true:使用dijkstra,false:A*）、use_grid_path（是：栅格路径；否：梯度路径–一般平滑性较好）

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)
            p_calc_ = new QuadraticCalculator(cx, cy); 
        else
            p_calc_ = new PotentialCalculator(cx, cy); 
        bool use_dijkstra;  是否使用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_);  //new 出path_maker_实例，从可行点中提取路径
        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);
		//是否探索未知区域，flase--不可到达
        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);
        private_nh.param("outline_map", outline_map_, true);
        ......
}

makePlan函数—主要函数
通过输入起始位姿、目标点，返回plan路径结果，调用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函数：

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_); //加锁

1.是否已经初始化；

	//初始化
	if (!initialized_) {
        ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }

2.清空路径；

    plan.clear();

3.判断目标点和起始点的坐标系和全局坐标系一致

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

4.判断起始点和目标点是否超出地图范围

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

5.清除起始单元格，不可能是障碍物

    clearRobotCell(start, start_x_i, start_y_i);

6.确保全局规划用的数组大小和地图大小一致

    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    potential_array_ = new float[nx * ny];
    if(outline_map_)
        outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);

7.计算势场数组：用到的是calculatePotentials函数

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

8.提取全局路径：使用函数getPlanFromPotential，结果保存在plan发出

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

9.给路径添加方向

    orientation_filter_->processPath(start, plan);    //orientation_filter.cpp

10.发布路径并可视化，删除new的对象，释放内存

    publishPlan(plan);
    delete[] potential_array_;
    return !plan.empty();

makePlan结束

2. calculatePotentials()

若全局规划器选择A* ,，就去astar.cpp中找；选择Dijkstra就去dijkstra.cpp中找。

bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,int cycles, float* potential) 
{
//参数：costs地图指针、起点坐标、终点坐标、循环次数（地图栅格的二倍）、代价数组
//下面是A*算法过程，使用队列 queue ,按照一定的优先级，将起放入队列；将potential数组全部设为最大POT_HIGH；将起点的potential设为0
	queue_.clear();//清空队列
    int start_i = toIndex(start_x, start_y);   //起点的索引
    queue_.push_back(Index(start_i, 0));  //起点加入队列
    std::fill(potential, potential + ns_, POT_HIGH);  // ns_ : 单元格总数
    potential[start_i] = 0;
    int goal_i = toIndex(end_x, end_y);   //目标点索引
    int cycle = 0;
    //进入循环，只要队列不为空，得到最小cost的索引，并删除
    while (queue_.size() > 0 && cycle < cycles){
        Index top = queue_[0];
        std::pop_heap(queue_.begin(), queue_.end(), greater1());
        queue_.pop_back();
        int i = top.i;
        //若i=目标点索引，搜索成功，返回true
        if (i == goal_i)
            return true;
        //将代价最小点i周围点加入搜索队里并更新代价值, 即对前后左右四个点执行add函数
        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函数
首先进行边界检查，超出地图范围返回，障碍物点返回，已经搜索过的点返回；
之后开始进行判断，这里用的是简单的potential_calculator计算代价，同时使用的是曼哈顿距离计算代价。

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)
        return;
    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))
        return;
    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);
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
    std::push_heap(queue_.begin(), queue_.end(), greater1());
}

3. getPlanFromPotential()

在planner_core.cpp中，同时调用 path_maker_->getPath函数得到path,但没有方向信息

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_;
    // 清空路径
    plan.clear();
    std::vector<std::pair<float, float> > path;
    // 这个path的点只有在map中的位置信息(x,y)
    if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {
        ROS_ERROR("NO PATH!");
        return false;
    }
    //获取到path
    ros::Time plan_time = ros::Time::now();
    // 将path中每个点转换到world下，方向信息还没加入，这里统一设为零，然后依次存储到plan中
    for (int i = path.size() -1; i>=0; i--) {
        std::pair<float, float> point = path[i];
        // 把map的全局路径转换到world下
        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.cpp

path_maker_->getPath函数：grid_path.cpp

bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {

path_maker_->getPath函数：gradient_path.cpp

bool GradientPath::getPath(float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path) {
 

---

总结

本文大概梳理了global_planner的主要工作流程，具体的函数实现没有特别解释，只用作大致梳理，用到再仔细研究。