文件amcl_node.cpp最主要的几个函数是构造函数，handleMapMessage, laserReceived。尤其是最后一个

杂项

sigintHandler回调函数在关闭程序之前只有一句amcl_node_ptr->savePoseToServer();，也就是关闭前把最近一次的机器人位姿存入参数服务器，这就可以理解为什么关掉程序后，用rosparam get获得的initial_pose_x等位姿仍然是以前的。

amcl根据输入参数来决定运行方式。从代码中可以看到amcl支持数据回放的，运行时只需要通过参数--run-from-bag指定bag文件即可

从main函数可以看出，amcl的所有业务逻辑都是由AmclNode类的构造函数完成

AmclNode构造函数

通过参数服务器，根据launch文件里的大量参数，对各类变量赋值

成员函数updatePoseFromServer，该函数就是用来从参数服务器上获取机器人的初始位姿和位姿误差的协方差矩阵的。

发布amcl_pose和particlecloud两个主题，分别用于输出机器人位姿估计和粒子集合。

接着注册三个服务：global_localization用于获取机器人的全局定位，request_nomotion_update则用于手动的触发粒子更新并发布新的位姿估计，set_map用于设定机器人位姿和地图信息。 这几个平时都用不到

接下来构建激光传感器的消息过滤器对象和tf2的过滤器，并注册回调函数laserReceived。

laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(
                                  *laser_scan_sub_, 
                                  *tf_, 
                                  odom_frame_id_, 
                                  100 );
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                 this, _1));

这里的message_filter为ROS系统提供了一些通用的消息过滤方法， 它对接收到的消息进行缓存，只有当满足过滤条件后才输出，在需要消息同步的时候应用比较多。这里主要是同时监听激光扫描消息和里程计坐标变换，同步两者的输出。 这一套路在GMapping中也有用到。

订阅用于初始化机器人位姿估计的主题initialpose

根据运行参数use_map_topic_获取地图，或者订阅地图主题，或者通过requestMap请求static_map服务。因为一般取false，所以后面我们直接看requestMap函数，也就是如下逻辑：

定义m_force_update为false，后面会用到

最后定义一个计时器用于每隔15s检查一次激光雷达的接收数据，如果期间没有收到新的数据给出告警

requestMap 和 handleMapMessage

requestMap函数请求static_map服务成功后，调用函数handleMapMessage处理接收到的地图数据，进一步完成AmclNode的初始化工作，内容庞大的其实是handleMapMessage

handleMapMessage的流程图在这里

释放了与地图相关的内存和对象之后，通过函数convertMap将地图消息转换成amcl中的地图数据结构。

接着构建粒子滤波器对象，并完成初始化：

// 构建粒子滤波器对象
pf_ = pf_alloc(min_particles_,
               max_particles_,
               alpha_slow_,
               alpha_fast_,
               (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
               (void *)map_ );
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;

// 从参数服务器获取初始位姿及方差放到pf中
updatePoseFromServer();
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];   // initial_pose_x
pf_init_pose_mean.v[1] = init_pose_[1];   // initial_pose_y
pf_init_pose_mean.v[2] = init_pose_[2];   // initial_pose_a

pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];

// 初始化高斯粒子滤波器
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;

这里面有两个重要函数pf_alloc和pf_init

构建里程计对象，根据launch文件里的参数配置里程计模型

构建雷达传感器对象，根据运行参数laser_model_type_构建不同模型的雷达

最后考虑到，在接收到地图数据之前，有可能已经接收到了机器人的初始位姿，这里调用函数applyInitialPose处理

pf_alloc

// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
               double alpha_slow, double alpha_fast,
               pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
  int i, j;
  pf_t *pf;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  
  srand48(time(NULL));

  pf = calloc(1, sizeof(pf_t));

  pf->random_pose_fn = random_pose_fn;
  pf->random_pose_data = random_pose_data;

  pf->min_samples = min_samples;
  pf->max_samples = max_samples;

  // Control parameters for the population size calculation.  [err] is
  // the max error between the true distribution and the estimated
  // distribution.  [z] is the upper standard normal quantile for (1 -
  // p), where p is the probability that the error on the estimated
  // distrubition will be less than [err].
  pf->pop_err = 0.01;
  pf->pop_z = 3;
  pf->dist_threshold = 0.5; 
  pf->current_set = 0;
  for (j = 0; j < 2; j++)
  {
    set = pf->sets + j;

    set->sample_count = max_samples;
    set->samples = calloc(max_samples, sizeof(pf_sample_t));

    for (i = 0; i < set->sample_count; i++)
    {
      sample = set->samples + i;
      sample->pose.v[0] = 0.0;
      sample->pose.v[1] = 0.0;
      sample->pose.v[2] = 0.0;
      sample->weight = 1.0 / max_samples;
    }

    // HACK: is 3 times max_samples enough?
    set->kdtree = pf_kdtree_alloc(3 * max_samples);

    set->cluster_count = 0;
    set->cluster_max_count = max_samples;
    set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));

    set->mean = pf_vector_zero();
    set->cov = pf_matrix_zero();
  }

  pf->w_slow = 0.0;
  pf->w_fast = 0.0;
  pf->alpha_slow = alpha_slow;
  pf->alpha_fast = alpha_fast;

  //set converged to 0
  pf_init_converged(pf);
  return pf;
}

pf_init

// Initialize the filter using a guassian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  pf_pdf_gaussian_t *pdf;
  
  set = pf->sets + pf->current_set;
  
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set->kdtree);

  set->sample_count = pf->max_samples;

  pdf = pf_pdf_gaussian_alloc(mean, cov);
    
  // Compute the new sample poses
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = pf_pdf_gaussian_sample(pdf);

    // Add sample to histogram
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }

  pf->w_slow = pf->w_fast = 0.0;

  pf_pdf_gaussian_free(pdf);
    
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set); 

  //set converged to 0
  pf_init_converged(pf);

  return;
}