一、入口
找入口就找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：
其實功能和上面的回調函數一樣，都是調用了makeplan（）函數，這里就不再贅述了。