(笔记)（9）AMCL包源码分析 | amcl_node.cpp (二)

AMCL定位算法主要是适用于激光传感器，所以对激光数据的处理是比较重要的一环。这一讲，我们将对amcl_node.cpp的laserReceived函数进行分析。

图1.laserReceived函数总览

从主要条件分支可以看出，确认了map是否已接收，是否有base_laser->base_link的转换关系，当激光被获取时机器人的位置在何处。以及考虑了粒子滤波器有无更新的情况，考虑激光更新的情况，如果是重采样或者是强制发布，那又如何处理。

//首先定义激光的坐标系id，从laser_scan的header那里获得frame_id
std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
last_laser_received_ts_ = ros::Time::now();

1.地图为空的情况

如果地图为空，那么就不需要往下进行了。

 if( map_ == NULL ) {
    return;
  }

这里加入一个互斥锁，以及初始化laser_index的序号：

boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
int laser_index = -1;

2.确认base_laser->base的坐标系转换关系

先来一个条件判定：

if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())

是的话，进入下面的代码：

  //对激光传感器laser_,激光传感器数据更新lasers_update_,激光射线laser_index进行初始化
    lasers_.push_back(new AMCLLaser(*laser_));
    lasers_update_.push_back(true);
    laser_index = frame_to_laser_.size();
  
  //定义一个新点ident
    geometry_msgs::PoseStamped ident;
  //定义点ident的坐标系为激光坐标系
    ident.header.frame_id = laser_scan_frame_id;
    ident.header.stamp = ros::Time();
  //初始化ident的位姿
    tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
  
   //定义一个新点 laser_pose
    geometry_msgs::PoseStamped laser_pose;
   //读取ident(激光坐标系下的点)，目标点：laser_pose(？坐标系下的点)，base_frame_id(base_link坐标系)
   //坐标转换关系
    try
    {
      this->tf_->transform(ident, laser_pose, base_frame_id_);
    }
    catch(tf2::TransformException& e)
    {
      //如果捕捉到异常会报错，报错内容为：不能从激光坐标系变换到base_link坐标系
      ROS_ERROR("Couldn't transform from %s to %s, "
                "even though the message notifier is in use",
                laser_scan_frame_id.c_str(),
                base_frame_id_.c_str());
      return;
    }
    
   //新建一个位姿laser_pose_v，来接收laser_pose
    pf_vector_t laser_pose_v;
    laser_pose_v.v[0] = laser_pose.pose.position.x;
    laser_pose_v.v[1] = laser_pose.pose.position.y;
    // laser mounting angle gets computed later -> set to 0 here!
    laser_pose_v.v[2] = 0;
    //将这个位姿用SetLaserPose函数保存到laser_激光传感器容器中
    lasers_[laser_index]->SetLaserPose(laser_pose_v);
    //这里可以打印出激光扫描到点的位置
    ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
              laser_pose_v.v[0],
              laser_pose_v.v[1],
              laser_pose_v.v[2]);
    //将laser_index赋给frame_to_laser_
    frame_to_laser_[laser_scan_frame_id] = laser_index;

否则的话，就说明我们已经有了激光的位姿，retrieve激光的序号index:

// we have the laser pose, retrieve laser index
    laser_index = frame_to_laser_[laser_scan_frame_id];

3.确认激光被获取时激光与机器人的位姿有关联

// Where was the robot when this scan was taken? 当激光被获取的时候机器人在哪里？
//先定义一个位姿pose
  pf_vector_t pose;
//进入getOdomPose函数进行判断，输入的是latest_odom_pose_,新建的位姿pose,
//激光的header.stamp，base_link坐标系
  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                  laser_scan->header.stamp, base_frame_id_))
  {
   //报错：无法决定机器人的位置当获取激光时
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
  }
//再定义一个里程计运动的差值delta
pf_vector_t delta = pf_vector_zero();
//这里定义的pose,delta均进入下面的粒子滤波器是否初始化的条件下处理

4.当粒子滤波器初始化标志符为真

//如果pf_init_为true
  if(pf_init_)
  {
    // Compute change in pose 好好计算位姿的改变
    //delta=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]; 
    //使用angle_diff函数求出角度的改变
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

    //看是否要更新粒子滤波器 跟d_thresh 和a_thresh比较
    // See if we should update the filter
    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;
   //将强制更新设置为false
    m_force_update=false;

    // Set the laser update flags
    if(update) //如果更新
     //遍历laser_update_,将laser_update_中的每个元素置为true
      for(unsigned int i=0; i < lasers_update_.size(); i++) 
        lasers_update_[i] = true;
  }
  //设置强制发布为false
  bool force_publication = false;

5.当粒子滤波器初始化标志符为假

//如果pf_init_为false
  if(!pf_init_)
  {
    //取出在最后一个粒子滤波器更新时的位姿，将这个位姿赋给pf_odom_pose_
    // Pose at last filter update
    pf_odom_pose_ = pose;

    // Filter is now initialized 滤波器现在已更新
    pf_init_ = true;//将pf_init_置为true

    // Should update sensor data 应该更新sensor数据
   //遍历laser_update_,将laser_update_中的每个元素置为true
    for(unsigned int i=0; i < lasers_update_.size(); i++) 
      lasers_update_[i] = true;
    //设置强制发布为true
    force_publication = true;
    //重采样数目为0
    resample_count_ = 0;
  }

6.如果粒子滤波器初始化标识符与激光传感器更新符同时为真

这里主要是设置运动模型数据odata去更新粒子滤波器。

 //如果机器人已经动了，那就更新粒子滤波器
// If the robot has moved, update the filter
//如果pf_init_ && laser_update_[laser_index] 同时为真
  else if(pf_init_ && lasers_update_[laser_index]) 
  {
    //printf("pose\n");
    //pf_vector_fprintf(pose, stdout, "%.3f");

    //建立里程计运动模型数据 
    AMCLOdomData odata;
    //将pose的值赋给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);
  }
  //将已重采样标志resampled置为false；因为还没有重采样呐
  bool resampled = false;

7.激光传感器更新符为真

激光的数据data从激光传感器实际扫描到的距离数组送入观测模型，进行粒子滤波器的更新，进一步进行重采样，重采样完了，将粒子滤波器的粒子sample集取出来，作为粒子云发布出去。

// If the robot has moved, update the filter 如果机器人动了，那就更新粒子滤波器
  if(lasers_update_[laser_index]) //如果laser_update_[laser_index]为true
  {
    AMCLLaserData ldata;//创建ldata
    
    ldata.sensor = lasers_[laser_index];//用laser[laser_index]填充ldata.sensor
    //laser_scan的ranges尺寸等于ldata的range_count
    ldata.range_count = laser_scan->ranges.size();

    // To account for lasers that are mounted upside-down, we determine the
    // min最小, max最大, and increment增加角度 angles of the laser in the
    // base frame base坐标系.
  
    // Construct min and max angles of laser, in the base_link frame.
    //定义四元数q
    tf2::Quaternion q;
    //q用于接收laser_scan的最小角度
    q.setRPY(0.0, 0.0, laser_scan->angle_min);//接收来自laser_scan的
    geometry_msgs::QuaternionStamped min_q, inc_q;
    min_q.header.stamp = laser_scan->header.stamp;//接收来自laser_scan的
    min_q.header.frame_id = stripSlash(laser_scan->header.frame_id);
    tf2::convert(q, min_q.quaternion);//将q转换成min_q
    
    //q接收来自laser_scan的最小角度+角分辨率
    q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
    inc_q.header = min_q.header;
    tf2::convert(q, inc_q.quaternion);//将新q转换成inc_q
     //min_q激光坐标系，base_frame_id（base_link坐标系）
    try
    {
      tf_->transform(min_q, min_q, base_frame_id_);
      tf_->transform(inc_q, inc_q, base_frame_id_);
    }
    catch(tf2::TransformException& e)
    {
      //报错：不能将最小/最大角度转换到base_link坐标系中
      ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
               e.what());
      return;
    }
    //定义最小角度
    double angle_min = tf2::getYaw(min_q.quaternion);
    //定义角分辨率
    double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min;
    // wrapping angle to [-pi .. pi] 约束角度在[-pi,pi]之间
    angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
    //这里报告激光多少个角度在base_link坐标系下，最小角度为 ，角分辨率为 
    ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
    //设置激光探测范围最小/最大阈值
    // Apply range min/max thresholds, if the user supplied them
    if(laser_max_range_ > 0.0)
      ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
    else
      ldata.range_max = laser_scan->range_max;
    double range_min;
    if(laser_min_range_ > 0.0)
      range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
    else
      range_min = laser_scan->range_min;

    //AMCLLaserData析构函数将会释放内存
    // The AMCLLaserData destructor will free this memory 
    //为观测模型数据ldata存储探测范围的数组分配内存
    ldata.ranges = new double[ldata.range_count][2];
    ROS_ASSERT(ldata.ranges);
    //遍历ldata的探测范围的数组
    for(int i=0;i<ldata.range_count;i++)
    {
      // amcl doesn't (yet) have a concept of min range.  So we'll map short
      // readings to max range.
      if(laser_scan->ranges[i] <= range_min)
        ldata.ranges[i][0] = ldata.range_max;
      else
        //将实际laser_scan探测的距离数组一一赋值给观测模型数据ldata
        ldata.ranges[i][0] = laser_scan->ranges[i];
      // Compute bearing 计算探测的距离对应的角度
      ldata.ranges[i][1] = angle_min +
              (i * angle_increment);
    }
    //使用观测模型的ldata更新粒子滤波器
    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
    //将lasers_update_的更新符号置为false
    lasers_update_[laser_index] = false;
    
    //将pose的值赋给pf_odom_pose_
    pf_odom_pose_ = pose;

    // Resample the particles 重采样粒子
    if(!(++resample_count_ % resample_interval_))
    {
      pf_update_resample(pf_);//这里使用的是pf_update_resample(pf_);也就是KLD采样算法
      //将已重采样标识符置为true
      resampled = true;
    }
   //新建粒子sample集set，来自于pf_
    pf_sample_set_t* set = pf_->sets + pf_->current_set;
    ROS_DEBUG("Num samples: %d\n", set->sample_count);

    // Publish the resulting cloud 发布粒子云
    // TODO: set maximum rate for publishing
    if (!m_force_update) //如果强制更新为false
    {
      geometry_msgs::PoseArray cloud_msg;//新建cloud_msg
      cloud_msg.header.stamp = ros::Time::now();
      cloud_msg.header.frame_id = global_frame_id_;
     //根据粒子sample集的sample数目 resize cloud_msg的尺寸
      cloud_msg.poses.resize(set->sample_count);
      for(int i=0;i<set->sample_count;i++) //遍历粒子sample集set
      {
        cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
        cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
        cloud_msg.poses[i].position.z = 0;
        tf2::Quaternion q;
        q.setRPY(0, 0, set->samples[i].pose.v[2]);
        tf2::convert(q, cloud_msg.poses[i].orientation);
      }
     //发布粒子云
      particlecloud_pub_.publish(cloud_msg);
    }
  }

8.如果已经重采样或者强制发布

已经重采样后或者是强制发布，这时候就要发布AMCL包经历粒子滤波算法估计出的世界坐标系下的机器人位姿p。如何选出呢？选出拥有最大权重的粒子sample簇，并将这个簇的均值赋给位姿p的位姿x,y,yaw，将粒子sample集的协方差赋给位姿p的协方差。

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;//设置位姿假设
    //根据pf_当前set的簇数目：cluster_count
    hyps.resize(pf_->sets[pf_->current_set].cluster_count);
    for(int hyp_count = 0;
       //这里要遍历pf_当前set的cluster_count
        hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++) 
    {
      double weight;
      pf_vector_t pose_mean;
      pf_matrix_t pose_cov;
      //好好注意pf_get_cluster_stats函数，在pf.cpp里
      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;//得到拥有最大权重的cluster
      }
    }

    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]);

      /*
         puts("");
         pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
         puts("");
       */

      geometry_msgs::PoseWithCovarianceStamped p;//设置位姿 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];

      tf2::Quaternion q;
      q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
      tf2::convert(q, p.pose.pose.orientation);
      // Copy in the covariance, converting from 3-D to 6-D
     //从粒子滤波器中取出粒子sample集，再新建一个粒子sample集
      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];//将粒子sample集的协方差取出来
        }
      }
      // 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];//将粒子sample集的协方差取出来

      pose_pub_.publish(p);//把位姿p发布出去
      last_published_pose = p;//并把发布出去的这个位姿p作为last_published_pose

      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]);
      //注意tf转换关系
      // subtracting base to odom(odom_base) from map to base(base_map) and send map to odom（odom_map） instead 
      geometry_msgs::PoseStamped odom_to_map;
      try
      {
        tf2::Quaternion q;
        q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
        tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                              hyps[max_weight_hyp].pf_pose_mean.v[1],
                                              0.0));//世界坐标系下的

        geometry_msgs::PoseStamped tmp_tf_stamped;//base_link坐标系下的
        tmp_tf_stamped.header.frame_id = base_frame_id_;
        tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
        tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);

        this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
      }
      catch(tf2::TransformException)
      {
        //报错：将base->odom的转换失败！
        ROS_DEBUG("Failed to subtract base to odom transform");
        return;
      }

      tf2::convert(odom_to_map.pose, latest_tf_);
      //这里将latst_tf_valid置为true
      latest_tf_valid_ = true;
     //如果tf_broadcast为真
      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_);
        geometry_msgs::TransformStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = global_frame_id_;//父坐标系：map坐标系
        tmp_tf_stamped.header.stamp = transform_expiration;
        tmp_tf_stamped.child_frame_id = odom_frame_id_;//子坐标系:odom坐标系
        tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
        //发布odom->map的转换关系，由tmp_tf_stamped表示
        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }
    }
    else
    {
      ROS_ERROR("No pose!");
    }
  }

9.如果最新的tf存在的话

else if(latest_tf_valid_)
  {
    if (tf_broadcast_ == true)
    {
      //什么也没改变，只是再次发布最后一次转换
      // Nothing changed, so we'll just republish the last transform, to keep
      // everybody happy.
      ros::Time transform_expiration = (laser_scan->header.stamp +
                                        transform_tolerance_);
      geometry_msgs::TransformStamped tmp_tf_stamped;
      tmp_tf_stamped.header.frame_id = global_frame_id_;//世界坐标系map
      tmp_tf_stamped.header.stamp = transform_expiration;
      tmp_tf_stamped.child_frame_id = odom_frame_id_;//里程计坐标系
      tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
      this->tfb_->sendTransform(tmp_tf_stamped);
    }
    //将最后的位姿存放到Server中，具体使用savePoseToSever函数，this表示AmclNode类实例化的对象
    // Is it time to save our last pose to the param server
    ros::Time now = ros::Time::now();
    if((save_pose_period.toSec() > 0.0) &&
       (now - save_pose_last_time) >= save_pose_period)
    {
      this->savePoseToServer();
      save_pose_last_time = now;
    }
  }

10.diagnosic_update_更新

diagnosic_updater_.update();

至此，laserReceived函数的源码分析完毕了。激光传感器观测到的数据/里程计模型数据对粒子滤波器洗洗刷刷，选出最大权重的粒子簇，发布唯一的位姿p；经历过重采样后的粒子滤波器，取出其粒子sample集作为粒子云发布；tf2作为坐标系管理工具从头服务到尾，积极算出odom到map的转换，从而维护了完整的tf树：laser->base_link->odom->map.

 

转自：9.AMCL包源码分析 | amcl_node.cpp (二) - 知乎 (zhihu.com)