标签：resize 左右 was a* 清空 解析 extra copy virt
一、入口
找入口就找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);
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（）函数继续看：
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_，这个函数将
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：
Navigation（六）global_planner源码解析之planner_core
标签：resize 左右 was a* 清空 解析 extra copy virt
原文地址：https://www.cnblogs.com/JuiceCat/p/13084473.html
