// 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.");
}

/**
 * Edge defining the cost function for keeping a minimum distance from obstacles.

 * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes:
 * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. 
 * \e dist2point denotes the minimum distance to the point obstacle.
 * \e weight can be set using setInformation(). \n
 * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow()

 * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
 * @remarks Do not forget to call setTebConfig() and setObstacle()
*/     
class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
{
public: 
  EdgeObstacle() 
  {
    _measurement = NULL;
  }
  /**
   * @brief Set pointer to associated obstacle for the underlying cost function 
   * @param obstacle 2D position vector containing the position of the obstacle
   */ 
  void setObstacle(const Obstacle* obstacle)
  {
      _measurement = obstacle;
  }
  /**
   * @brief Set pointer to the robot model 
   * @param robot_model Robot model required for distance calculation
   */ 
  void setRobotModel(const BaseRobotFootprintModel* robot_model)
  {
      robot_model_ = robot_model;
  }
  /**
   * @brief Set all parameters at once
   * @param cfg TebConfig class
   * @param robot_model Robot model required for distance calculation
   * @param obstacle 2D position vector containing the position of the obstacle
   */ 
  void setParameters(const TebConfig& cfg, 
       const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
    {
      cfg_ = &cfg;
      robot_model_ = robot_model;
      _measurement = obstacle;
    }
      // cost function
    void computeError()
    {
        ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()" );
        const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
          /*  inline double penaltyBoundFromBelow( const double& var, 
                          const double& a,  const double& epsilon)
        {
          if (var >= a+epsilon)
            return 0.;
          else
            return (-var + (a+epsilon));
        }  */
        double dist = robot_model_->calculateDistance(bandpt->pose(),  _measurement);

          // Original obstacle cost
        _error[0] = penaltyBoundFromBelow( dist, 
                    cfg_->obstacles.min_obstacle_dist, 
                    cfg_->optim.penalty_epsilon );
        if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
        {
            // Optional non-linear cost. Note the max cost (before weighting) is
            // the same as the straight line version and that all other costs are
            // below the straight line (for positive exponent), so it may be
            // necessary to increase weight_obstacle and/or the inflation_weight
            // when using larger exponents
            _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(
                _error[0] / cfg_->obstacles.min_obstacle_dist, 
                cfg_->optim.obstacle_cost_exponent);
        }
        ROS_ASSERT_MSG(std::isfinite(_error[0]), 
              "EdgeObstacle::computeError() _error[0]=%f\n",
              _error[0]);
    }
    protected:
      const BaseRobotFootprintModel* robot_model_;
    public:   
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier)
{
  if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr )
    return;
  // 判断 inflation_dist > min_obstacle_dist
  bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist;

  Eigen::Matrix<double,1,1> information;
  information.fill(cfg_->optim.weight_obstacle * weight_multiplier);
  
  Eigen::Matrix<double,2,2> information_inflated;
  information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier;
  information_inflated(1,1) = cfg_->optim.weight_inflation;
  information_inflated(0,1) = information_inflated(1,0) = 0;

  auto iter_obstacle = obstacles_per_vertex_.begin();

  auto create_edge = [inflated, &information, &information_inflated, this] (int index, 
      const Obstacle* obstacle) {
    if (inflated)
    {
      // 一元边，观测值维度为2，类型为Obstacle基类，连接VertexPose顶点
      EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle;
      dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
      // 信息矩阵为2x2对角阵，(0,0) = weight_obstacle, (1,1) = weight_inflation
      dist_bandpt_obst->setInformation(information_inflated);
      dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle);
      optimizer_->addEdge(dist_bandpt_obst);
    }
    else
    {
      // 一元边，观测值维度为1，测量值类型为Obstacle基类，连接VertexPose顶点
      EdgeObstacle* dist_bandpt_obst = new EdgeObstacle;
      dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index));
      // 信息矩阵为 1x1  weight_obstacle * weight_multiplier
      dist_bandpt_obst->setInformation(information);
      dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obstacle);
      optimizer_->addEdge(dist_bandpt_obst);
    };
  };
    
   /*  iterate all teb points, skipping the last and, if the 
   EdgeVelocityObstacleRatio edges should not be created, the first one too*/
  const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0;
  for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i)
  {
      double left_min_dist = std::numeric_limits<double>::max();
      double right_min_dist = std::numeric_limits<double>::max();
      ObstaclePtr left_obstacle;
      ObstaclePtr right_obstacle;
      
      const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec();
      
      // iterate obstacles
      for (const ObstaclePtr& obst : *obstacles_)
      {
        // we handle dynamic obstacles differently below
        if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic())
          continue;

          // calculate distance to robot model
          double dist = robot_model_->calculateDistance(teb_.Pose(i), obst.get());
          
          // force considering obstacle if really close to the current pose
        if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor)
        {
            iter_obstacle->push_back(obst);
            continue;
        }
          // cut-off distance
          if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor)
              continue;
          
          // determine side (left or right) and assign obstacle if closer than the previous one
          if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
          {
              if (dist < left_min_dist)
              {
                  left_min_dist = dist;
                  left_obstacle = obst;
              }
          }
          else
          {
              if (dist < right_min_dist)
              {
                  right_min_dist = dist;
                  right_obstacle = obst;
              }
          }
      }
      // create obstacle edges
      if (left_obstacle)
          iter_obstacle->push_back(left_obstacle);
      if (right_obstacle)
          iter_obstacle->push_back(right_obstacle);

      // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges
      if (i == 0)
      {
        ++iter_obstacle;
        continue;
      }
      // create obstacle edges
      for (const ObstaclePtr obst : *iter_obstacle)
        create_edge(i, obst.get());
      ++iter_obstacle;
  }
}

if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
{
  /*  Optional 非线性代价. Note the max cost (before weighting) is
  the same as the straight line version and that all other costs are
  below the straight line (for positive exponent), so it may be
  necessary to increase weight_obstacle and/or the inflation_weight
  when using larger exponents  */
  _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(
    _error[0] / cfg_->obstacles.min_obstacle_dist,  cfg_->optim.obstacle_cost_exponent);
}

// 如果优化结果违反约束(由于是软约束，所以是可能的)， Saturate 速度
saturateVelocity(cmd_vel.linear.x,  cmd_vel.linear.y,  cmd_vel.angular.z, 
    cfg_.robot.max_vel_x,      cfg_.robot.max_vel_y, 
    cfg_.robot.max_vel_theta,  cfg_.robot.max_vel_x_backwards);

// Limit translational velocity for forward driving
if (vx > max_vel_x)
  vx = max_vel_x;

// limit strafing velocity
if (vy > max_vel_y)
  vy = max_vel_y;
else if (vy < -max_vel_y)
  vy = -max_vel_y;

if(vx>= -0.12 && vx< 0)
{
  vx = -0.12;
}
else if(vx< -0.12)
{
  planner_->clearPlanner();
}
// Limit angular velocity
if (omega > max_vel_theta)
  omega = max_vel_theta;
else if (omega < -max_vel_theta)
  omega = -max_vel_theta;

// 对汽车机器人，convert rot-vel to steering angle if desired
// The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
// and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.
if (cfg_.robot.cmd_angle_instead_rotvel)
{ // 目前是false，省略 
}
// a feasible solution should be found, reset counter
no_infeasible_plans_ = 0;
// 保存上一个速度命令 (for recovery analysis etc.)
last_cmd_ = cmd_vel;

// Now visualize everything    
planner_->visualize();
visualization_->publishObstacles(obstacles_);
visualization_->publishViaPoints(via_points_);
visualization_->publishGlobalPlan(global_plan_);
return true;  // 函数 computeVelocityCommands 结束

// Get the velocity command for this sampling interval
if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, 
		cfg_.trajectory.control_look_ahead_poses,  robot_vel_.linear.x, 
		robot_vel_.angular.z)  )
{
	planner_->clearPlanner();
	ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner...");
	++no_infeasible_plans_; // increase number of infeasible solutions in a row
	time_last_infeasible_plan_ = ros::Time::now();
	last_cmd_ = cmd_vel;
	return false;
}

bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, 
          int look_ahead_poses, double current_vx, double current_vtheta) const
{
  // TimedElasticBand  teb_; //!< Actual trajectory object
  if (teb_.sizePoses()<2)
  {
    ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): 
    	The trajectory  contains less than 2 poses. Make sure to init and 
    			optimize/plan the trajectory fist." );
    vx = 0;
    vy = 0;
    omega = 0;
    return false;
  }
  // teb_.sizePoses() 在规划中一直很大，除非是接近终点时候
  look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
  double dt = 0.0;
  /*  double& TimeDiff(int index)
	{
		ROS_ASSERT(index<sizeTimeDiffs()); 
		return timediff_vec_.at(index)->dt();
	} */
  ROS_INFO("\033[45;37m  before  counter, look_ahead_poses: %f  \033[0m", look_ahead_poses);
  for(int counter = 0; counter < look_ahead_poses; ++counter)
  {
		dt += teb_.TimeDiff(counter);
		// TODO: change to look-ahead time? Refine trajectory ?
		if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses)  
		{
		    look_ahead_poses = counter + 1;
		    break;
		}
  }
  if (dt<=0)
  {	
    ROS_ERROR("getVelocityCommand()  timediff<=0 is invalid!");
    vx = 0; 	vy = 0; 	omega = 0;
    return false;
  }
  // Get velocity from the first two configurations
  extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega);

  double v_new = vx;
  double k = (v_new - current_vx) * (v_new - current_vx) / 
  (  (v_new - current_vx) * (v_new - current_vx)
  	+ (cfg_->robot.acc_lim_x * dt) * (cfg_->robot.acc_lim_x * dt)  );
  vx = current_vx + k * (v_new - current_vx);
  return true;
}

// pose2 是 teb_.Pose(look_ahead_poses)
void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, 
	double dt,  double& vx,  double& vy,  double& omega)  const
{
  if (dt == 0)
  {
    vx = 0;   vy = 0;   omega = 0;
    return;
  }
  Eigen::Vector2d deltaS = pose2.position() - pose1.position();
  
  if (cfg_->robot.max_vel_y == 0) // nonholonomic robot
  {
    Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta())  );
    //  translational  velocity
    double dir = deltaS.dot(conf1dir);
    // 无后退的情况下，sign函数都返回 1
    vx = (double) g2o::sign(dir) * deltaS.norm() / dt;
    vy = 0;
  }
  else  // holonomic robot   ...... 省略

   // rotational velocity
  double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
  omega = orientdiff / dt;
}

inline int sign(T x)
{
  if (x > 0)        return 1;
  else if (x < 0)   return -1;
  else   return 0;
}