AMCL源码阅读(1)——程序流程

概述

阅读AMCL源码，首先撇开算法具体细节以及技术细节，对ACML核心程序流程进行整理，看懂AMCL由外部数据触发到完成定位的整个过程。这里读的是navigation仓库melodic-devel分支，提交id为b52e0ea9362131dc81326da471792ab8698c9564的源码。

程序入口

先找到程序入口，也就是main函数，可以直接搜索main就可以很快定位到该函数了，主函数很短，如下

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "amcl");
  ros::NodeHandle nh;

  // Override default sigint handler
  signal(SIGINT, sigintHandler);

  // Make our node available to sigintHandler
  amcl_node_ptr.reset(new AmclNode());

  if (argc == 1)
  {
    // run using ROS input
    ros::spin();
  }
  else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
  {
    amcl_node_ptr->runFromBag(argv[2]);
  }

  // Without this, our boost locks are not shut down nicely
  amcl_node_ptr.reset();

  // To quote Morgan, Hooray!
  return(0);
}

可以看到，主函数中简单的初始化一下节点，初始化AmclNode类，然后根据命令参数的个数argc分别有两种执行方式，一种是直接接受传感器数据运行，一种是利用bag文件回放数据运行。我们跳到runFromBag函数中，可以看到利用bag文件数据文件运行的方式大概就是回放bag文件中的话题数据，不断读取并发布。所以，核心的流程还是一致的。

初始化

在主函数中可以看到，那里直接实例化AmclNode类后就进入ros::spin();。因此，从AmclNode构造函数开始看，可以看到，在构造函数中，AmclNode对很对成员变量进行初始化以及从参数服务器读取各个配置参数，然后注册几个话题的发布和订阅以及一些服务，大致就完成了。所以初始化完成之后，主要入口就是几个话题和服务了。

这里为了把握住主线，我们还是得找出核心的东西来，首先服务和发布的话题先不管，因为服务相对还是比较“被动”地调用，而发布的话题，则是对外的，也先不管。因此，就主要关注一下订阅的几个话题了，包括scan，initialpose和map。其实还有tf，不过tf主要就是读一下一些变化或者发布一下，主要在代码执行过程中需要的时候查询一下获取信息就完事了。

回调函数

经过上面的整理，我们的重点就落在了几个订阅话题的回调函数中了，也不多，也就仨。一看就可以看出来哪个是重头戏了，initialpose和map话题回调函数中其实还是设置算法参数，分别设置初始化姿态和栅格地图。到这里，核心的部分也就出来了，就是订阅雷达话题scan的回调函数laserReceived,这里将这段代码贴出来，在注释中说明到底干了啥！

void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
  // 首先获取当前激光雷达的frame_id
  std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
  // 记录当前时间
  last_laser_received_ts_ = ros::Time::now();
  if( map_ == NULL ) {
    return;
  }
  boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
  int laser_index = -1;

  // Do we have the base->base_laser Tx yet?
  // 查询一下该雷达之前是不是有记录过，否则说明就是新的雷达，也就是第一次收到该雷达数据
  if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())
  {
    // 如果当真是新的雷达，则进入到这里
    ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str());
    // 将雷达先用自定义的类封装一下并且保存到雷达列表，并且设置对应的更新标记，和在雷达列表中的索引，后面要用到
    lasers_.push_back(new AMCLLaser(*laser_));
    lasers_update_.push_back(true);
    laser_index = frame_to_laser_.size();

    // 接下来主要就是通过tf查询该雷达与base_frame之间的转换，即雷达相对于base_link的位姿
    geometry_msgs::PoseStamped ident;
    ident.header.frame_id = laser_scan_frame_id;
    ident.header.stamp = ros::Time();
    tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);

    geometry_msgs::PoseStamped laser_pose;
    try
    {
      this->tf_->transform(ident, laser_pose, base_frame_id_);
    }
    catch(tf2::TransformException& e)
    {
      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;
    }

    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;
    // 到这里查询完了该雷达相对于base_link的位姿了，就调用自定义封装激光的类进行设置这个参数，也就是保存下来
    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]);
    // 最后添加key-value到一个map中，该map作用是利用雷达的frame_id查询该雷达在列表中的索引，
    // 也就是key为雷达的frame_id，value为雷达在列表中的索引
    frame_to_laser_[laser_scan_frame_id] = laser_index;
    // 到这里完成了保存新的雷达信息了
  } else {
    // 如果该雷达在记录中存在，那么就获取该雷达索引
    // we have the laser pose, retrieve laser index
    laser_index = frame_to_laser_[laser_scan_frame_id];
  }

  // Where was the robot when this scan was taken?
  // 获取当前机器人位姿
  pf_vector_t pose;
  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;
  }


  pf_vector_t delta = pf_vector_zero();

  // 下来这部分就是根据是否已经初始化粒子滤波来设置每个雷达是否需要更新的标记
  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]);

    // 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;
    m_force_update=false;

    // Set the laser update flags
    // 在上面计算后，在两种情况下全部雷达需要设置更新标记
    // 一种是机器人运动运动距离超过设定的距离
    // 一种是设置了没有运动也强制更新
    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;

    // 在这里可以看到只有这个地方设置了pf_init_ = true
    // 也就是说该标记为只有在第一次收到雷达数据后才设置为true
    // 即第一次接收到雷达数据进入的是这里
    // 后面的在没有异常（重新加载地图、重新发布初始化位姿等等）的情况下将都不会进入这里
    // 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;
  }
  // If the robot has moved, update the filter
  // 如果粒子滤波器已经初始化，并且当前雷达标记为需要更新，则进入下面的代码块
  else if(pf_init_ && lasers_update_[laser_index])
  {
    // 这里面可以看到的是，利用UpdateAction更新粒子滤波器，确切的说就是用里程计预测位姿
    //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;
  }

  bool resampled = false;
  // If the robot has moved, update the filter
  // 如果当前雷达标记了更新，下面步骤就是进行更新步骤，也就是利用雷达数据更细粒子的权重
  if(lasers_update_[laser_index])
  {
    // 下面一大段大概时获取当前雷达的一个AMCLLaserData对象
    // 包括各种转换和判断等等
    AMCLLaserData ldata;
    ldata.sensor = lasers_[laser_index];
    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.
    //
    // Construct min and max angles of laser, in the base_link frame.
    tf2::Quaternion q;
    q.setRPY(0.0, 0.0, laser_scan->angle_min);
    geometry_msgs::QuaternionStamped min_q, inc_q;
    min_q.header.stamp = laser_scan->header.stamp;
    min_q.header.frame_id = stripSlash(laser_scan->header.frame_id);
    tf2::convert(q, min_q.quaternion);

    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);
    try
    {
      tf_->transform(min_q, min_q, base_frame_id_);
      tf_->transform(inc_q, inc_q, base_frame_id_);
    }
    catch(tf2::TransformException& e)
    {
      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]
    angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;

    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;
    // The AMCLLaserData destructor will free this memory
    ldata.ranges = new double[ldata.range_count][2];
    ROS_ASSERT(ldata.ranges);
    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
        ldata.ranges[i][0] = laser_scan->ranges[i];
      // Compute bearing
      ldata.ranges[i][1] = angle_min +
              (i * angle_increment);
    }
    // 到这里算是已经获得了当前雷达的AMCLLaserData对象了

    // 然后利用UpdateSensor进行更新粒子权重
    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);

    // 更新了权重之后，重新清空了当前雷达的更新标记
    lasers_update_[laser_index] = false;

    pf_odom_pose_ = pose;

    // Resample the particles
    // 完成了预测和更新后，就开始重要性采样了
    // 这里设置了重要性采样的周期，也就是每接收几次雷达数据采样一次
    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);

    // Publish the resulting cloud
    // TODO: set maximum rate for publishing
    // 这里就开始发布当前的全部粒子位姿
    if (!m_force_update)
    {
      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++)
      {
        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);
    }
  }

  // 如果已经采样，或者强制发布当前机器人位姿，则进入下面流程
  // 最后这里主要就是根据例子加权估计当前位姿并发布，以及发布一下odom到map的变换关系
  if(resampled || force_publication)
  {
    ... // 这里省略若干行
  }
  else if(latest_tf_valid_)
  {
    ... // 这里省略若干行
  }

  diagnosic_updater_.update();
}

上面就是Amcl执行的核心流程，可以说思路是很明确的，在各种情况判断后，依次进行odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);，lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);和pf_update_resample(pf_);，这三步就是粒子滤波三个核心步骤的接口，分别实现预测、更新和重要性采样算法。

参考

• wiki ROS Amcl