Navigationglobal_planner源码解析之planner_core

Posted 2020-12-10 juicecat

 

一、入口

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

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

 

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

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

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

 

二、声明

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

1 class PlannerWithCostmap : public GlobalPlanner

 

这个类中声明了几个函数和变量：

1     public:
2         PlannerWithCostmap(string name, Costmap2DROS* cmap);
3         bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp);//全局路径
4 
5     private:
6         void poseCallback(const rm::PoseStamped::ConstPtr& goal);
7         Costmap2DROS* cmap_;
8         ros::ServiceServer make_plan_service_;
9         ros::Subscriber pose_sub_;

 

三、Planner_core

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

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

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

 

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

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

 

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

 1         private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
 2         if(!old_navfn_behavior_)
 3             convert_offset_ = 0.5;
 4         else
 5             convert_offset_ = 0.0;
 6 
 7         bool use_quadratic;
 8         private_nh.param("use_quadratic", use_quadratic, true);
 9         if (use_quadratic)
10             p_calc_ = new QuadraticCalculator(cx, cy);
11         else
12             p_calc_ = new PotentialCalculator(cx, cy);  //p_calc_实例：计算“一个点”的可行性
13 
14         bool use_dijkstra;
15         private_nh.param("use_dijkstra", use_dijkstra, true);
16         if (use_dijkstra)
17         {
18             DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
19             if(!old_navfn_behavior_)
20                 de->setPreciseStart(true);
21             planner_ = de;
22         }
23         else
24             planner_ = new AStarExpansion(p_calc_, cx, cy);  //planner_实例：计算“所有”的可行点potential array
25 
26         bool use_grid_path;
27         private_nh.param("use_grid_path", use_grid_path, false);
28         if (use_grid_path)
29             path_maker_ = new GridPath(p_calc_);//栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点
30         else
31             path_maker_ = new GradientPath(p_calc_);  //梯度路径，从周围八个栅格中找到下降梯度最大的点

 

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

 1         orientation_filter_ = new OrientationFilter();  //
 2         //定义两个发布器
 3         plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
 4         potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
 5 
 6         private_nh.param("allow_unknown", allow_unknown_, true);
 7         planner_->setHasUnknown(allow_unknown_);
 8         private_nh.param("planner_window_x", planner_window_x_, 0.0);
 9         private_nh.param("planner_window_y", planner_window_y_, 0.0);
10         private_nh.param("default_tolerance", default_tolerance_, 0.0);
11         private_nh.param("publish_scale", publish_scale_, 100);

 

发布一个make_plan的Service，make_plan_srv_：

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

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

1 bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
2     makePlan(req.start, req.goal, resp.plan.poses);
3 
4     resp.plan.header.stamp = ros::Time::now();
5     resp.plan.header.frame_id = frame_id_;
6 
7     return true;
8 }

 

接着加载动态调参：

1 void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) {
2     planner_->setLethalCost(config.lethal_cost);
3     path_maker_->setLethalCost(config.lethal_cost);
4     planner_->setNeutralCost(config.neutral_cost);
5     planner_->setFactor(config.cost_factor);
6     publish_potential_ = config.publish_potential;
7     orientation_filter_->setMode(config.orientation_mode);
8     orientation_filter_->setWindowSize(config.orientation_window_size);
9 }

 

 

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

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

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

 1 bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {
 2     vector<PoseStamped> path;
 3 
 4     req.start.header.frame_id = "map";
 5     req.goal.header.frame_id = "map";
 6     bool success = makePlan(req.start, req.goal, path);
 7     resp.plan_found = success;
 8     if (success) {
 9         resp.path = path;
10     }
11 
12     return true;
13 }

 

其中，调用了makePlan（）函数，跳转到该函数，传入的参数是起始点、目标点和规划的路径，定义如下：

1 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
2                            std::vector<geometry_msgs::PoseStamped>& plan) {
3     return makePlan(start, goal, default_tolerance_, plan);//调用下面的makeplan
4 }

 

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

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

 

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

1     plan.clear();

 

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

 1     if (goal.header.frame_id != global_frame) {
 2         ROS_ERROR(
 3                 "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());
 4         return false;
 5     }
 6 
 7     if (start.header.frame_id != global_frame) {
 8         ROS_ERROR(
 9                 "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());
10         return false;
11     }

 

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

1     double wx = start.pose.position.x;
2     double wy = start.pose.position.y;
3     unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
4     double start_x, start_y, goal_x, goal_y;
5     if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i))。。。

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

 1 bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
 2 {
 3   if (wx < origin_x_ || wy < origin_y_)
 4     return false;
 5 
 6   mx = (int)((wx - origin_x_) / resolution_);
 7   my = (int)((wy - origin_y_) / resolution_);
 8 
 9 
10   if (mx < size_x_ && my < size_y_)
11     return true;
12 
13   return false;
14 }

 

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

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

1     clearRobotCell(start, start_x_i, start_y_i);

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

 1 void GlobalPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my) {
 2     if (!initialized_) {
 3         ROS_ERROR(
 4                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
 5         return;
 6     }
 7 
 8     //把关联的cost设置为free
 9     costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
10 }

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

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

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

 

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

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

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

 

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

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

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

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

 1 void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) {
 2     unsigned char* pc = costarr;
 3     for (int i = 0; i < nx; i++)
 4         *pc++ = value;
 5     pc = costarr + (ny - 1) * nx;
 6     for (int i = 0; i < nx; i++)
 7         *pc++ = value;
 8     pc = costarr;
 9     for (int i = 0; i < ny; i++, pc += nx)
10         *pc = value;
11     pc = costarr + nx - 1;
12     for (int i = 0; i < ny; i++, pc += nx)
13         *pc = value;
14 }

 

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

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

1  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（）函数，该函数把终点周围的点更新了一下：

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

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

 1         void clearEndpoint(unsigned char* costs, float* potential, int gx, int gy, int s){
 2             int startCell = toIndex(gx, gy);
 3             for(int i=-s;i<=s;i++){
 4             for(int j=-s;j<=s;j++){
 5                 int n = startCell+i+nx_*j;
 6                 if(potential[n]<POT_HIGH)
 7                     continue;
 8                 float c = costs[n]+neutral_cost_;
 9                 float pot = p_calc_->calculatePotential(potential, c, n);
10                 potential[n] = pot;
11             }
12             }
13         }

 

然后发布可行点：

1 publishPotential(potential_array_)

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

 1 void GlobalPlanner::publishPotential(float* potential)
 2 {
 3     int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
 4     double resolution = costmap_->getResolution();
 5     nav_msgs::OccupancyGrid grid;
 6     // Publish Whole Grid
 7     grid.header.frame_id = frame_id_;
 8     grid.header.stamp = ros::Time::now();
 9     grid.info.resolution = resolution;
10 
11     grid.info.width = nx;
12     grid.info.height = ny;
13 
14     double wx, wy;
15     costmap_->mapToWorld(0, 0, wx, wy);
16     grid.info.origin.position.x = wx - resolution / 2;
17     grid.info.origin.position.y = wy - resolution / 2;
18     grid.info.origin.position.z = 0.0;
19     grid.info.origin.orientation.w = 1.0;
20 
21     grid.data.resize(nx * ny);
22 
23     float max = 0.0;
24     for (unsigned int i = 0; i < grid.data.size(); i++) {
25         float potential = potential_array_[i];
26         if (potential < POT_HIGH) {
27             if (potential > max) {
28                 max = potential;
29             }
30         }
31     }
32 
33     for (unsigned int i = 0; i < grid.data.size(); i++) {
34         if (potential_array_[i] >= POT_HIGH) {
35             grid.data[i] = -1;
36         } else
37             grid.data[i] = potential_array_[i] * publish_scale_ / max;
38     }
39     potential_pub_.publish(grid);
40 }

 

主要步骤二：从所有可行点中获取路径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这个数组是潜力，距离起始栅格越远的栅格潜力越高，距离障碍物越近的栅格潜力越低。每个栅格可以根据附近的栅格求出其梯度信息，且梯度方向指向起始栅格或远离障碍物栅格。

1         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
3             geometry_msgs::PoseStamped goal_copy = goal;
4             goal_copy.header.stamp = ros::Time::now();
5             plan.push_back(goal_copy);
6         } else {
7             ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn‘t happen.");
8         }

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

 1 bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y,
 2                                       const geometry_msgs::PoseStamped& goal,
 3                                        std::vector<geometry_msgs::PoseStamped>& plan) {
 4     if (!initialized_) {
 5         ROS_ERROR(
 6                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
 7         return false;
 8     }
 9 
10     std::string global_frame = frame_id_;
11 
12     //clear the plan, just in case
13     plan.clear();
14 
15     std::vector<std::pair<float, float> > path;
16 
17     ////use_grid_path   调用path_maker_->getPath()实例，提取路径
18     if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) {
19         ROS_ERROR("NO PATH!");
20         return false;
21     }
22 
23     ros::Time plan_time = ros::Time::now();
24     for (int i = path.size() -1; i>=0; i--) {
25         std::pair<float, float> point = path[i];
26         //convert the plan to world coordinates
27         double world_x, world_y;
28         mapToWorld(point.first, point.second, world_x, world_y);
29 
30         geometry_msgs::PoseStamped pose;
31         pose.header.stamp = plan_time;
32         pose.header.frame_id = global_frame;
33         pose.pose.position.x = world_x;
34         pose.pose.position.y = world_y;
35         pose.pose.position.z = 0.0;
36         pose.pose.orientation.x = 0.0;
37         pose.pose.orientation.y = 0.0;
38         pose.pose.orientation.z = 0.0;
39         pose.pose.orientation.w = 1.0;
40         plan.push_back(pose);
41     }
42     if(old_navfn_behavior_){
43             plan.push_back(goal);
44     }
45     return !plan.empty();
46 }

 

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

1     orientation_filter_->processPath(start, plan);
2 
3     publishPlan(plan);//发布可视化路径
4     delete potential_array_;
5     return !plan.empty();

其中，publishPlan(plan)如下：

 1 void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
 2     if (!initialized_) {
 3         ROS_ERROR(
 4                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
 5         return;
 6     }
 7 
 8     //create a message for the plan
 9     nav_msgs::Path gui_path;
10     gui_path.poses.resize(path.size());
11 
12     gui_path.header.frame_id = frame_id_;
13     gui_path.header.stamp = ros::Time::now();
14 
15     // Extract the plan in world co-ordinates, we assume the path is all in the same frame
16     for (unsigned int i = 0; i < path.size(); i++) {
17         gui_path.poses[i] = path[i];
18     }
19 
20     plan_pub_.publish(gui_path);
21 }

 

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