tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);

void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
{
  if(!initialized_)
  {
    // create Node Handle with name of plugin (as used in move_base for loading)
    ros::NodeHandle nh("~/" + name);
	        
    // get parameters of TebConfig via the nodehandle and override the default config
    cfg_.loadRosParamFromNodeHandle(nh);  // TebConfig  cfg_;
    // nh.param("trapped_threshold", trapped_threshold_, 20);
    nh.param("filter_back_velocity", filter_back_velocity_, true); 
    // nh.param("setting_acc", setting_acc_, 1.5);     

    obstacles_.reserve(500);    // ObstContainer  obstacles_;
        
    // 在TebVisualization构造函数里,注册话题 global_plan, local_plan,teb_poses, teb_markers, teb_feedback
    visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); 
        
    //创建robot footprint/contour model for optimization，对5种类型分别处理
    // 点类型最简单，返回新建的PointRobotFootprint共享指针
    // 对于polygon类型，只对 Point2dContainer vertices_;赋值
    RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);
    
    // 根据参数，选择创建哪种 planner
    if (cfg_.hcp.enable_homotopy_class_planning)
    {
      planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies enabled.");
    }
    else
    {
      planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_));
      ROS_INFO("Parallel planning in distinctive topologies disabled.");
    }
    // init other variables
    tf_ = tf;   // 在 move_base_node.cpp中最初定义
    // 这个就是 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    costmap_ros_ = costmap_ros;
    // locking should be done in MoveBase
    costmap_ = costmap_ros_->getCostmap();

    // 创建 CostmapModel 共享指针
    costmap_model_ = boost::make_shared<base_local_planner::CostmapModel>(*costmap_);

    // 就是return global_frame, 实际是 "map"
    global_frame_ = costmap_ros_->getGlobalFrameID();
    cfg_.map_frame = global_frame_; // 更新cf_的成员

    // 代价地图的local frame, 实际是 "base_footprint"
    robot_base_frame_ = costmap_ros_->getBaseFrameID();

    //Initialize a costmap to polygon converter
    if (!cfg_.obstacles.costmap_converter_plugin.empty())
    {
      try
      {
        costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin);
        std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin);
        // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
        boost::replace_all(converter_name, "::", "/");
        costmap_converter_->setOdomTopic(cfg_.odom_topic);
        costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));
        costmap_converter_->setCostmap2D(costmap_);
        
        costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread);
        ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded.");        
      }
      catch(pluginlib::PluginlibException& ex)
      {
        ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treated as point obstacles. Error message: %s", ex.what());
        costmap_converter_.reset();
      }
    }
    else 
      ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles.");
  
    // 类型是 vector<geometry_msgs::Point>
    footprint_spec_ = costmap_ros_->getRobotFootprint();
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
    // 更新cf_的成员
    cfg_.robot.robot_inscribed_radius =  robot_inscribed_radius_;
    cfg_.robot.robot_circumscribed_radius = robot_circumscribed_radius; 
    
    // init the odom helper to receive the robot's velocity from odom messages
    odom_helper_.setOdomTopic(cfg_.odom_topic);

    // setup dynamic reconfigure
    dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(nh);
    dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);
    
    // validate optimization footprint and costmap footprint
    // 对于point类型，第1个参数是0，要求最好第1项+第3项 > 第2项
    // 否则 Infeasible optimziation results might occur frequently
    validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

    // custom obstacles的回调函数，话题 /move_base/TebLocalPlannerROS/obstacles
    custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

    // custom via-points的回调，话题 /move_base/TebLocalPlannerROS/via_points
    via_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this);
    // queue_follow 所用，话题 /rectangle_distance
    front_dist_sub_ = nh.subscribe("/rectangle_distance", 1, &TebLocalPlannerROS::frontDistanceCB, this);
    // 新添加
    footprint_sub_ = nh.subscribe("/footprint", 10, &TebLocalPlannerROS::footprintCB, this);

    
    ros::NodeHandle nh_move_base("~");
    double controller_frequency = 5;
    // 实际配置是 30
    nh_move_base.param("controller_frequency", controller_frequency, controller_frequency);
    // initialize failure detector，Detect if the robot got stucked
    failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency));
    // 初始化，返回一个很大的数
    min_distance_custom_obstacle = std::numeric_limits<double>::max();
    
    // set initialized flag
    initialized_ = true;
    ROS_DEBUG("teb_local_planner plugin initialized.");
  }
  else
    ROS_WARN("teb_local_planner has already been initialized, doing nothing.");
}

// 局部的子目标 (transformed plan最后一个元素)
tf::Stamped<tf::Pose> goal_point;
tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
robot_goal_.x() = goal_point.getOrigin().getX();
robot_goal_.y() = goal_point.getOrigin().getY();

if (cfg_.trajectory.global_plan_overwrite_orientation)
{
    robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, 
       goal_idx,  tf_plan_to_global);
  // overwrite/update goal orientation of the transformed plan with the actual goal
   // (enable using the plan as initialization)
  transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());
}
else
    robot_goal_.theta() = tf::getYaw(goal_point.getRotation());

// plan only contains the goal, 一般在导航的最后出现size是1的情况
// 用实际的机器人位姿更新 transformed plan 的起点(allows using the plan as initial trajectory)
if (transformed_plan.size()==1) 
{    // 插入起点，但不初始化
  transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped() );
}
tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // 把起点更新为 机器人当前位姿

int main()
{
  VectorXf v(2);
  MatrixXf m(2,2), n(2,2);
  
  v << -1,
       2;
  
  m << 1,-2,
       -3,4;
  cout << "v.squaredNorm() = " << v.squaredNorm() << endl;
  cout << "v.norm() = " << v.norm() << endl;
  cout << "v.lpNorm<1>() = " << v.lpNorm<1>() << endl;
  cout << "v.lpNorm<Infinity>() = " << v.lpNorm<Infinity>() << endl;
  cout << endl;
  cout << "m.squaredNorm() = " << m.squaredNorm() << endl;
  cout << "m.norm() = " << m.norm() << endl;
  cout << "m.lpNorm<1>() = " << m.lpNorm<1>() << endl;
  cout << "m.lpNorm<Infinity>() = " << m.lpNorm<Infinity>() << endl;
}

ros::Publisher cancle_pub_ = nh_.advertise("move_base/cancel",1);

actionlib_msgs::GoalID first_goal;
cancle_pub_.publish(first_goal);