// ranges and angles inputs are vectors of the same length
scan = lidarScan(ranges,angles)
scan = lidarScan(cart)
scan = lidarScan(scanMsg) # 从ROS message中创建

minRange = 0.1;
maxRange = 7;
scan2 = removeInvalidData(scan,'RangeLimits',[minRange maxRange]);
hold on
plot(scan2)

if (!initialized_)
{
    ros::NodeHandle private_nh("~/" + name);
    costmap_ = costmap;
    frame_id_ = frame_id;   //costmap_ros->getGlobalFrameID()

    unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
    // 参数赋值, 默认false
    private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
    if(!old_navfn_behavior_)
        convert_offset_ = 0.5;
    else
        convert_offset_ = 0.0;

    bool use_quadratic;   // 二次方的
    private_nh.param("use_quadratic", use_quadratic, true);

    // p_calc_声明为PotentialCalculator指针
    // QuadraticCalculator是它的派生类, 唯一区别是覆盖了calculatePotential函数
    if (use_quadratic)    // 默认使用二次
        p_calc_ = new QuadraticCalculator(cx, cy);
    else
        p_calc_ = new PotentialCalculator(cx, cy);

    bool use_dijkstra;
    private_nh.param("use_dijkstra", use_dijkstra, true);
    //DijkstraExpansion 和 AStarExpansion 都继承 Expander类
    if (use_dijkstra)
    {
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
        if(!old_navfn_behavior_)
            de->setPreciseStart(true);
        planner_ = de;      // Expander* planner_;
    }
    else   // 如果不用dijkstra,就用A星算法.
        planner_ = new AStarExpansion(p_calc_, cx, cy);

    bool use_grid_path;
    private_nh.param("use_grid_path", use_grid_path, false);
    // GradientPath 和 GridPath都继承了Traceback
    if (use_grid_path)
        path_maker_ = new GridPath(p_calc_);
    else
        path_maker_ = new GradientPath(p_calc_);
        
    orientation_filter_ = new OrientationFilter();
    // NavfnROS::initialize中也注册此话题
    plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
    potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
    // 一系列参数赋值
    private_nh.param("allow_unknown", allow_unknown_, true);
    planner_->setHasUnknown(allow_unknown_);
    private_nh.param("planner_window_x", planner_window_x_, 0.0);
    private_nh.param("planner_window_y", planner_window_y_, 0.0);
    private_nh.param("default_tolerance", default_tolerance_, 0.0);
    private_nh.param("publish_scale", publish_scale_, 100);

    double costmap_pub_freq;
    private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);

    //get the tf prefix
    ros::NodeHandle prefix_nh;
    tf_prefix_ = tf::getPrefixParam(prefix_nh);

    make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

    dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
    dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
            &GlobalPlanner::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);

    initialized_ = true;
} else
    ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");

// 粒子重采样
if(!(++resample_count_ % resample_interval_))
{
  pf_update_resample(pf_);
  resampled = true;
}

pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);

// 发布话题particlecloud, 发布的是最终结果cloud，在机器人移动后执行
if (!m_force_update)  //在运动模型那里赋值false
{
  geometry_msgs::PoseArray cloud_msg;
  cloud_msg.header.stamp = ros::Time::now();
  cloud_msg.header.frame_id = global_frame_id_;
  cloud_msg.poses.resize(set->sample_count);
  for(int i=0;i<set->sample_count;i++)  // 粒子个数
  {
    // tf::Pose转为geometry_msgs::Pose，就是用于后面发布消息
    tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
                             tf::Vector3(set->samples[i].pose.v[0],
                                         set->samples[i].pose.v[1], 0) ),
                    cloud_msg.poses[i]);
  }
    // 构造函数中注册了话题
    particlecloud_pub_.publish(cloud_msg);
}

// 刚启动amcl时不执行,机器人移动后执行
if(resampled || force_publication)
{
    // Read out the current hypotheses
    double max_weight = 0.0;
    int max_weight_hyp = -1;
    std::vector<amcl_hyp_t> hyps;
    hyps.resize(pf_->sets[pf_->current_set].cluster_count);
    for(int hyp_count = 0; hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
    {
        double weight;
        pf_vector_t pose_mean;
        pf_matrix_t pose_cov;
        if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
        {
            ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
            break;
        }
        
        hyps[hyp_count].weight = weight;
        hyps[hyp_count].pf_pose_mean = pose_mean;
        hyps[hyp_count].pf_pose_cov = pose_cov;
        
        if(hyps[hyp_count].weight > max_weight)
        {
            max_weight = hyps[hyp_count].weight;
            max_weight_hyp = hyp_count;
        }
    }
    
    if(max_weight > 0.0)
    {
        ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                  hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2] );
        geometry_msgs::PoseWithCovarianceStamped p;
        // Fill in the header
        p.header.frame_id = global_frame_id_;
        p.header.stamp = laser_scan->header.stamp;
        // Copy in the pose
        p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
        p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                p.pose.pose.orientation);
        // Copy in the covariance, converting from 3-D to 6-D
        pf_sample_set_t* set = pf_->sets + pf_->current_set;
        for(int i=0; i<2; i++)
        {
            for(int j=0; j<2; j++)
            {
                // Report the overall filter covariance, rather than the
                // covariance for the highest-weight cluster
                //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
                p.pose.covariance[6*i+j] = set->cov.m[i][j];
            }
        }
        // Report the overall filter covariance, rather than the
        // covariance for the highest-weight cluster
        //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
        p.pose.covariance[6*5+5] = set->cov.m[2][2];
        
        pose_pub_.publish(p);
        last_published_pose = p;
        
        ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
                  hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2]);
        
        // subtracting base to odom from map to base and send map to odom instead
        tf::Stamped<tf::Pose> odom_to_map;
        try
        {
            tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                    tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                    hyps[max_weight_hyp].pf_pose_mean.v[1],
                    0.0));
            tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                                  laser_scan->header.stamp,
                                                  base_frame_id_);
            this->tf_->transformPose(odom_frame_id_,
                                     tmp_tf_stamped,
                                     odom_to_map);
        }
        catch(tf::TransformException)
        {
            ROS_DEBUG("Failed to subtract base to odom transform");
            return;
        }
        latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                   tf::Point(odom_to_map.getOrigin()));
        latest_tf_valid_ = true;
        if (tf_broadcast_ == true)
        {
            // We want to send a transform that is good up until a
            // tolerance time so that odom can be used
            ros::Time transform_expiration = (laser_scan->header.stamp +
                                              transform_tolerance_);
            tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                                transform_expiration,
                                                global_frame_id_, odom_frame_id_);
            this->tfb_->sendTransform(tmp_tf_stamped);
            sent_first_transform_ = true;
        }
    }
    else
    {
        ROS_ERROR("No pose!");
    }
}

// Resample 粒子分布
void pf_update_resample(pf_t *pf)
{
  int i;
  double total;
  pf_sample_set_t *set_a, *set_b;
  pf_sample_t *sample_a, *sample_b;

  double* c;
  double w_diff;
  // 用来接收当前粒子集的信息
  set_a = pf->sets + pf->current_set;
  // 保存重采样后的粒子
  set_b = pf->sets + (pf->current_set + 1) % 2;

  // Build up cumulative probability table for resampling.
  // TODO: Replace this with a more efficient procedure  (e.g., GeneralDiscreteDistributions.html)
  // 对粒子的权重进行积分，获得分布函数，后面用于重采样
  c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
  c[0] = 0.0;
  for(i=0;i<set_a->sample_count;i++)
    c[i+1] = c[i] + set_a->samples[i].weight;
	// c[1] = 第一个粒子权重
	// c[2] = 第一和第二粒子权重之和，以此类推 c[i] 是粒子集的 前i个粒子的权重之和
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set_b->kdtree);
  
  // Draw samples from set a to create set b.
  total = 0;
  set_b->sample_count = 0;
  // 表示注入粒子的概率
  w_diff = 1.0 - pf->w_fast / pf->w_slow;
  if(w_diff < 0.0)
    w_diff = 0.0;

  // Can't (easily) combine low-variance sampler with KLD adaptive
  // sampling, so we'll take the more traditional route.
  /*
  // Low-variance resampler, taken from Probabilistic Robotics, p110
  count_inv = 1.0/set_a->sample_count;
  r = drand48() * count_inv;
  c = set_a->samples[0].weight;
  i = 0;
  m = 0;
  */
	// 确保重采样生成的粒子集（set_b）的粒子数不超过规定的最大的粒子数
  while(set_b->sample_count < pf->max_samples)
  {
    sample_b = set_b->samples + set_b->sample_count++;
    // 产生的随机数小于w_diff时，将往set_b中随机注入粒子
    if(drand48() < w_diff)
    	// pf->random_pose_fn 为一个函数指针，其返回一个随机位姿
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
    else
    {
      // Can't (easily) combine low-variance sampler with KLD adaptive
      // sampling, so we'll take the more traditional route.
      /*
      // Low-variance resampler, taken from Probabilistic Robotics, p110
      U = r + m * count_inv;
      while(U>c)
      {
        i++;
        // Handle wrap-around by resetting counters and picking a new random
        // number
        if(i >= set_a->sample_count)
        {
          r = drand48() * count_inv;
          c = set_a->samples[0].weight;
          i = 0;
          m = 0;
          U = r + m * count_inv;
          continue;
        }
        c += set_a->samples[i].weight;
      }
      m++;
      */

      // Naive discrete event sampler
      double r = drand48();  // drand48 返回服从均匀分布的[0.0, 1.0)之间的 double 随机数
      // c[i]相当于把粒子权重以数轴距离的形式表现，查看r容易落在数轴哪个位置。
      // 当set_a的第i个粒子权重很大时，r落在c[i]与c[i+1]之间的概率就很高，找符合条件的i
      // 虽然这里不能保证每次都能从set_a中提取权重最大的粒子，因为r是一个随机数。但是由于粒子数量较大，
      // 因此提取的大多数粒子权重还是高的，这是一个统计学的方法
      for(i=0; i<set_a->sample_count; i++)
      {
        if((c[i] <= r) && (r < c[i+1]))
          break;
      }
      assert(i<set_a->sample_count);

      sample_a = set_a->samples + i;
      assert(sample_a->weight > 0);
      // 从set_a中挑选粒子赋给一个粒子，之后放进set_b
      sample_b->pose = sample_a->pose;
    }
    sample_b->weight = 1.0;
    // total之前是0
    total += sample_b->weight;
    // Add sample to histogram
    pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
    // 大的粒子数在定位初期能提高定位精度，但是当粒子集开始聚合后，程序便不再需要这么多的粒子
    //  因此这里需要引入一个新的控制条件来节省计算资源
    // pf_resample_limit根据时间与粒子集测量更新的结果，返回粒子集所需要的最佳粒子数量，
    // 从而实现了粒子数量随着时间的变化而自我调整
    if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
      break;
  }
  // 重置，避免w_diff越来越大，导致大量插入随机位姿的粒子 
  if(w_diff > 0.0)
    pf->w_slow = pf->w_fast = 0.0;

  // 将set_b的新粒子插入Kd-Tree中并且对粒子的权重归一化，为了之后的聚类分析做准备
  for (i = 0; i < set_b->sample_count; i++)
  {
    sample_b = set_b->samples + i;
    sample_b->weight /= total;   // 粒子集b的粒子权重都是 1/total
  }
  // 重新聚类分析
  pf_cluster_stats(pf, set_b);
  // 将set_b设为当前粒子集，参与下一次的更新循环
  // 此时的set_b就对应rviz显示的粒子云
  pf->current_set = (pf->current_set + 1) % 2; 

  pf_update_converged(pf);
  free(c);
  return;
}