amcl定位在初始位姿的自动定位

 if(pf_init_)
   {
    // Compute change in pose
    //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
    delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
    delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
    bool update = fabs(delta.v[0]) > d_thresh_ ||
                  fabs(delta.v[1]) > d_thresh_ ||
                  fabs(delta.v[2]) > a_thresh_;
    update = update || m_force_update;
    m_force_update=false;
    if(update)
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;
  }
  bool force_publication = false;
  if(!pf_init_)
  {
    // Pose at last filter update
    pf_odom_pose_ = pose;
    // Filter is now initialized
    pf_init_ = true;
    // Should update sensor data
    for(unsigned int i=0; i < lasers_update_.size(); i++)
      lasers_update_[i] = true;
       force_publication = true;
       resample_count_ = 0;
         else if(pf_init_ && lasers_update_[laser_index])
  {
    //printf("pose\n");
    //pf_vector_fprintf(pose, stdout, "%.3f");
    AMCLOdomData odata;
    odata.pose = pose;
    // HACK
    // Modify the delta in the action data so the filter gets
    // updated correctly
    odata.delta = delta;
    // Use the action data to update the filter
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
    // Pose at last filter update
    //this->pf_odom_pose = pose;
  }
 }
   

ROS中的啊amcl包，2D的概率定位系统，采用自适应蒙特卡洛定位，这个方法是在已知地图中使用粒子滤波方法得到位姿的。输入激光雷达数据、里程计数据，输出机器人在地图中的位姿。
      从输入的topic来看，amcl订阅scan雷达数据，map地图数据，tf转换。当按照官网相关博主将amcl.launch配置好后，就可以实时定位了。但是在初始位姿时initialpose在RVIZ中设定好后，粒子在设定的位置周围分布，但是如果机器人不移动，amcl是不会更新其实时位姿。如果想在初始位姿就可以估计出相对准确的位姿可以修改amcl的源码，博主进行相关尝试，这里给大家参考。
      amcl源码中主要流程在amcl_node.cpp中，其中在AmclNode::laserReceived函数中有一个判断，if(pf_init_)，if(!pf_init_）和else if(pf_init_ && lasers_update_[laser_index])即如果产生位姿更新，才会去更新粒子发布实时位姿。

其中if(pf_init_)判断时候发生移动，所以可以将阈值d_thresh_改小甚至成为-1，这个值可以在launch文件中d_min值修改。这样机器人可实时发布位姿，即使没有运动。
这是比较简单粗暴的做法，但是会使粒子个数减少，会使后续移动中位姿估计发生错误，因为粒子太少了。那么我们可以只用这个程序做初始位姿估计，估计出初始位姿给到另一个amcl的initialpose定位，这样就可以在初始位姿就有相对准确的位姿了。
这是现在实现的做法，应该会有更好的处理方法，本人才疏学浅，希望可以多交流。