Vector3  getAxis () const    //Return the axis of the rotation represented by this quaternion. 
//Set the quaternion using fixed axis RPY. 
void 	setRPY (const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)    
//Return an identity quaternion.
tf::Quaternion createIdentityQuaternion()  

tf::Quaternion::getAngle () const

// [inline, static]
static void tf::quaternionMsgToTF(const geometry_msgs::Quaternion& msg, tf::Quaternion& bt)

tf::Quaternion quat;
double roll, pitch, yaw;
// 赋值给quat，getRPY函数 将quat转为欧拉角
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

#include “tf2/LinearMath/Matrix3x3.h”

tf2::Matrix3x3 mat(tf2::Quaternion( x, y, z, w) );
mat.getEulerYPR(tf2Scalar&  yaw,
tf2Scalar&  pitch,
tf2Scalar& 	roll,
unsigned int 	solution_number = 1); 

//只通过yaw计算四元数，用于平面小车
static geometry_msgs::Quaternion  tf::createQuaternionMsgFromYaw(double yaw);

static tf::Quaternion  createQuaternionFromYaw (double yaw)

static tf::Quaternion  createQuaternionFromRPY(double roll, double pitch, double yaw)

static geometry_msgs::Quaternion  createQuaternionMsgFromRollPitchYaw( double roll, 
	double pitch,   double yaw)

void HomotopyClassPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacles,
          RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, 
          const ViaPointContainer* via_points)
{
  cfg_ = &cfg;
  obstacles_ = obstacles;
  via_points_ = via_points;
  robot_model_ = robot_model;

  if (cfg_->hcp.simple_exploration)
    graph_search_ = boost::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
  else
    graph_search_ = boost::shared_ptr<GraphSearchInterface>(new ProbRoadmapGraph(*cfg_, this));

  initialized_ = true;
  setVisualization(visual);     // 只有一句，对 visualization_ 赋值
}

// initial_plan 其实就是 transformed_plan
bool HomotopyClassPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, 
          const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");

  // store initial plan for further initializations (must be valid for the 
  // lifetime of this object or clearPlanner() is called!)
  initial_plan_ = &initial_plan;

  PoseSE2 start(initial_plan.front().pose);
  PoseSE2 goal(initial_plan.back().pose);

  return plan(start, goal, start_vel, free_goal_vel);
}

// 然后是下面重要的重载函数
bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, 
          const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");

  // Update old TEBs with new start, goal and velocity
  updateAllTEBs(&start, &goal, start_vel);

  // Init new TEBs based on newly explored homotopy classes
  exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel);
  // update via-points if activated
  updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
  // Optimize all trajectories in alternative homotopy classes
  optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
  // Select which candidate (based on alternative homotopy classes) should be used
  selectBestTeb();

  initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
  return true;
}

PointObstacle::checkLineIntersection
{
  return getMinimumDistance(point) < min_dist;
}

virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
{
      return getMinimumDistance(point) < min_dist;
}

void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
{
  // optimize TEBs in parallel since they are independend of each other

  if (cfg_->hcp.enable_multithreading)
  {
    // Must prevent .join_all() from throwing exception if interruption was
    // requested, as this can lead to multiple threads operating on the same
    // TEB, which leads to SIGSEGV
    boost::this_thread::disable_interruption di;

    boost::thread_group teb_threads;
    for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
    {
      teb_threads.create_thread( boost::bind(&TebOptimalPlanner::optimizeTEB, 
        it_teb->get(), iter_innerloop, iter_outerloop,  true, 
        cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale,
        cfg_->hcp.selection_alternative_time_cost)  );
    }
    teb_threads.join_all();
  }
}

if (!observation_list_.empty() )
{
    list<Observation>::iterator obs_it = observation_list_.begin();
    // if keeping observations for no time, only keep one observation

    // observation_keep_time_ 就是参数 observation_persistence
    if (observation_keep_time_ == ros::Duration(0.0))
    {
		  observation_list_.erase(++obs_it, observation_list_.end());
		  return;
    }
    // or loop through the observations to see which ones are stale
    for ( obs_it = observation_list_.begin(); obs_it != observation_list_.end(); 
            ++obs_it )
    {
		Observation& obs = *obs_it;
		// if observation is out of date, remove it and those follow from the list
		// kinetic: ros::Duration time_diff = last_updated_ - pcl_conversions::fromPCL(obs.cloud_->header).stamp;
		if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
		{
  			observation_list_.erase(obs_it, observation_list_.end());
  			return;
		}
    }
}