ros::Duration(0.3).sleep();
ros::NodeHandle node_handle;

ros::ServiceClient finish_trajiectry_client =
    node_handle.serviceClient<cartographer_ros_msgs::FinishTrajectory>(
        kFinishTrajectoryServiceName);

cartographer_ros_msgs::FinishTrajectory f_srv;
f_srv.request.trajectory_id = 1;
finish_trajiectry_client.call(f_srv);

geometry_msgs::Pose2D pos_now;

pos_now.x = static_cast<double>(stamped_transform.transform.translation.x);
pos_now.y = static_cast<double>(stamped_transform.transform.translation.y);
pos_now.theta = tf::getYaw(stamped_transform.transform.rotation);
current_pose_pub.publish(pos_now);

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;
		}
    }
}

if (track_unknown_space)
  default_value_ = NO_INFORMATION;
else
  default_value_ = FREE_SPACE;

// create an observation buffer
observation_buffers_.push_back(
  boost::shared_ptr < ObservationBuffer>(new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,  max_obstacle_height, 
      	obstacle_range, raytrace_range,  *tf_,  global_frame_,
          sensor_frame, transform_tolerance) )  );

// check if we'll add this buffer to our marking observation buffers
if (marking)
  	marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
  	clearing_buffers_.push_back(observation_buffers_.back());

if (data_type == "LaserScan")
	......
else if (data_type == "PointCloud")
	......
else    // PointCloud2
{
  boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
      > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
  if (inf_is_valid)
     ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
  
  boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud2>
      > filter(new tf::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50)  );
  filter->registerCallback(
      boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back() )  );
  observation_subscribers_.push_back(sub);	  // 传感器数据订阅者
  observation_notifiers_.push_back(filter);   // 保证变换可行
}

void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
                                        const boost::shared_ptr<ObservationBuffer>& buffer)
{
  buffer->lock();
  buffer->bufferCloud(*message);
  buffer->unlock();
}

void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
  Stamped < tf::Vector3 > global_origin;

  // std::list<Observation> observation_list_;
  // create a new observation on the list to be populated
  observation_list_.push_front(Observation());
  
  // check whether the origin frame has been set explicitly or 
  // whether we should get it from the cloud
  // yaml里可以不设置sensor_frame，在这里取点云的坐标系名称
  string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;

  try
  {
    // given these observations come from sensors, we store the origin pt of the sensor
    Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
                            pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);

    // global_frame_ 来源是obstacle_layer.cpp 中的 layered_costmap_->getGlobalFrameID();
    // 转换 global_frame_ 和 origin_frame坐标系
    tf_.waitForTransform(global_frame_,  local_origin.frame_id_, 
                         local_origin.stamp_,   ros::Duration(0.5) );
    // local_origin 转换到 global_frame_坐标系，变成global_origin
    tf_.transformPoint(global_frame_, local_origin, global_origin);
    observation_list_.front().origin_.x = global_origin.getX();
    observation_list_.front().origin_.y = global_origin.getY();
    observation_list_.front().origin_.z = global_origin.getZ();

    // pass on the raytrace/obstacle range of the observation buffer to the observations
    observation_list_.front().raytrace_range_ = raytrace_range_;
    observation_list_.front().obstacle_range_ = obstacle_range_;

    pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;

    // 把参数 cloud 转到global_frame_坐标系
    pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
    global_frame_cloud.header.stamp = cloud.header.stamp;

    // now we need to remove observations from the cloud that are 
    // below or above our height thresholds

    // 新的变量 observation_cloud，指向observation_list_的成员 cloud_
    pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
    unsigned int cloud_size = global_frame_cloud.points.size();
    observation_cloud.points.resize(cloud_size);
    unsigned int point_count = 0;

    // copy over the points that are within our height bounds
    // 从 global_frame_cloud中取出符合 z坐标的点，放到 observation_cloud
    for (unsigned int i = 0; i < cloud_size; ++i)
    {
      if (global_frame_cloud.points[i].z <= max_obstacle_height_
          && global_frame_cloud.points[i].z >= min_obstacle_height_)
      {
        observation_cloud.points[point_count++] = global_frame_cloud.points[i];
      }
    }

    // resize the cloud for the number of legal points
    observation_cloud.points.resize(point_count);
    // 时间戳和坐标系也复制过来
    observation_cloud.header.stamp = cloud.header.stamp;
    observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
  }
  catch (TransformException& ex)
  {
    // if an exception occurs, we need to remove the empty observation from the list
    observation_list_.pop_front();
    ROS_ERROR("TF Exception that should never happen for sensor frame: %s, 
      cloud frame: %s, %s",  sensor_frame_.c_str(),
              cloud.header.frame_id.c_str(),  ex.what());
    return;
  }
  // if the update was successful, we want to update the last updated time
  last_updated_ = ros::Time::now();

  // remove any stale observations from the list
  purgeStaleObservations();
}

// 定义在 MoveBase构造函数
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

// 回调函数，其实就是两个代价地图重置所有层
bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
{
    //clear the costmaps
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
    controller_costmap_ros_->resetLayers();

    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner( *(planner_costmap_ros_->getCostmap()->getMutex())  );
    planner_costmap_ros_->resetLayers();
    return true;
}

// 主要是 Costmap2D::resetMap 和 Layer::reset()
void Costmap2DROS::resetLayers()
{
	Costmap2D* top = layered_costmap_->getCostmap();
	top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());

	std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
	for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); 
				plugin != plugins->end();  ++plugin)
	{
      (*plugin)->reset();    //每一层覆盖Layer的虚函数reset()
	}
}

void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
	boost::unique_lock<mutex_t> lock(*(access_) );
	unsigned int len = xn - x0;
	for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
		memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

void InflationLayer::reset() { onInitialize(); }

void ObstacleLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

void StaticLayer::reset()
{
  if (first_map_only_)
  {
    has_updated_data_ = true;
  }
  else
  {
    onInitialize();
  }
}