boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex() ));
plan.clear();
if(planner_costmap_ros_ == NULL) {
    log.error("Planner costmap ROS is NULL, unable to create global plan");
    return false;
}
//get the starting pose of the robot
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
    log.warn("Unable to get starting pose of robot, unable to create global plan");
    return false;
}
geometry_msgs::PoseStamped start;
tf::poseStampedTFToMsg(global_pose, start);

// 这里就是 GlobalPlanner::makePlan 了
if(!planner_->makePlan(start, goal, plan) || plan.empty()){
    ROS_WARN("[move_base] Failed to find a  plan to point (%.2f, %.2f)", 
    	goal.pose.position.x, goal.pose.position.y);
    return false;
}
return true;

if (inf_is_valid)
{
  ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}

nav2_util::LifecycleNode::SharedPtr  nh
last_updated_(nh->now())

// clear_after 调用时写死为false
bool TebOptimalPlanner::optimizeGraph(int no_iterations, bool clear_after)
{
  if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples)
  {
    ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization.");
    if (clear_after)
        clearGraph();
    return false; 
  }
  // 头文件定义 boost::shared_ptr<g2o::SparseOptimizer> optimizer_;
  // 开始图优化
  optimizer_->setVerbose(cfg_->optim.optimization_verbose);
  optimizer_->initializeOptimization();
  int iter = optimizer_->optimize(no_iterations);  // 优化结束
  
  //   可以保存 海森矩阵
   /*  g2o::OptimizationAlgorithmLevenberg lm = 
      dynamic_cast<g2o::OptimizationAlgorithmLevenberg*>(optimizer_->solver());
   lm->solver()->saveHessian("~/Matlab/Hessian.txt");*/
  if (!iter)
  {
     ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter);
     return false;
  }
  if (clear_after)
     clearGraph();  
    
  return true;
}

success = optimizeGraph(iterations_innerloop, false);
if (!success) 
{
    clearGraph();
    return false;
}
optimized_ = true;
// 只在最后一次外循环时 compute cost vec
if (compute_cost_afterwards && i==iterations_outerloop-1) 
  computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);

clearGraph();
weight_multiplier *= cfg_->optim.weight_adapt_factor;
// iterations_outerloop 结束

void TebOptimalPlanner::computeCurrentCost( double obst_cost_scale, 
      double viapoint_cost_scale,   bool alternative_time_cost)
{ 
  // check if graph is empty/exist, important if function is called 
  //  between buildGraph and optimizeGraph/clearGraph
  bool graph_exist_flag(false);
  if (optimizer_->edges().empty() && optimizer_->vertices().empty())
  {
    // here the graph is build again, for time efficiency make sure to call this function 
    // between buildGraph and Optimize (deleted), but it depends on the application
    buildGraph(); 
    optimizer_->initializeOptimization();
  }
  else
      graph_exist_flag = true;

  optimizer_->computeInitialGuess();
  //  cost_ 在 TebOptimalPlanner构造函数里 初始化为 HUGE_VAL
  cost_ = 0;  
  if (alternative_time_cost)
  {
    cost_ += teb_.getSumOfAllTimeDiffs();
    /*  we use SumOfAllTimeDiffs() here, because edge cost depends on 
    number of samples, which is not always the same for similar TEBs,
    since we are using an AutoResize Function with hysteresis  */
  }
  // now we need pointers to all edges -> calculate error for each edge-type
  // since we aren't storing edge pointers, we need to check every edge
  for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = 
    optimizer_->activeEdges().begin();  it!= optimizer_->activeEdges().end(); it++)
  {
    double cur_cost = (*it)->chi2();

    if (dynamic_cast<EdgeObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr
        || dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr)
    {
      cur_cost *= obst_cost_scale;
    }
    else if (dynamic_cast<EdgeViaPoint*>(*it) != nullptr)
        cur_cost *= viapoint_cost_scale;
    else if (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost)
    {
      // skip these edges if alternative_time_cost is active
      continue; 
    }
    cost_ += cur_cost;
  }
  // delete temporary created graph
  if (!graph_exist_flag) 
    clearGraph();
}

// 每层的初始化
void Layer::initialize(LayeredCostmap* parent, std::string name, 
                          tf::TransformListener *tf)
{
    layered_costmap_ = parent;
    name_ = name;
    tf_ = tf;
    onInitialize(); 	//虚函数，空
}
const std::vector<geometry_msgs::Point>& Layer::getFootprint() const
{
    return layered_costmap_->getFootprint();
}

if (!private_nh.hasParam("plugins") )
    resetOldParameters(private_nh);

if (private_nh.hasParam("plugins") )
{
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str()  );

      // plugin_loader_ 类型是　pluginlib::ClassLoader<Layer>，ROS插件机制
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      // 添加到容器 plugins_
      layered_costmap_->addPlugin(plugin);
      // 执行 Layer::initialize，它向各层地图通知主地图的存在，并调用
      // oninitialize方法（Layer类中的虚函数，被定义在各层地图中）
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
}

/* called by the LayeredCostmap to poll this plugin as to how
   much of the costmap it needs to update. 
   Each layer can increase  the size of this bounds.
 */
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, 
      double* min_x, double* min_y, double* max_x, double* max_y) {}

/** Actually update the underlying costmap, only within the bounds
 * calculated during UpdateBounds(). */
virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}

/* Stop publishers. */
virtual void deactivate() {}
/* Restart publishers if they've been stopped. */
virtual void activate() {}
virtual void reset() {}
  /* make this layer match the size of the parent costmap. */
virtual void matchSize() {}
  /** LayeredCostmap calls this whenever the footprint there
 * changes (via LayeredCostmap::setFootprint()).  
  Override to be notified of changes to the robot's footprint. */
virtual void onFootprintChanged() {}
virtual void onInitialize() {}

#define ROS_INFO(...)
#define ROS_INFO_COND(cond, ...)
#define ROS_INFO_ONCE(...)
#define ROS_INFO_STREAM(args)
#define ROS_INFO_NAMED(name, ...)
#define ROS_INFO_THROTTLE(rate, ...)
#define ROS_INFO_FILTER(filter, ...)

for(int i=0; i<3; i++)
    ROS_INFO_ONCE("1111111");

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

boost::mutex::scoped_lock lock(mutex_);
if (!initialized_) {
    ROS_ERROR(  "This planner has not been initialized yet, but it is being used, 
        please call initialize() before use");
    return false;
}
plan.clear();

bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), 
      start_x, start_y, goal_x, goal_y, 
      nx*ny*2, potential_array_ );

if(oscillation_timeout_ > 0.0 &&  ros::Time::now() - last_oscillation_reset_ > ros::Duration(oscillation_timeout_)  )
{
  publishZeroVelocity();
  state_ = CLEARING;
  recovery_trigger_ = OSCILLATION_R;
  movebase_log.error("oscillate in a small area for a long time !");
}

if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
  last_oscillation_reset_ = ros::Time::now();
  oscillation_pose_ = current_position;

  //if our last recovery was caused by oscillation, we want to reset the recovery index 
  if(recovery_trigger_ == OSCILLATION_R)
    recovery_index_ = 0;
}

else if(recovery_trigger_ == OSCILLATION_R)
{
	movebase_log.error("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
	as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
}