(笔记)（8）AMCL包源码分析 | amcl_node.cpp (一) - tdyizhen1314

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

关于amcl_node.cpp，主要作用是发布一个位姿p，来表征这个粒子滤波算法对机器人当前位置的估计。这个估计的位姿，是机器人坐标系(base_link)在世界坐标系(map)下的具体坐标呈现。

另一方面呢，amcl_node.cpp也要接收外界给定的位姿来进行粒子滤波器的初始化，通俗一点讲就是将位姿初始化，初始化之后，amcl包就正式开始工作了，实时上报一个机器人位置的估计值p（位姿），供后续的导航模块作用。

1.amcl_node.cpp的功能

梳理来看， amcl_node.cpp需要做的事情有：

及时处理激光传感器扫描到障碍物后返回的数据data，并将这个数据data传入到观测模型，以用来更新粒子滤波器。
及时处理里程计的位姿data,并将这个数据data传入到运动模型，以用来更新粒子滤波器。
要建立坐标系之间连续转换关系,AMCL包采用ROS自带的tf2库来处理相关坐标变换关系。

激光传感器所在的坐标系(laser_link)到机器人所在的坐标系(base_link)是什么转换关系。（坐标系之间的转换：laser_link->base_link）
激光传感器扫描到的障碍物点，只会返回一个距离和角度，那么如何将这个障碍物点的坐标转换到机器人坐标系(base_link)下的坐标。（A坐标系的点要转换到B坐标系下去）
里程计所在的坐标系(odom)与机器人坐标系(base_link)的关系是如何定义的呢，是不是从简单的运动模型就可以推导出它俩的相关关系？(坐标系的转换:base_link->odom)
粒子滤波算法提供的是一个世界坐标系(map)下的机器人坐标系(base_link)的坐标。(已知A坐标系在B坐标系下的坐标)

图1.AMCL包坐标系转换示意图，摘自amcl-ROS wiki

从图1来看，base_link->map已经明了，base_link->odom已经明了，那么odom->map又该如何求出呢？关于base_link，odom，map相关术语的解释：

Coordinate Frameswww.ros.org/reps/rep-0120.html

https://www.ros.org/reps/rep-0105.htmlwww.ros.org/reps/rep-0105.html

4.如何接收新的位姿以及接收新的位姿之后怎么处理。

5.怎么选出要发布的位姿呢？粒子云又是如何发布。

带着上述的问题，我们接下来对ROS自带的坐标/坐标系管理工具tf2进行深入分析，深度讨论粒子滤波器接收外界给定的位姿后该如何处理，探究AMCL包如何选出最合适的位姿发布，以及如何发布粒子云的机制。

2.管理坐标系变换关系的好工具:tf2介绍

先来个ros官方的tf2介绍铺垫铺垫：

http://wiki.ros.org/tf2wiki.ros.org/tf2

要问怎么学会tf2，我也是看官网的Tutorials，有意思的是Tutorials的后两个比较有趣：teaches you to wait for a transform to be available on the tf2 tree when using the lookupTransform() function && teaches you about advanced time travel features of tf2.

http://wiki.ros.org/tf2/Tutorialswiki.ros.org/tf2/Tutorials

关于tf2的相关函数，简单的可会了，比如说：

tf2/Tutorials/Migration/DataConversions - ROS Wikiwiki.ros.org/tf2/Tutorials/Migration/DataConversions

但是源码里怎么那么多关于tf2的陌生的函数？请到这里查询：

tf2: Main Pagedocs.ros.org/en/latest/api/tf2/html/

好家伙，回到AMCL包的amcl_node.cpp，看它引用的关于tf2的相关头文件：

图1.引用的tf2相关的头文件

再对比ros官网的tf2介绍截图，这里可以看到有tf2_ros,tf2_geometry_msgs可供我们查询。

图2.tf2所包含的各种类型

当然tf2_ros的相关函数的靠谱的查询，跳转至这里:

http://docs.ros.org/en/latest/api/tf2_ros/html/c++/index.htmldocs.ros.org/en/latest/api/tf2_ros/html/c++/index.html

tf2_msgs又到哪里查呢？真是个问题。

这里主要声明：

图3.tf2相关术语

其它关于tf常见的广播和监听功能:

tf(Transform Frame)变换blog.csdn.net/weixin_38145317/article/details/83956463

关于tf的广播功能：

图4.tf2的广播含义

2.关于tf的监听功能：

lookupTransform（）函数功能：可以获得两个坐标系之间转换的关系，包括旋转和平移。
transformPoint()函数的功能:将一个坐标系下的点的坐标转换到另一个坐标系下。

3.AMCL包的坐标系关系如何管理

这里有一篇博文讲解odom->map的坐标系转换原理比较详细，贴上来：

2D激光SLAM：：AMCL发布的odom----map坐标TF变换解读_dieju8330的博客-CSDN博客blog.csdn.net/dieju8330/article/details/96770964

而在amcl_node.cpp中具体代码实现,在laserReceived函数 if(resampled || forcepublication) 条件下表达。这里贴出的图3是根据上面引用的博主博客的内容，值的注意的是AMCL主要解决的事情--估计出机器人真实的位姿(世界坐标系下)，我们在前面的AMCL系列讲的比较清晰了。

图5.AMCL包tf树实现逻辑

      ....
     //发布位姿p，即AMCL包粒子滤波算法估计出的在世界坐标系下(map坐标系)机器人所在的位置
      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
      // map->odom = map->base_link - base_link->odom
      geometry_msgs::PoseStamped odom_to_map;
      try
      {
        tf2::Quaternion q;
        q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
        //tmp_tf是base_link在global map下的坐标，即base-->map
        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;
        tmp_tf_stamped.header.frame_id = base_frame_id_;
        tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
        //tmp_tf.inverse()是输入，tmp_tf_stamped.pose是输出
        //tmp_tf_stamped是global map原点在base_link下的坐标，即map-->base
        tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose);
 
        //odom_frame_id_ default value is "odom"
        //将global map原点在base_link下的坐标变换成global map原点在odom下的坐标
        //即map-->odom，（相当于在odom原点看map原点的位置）
        //这里的odom_to_map并非真的odom-->map，而是反过来map-->odom
        this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_);
      }
      catch(tf2::TransformException)
      {
        ROS_DEBUG("Failed to subtract base to odom transform");
        return;
      }
 
      //转换odom_to_map.pose为latest_tf_
      tf2::convert(odom_to_map.pose, latest_tf_);
      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_);
        //设置tmp_tf_stamped头部信息
        geometry_msgs::TransformStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = global_frame_id_;
        tmp_tf_stamped.header.stamp = transform_expiration;
        //这个变换就是child_frame_id在parent_frame_id下的坐标
        tmp_tf_stamped.child_frame_id = odom_frame_id_;
        //tmp_tf_stamped这个变换是odom原点在map坐标系的坐标，即odom-->map
        tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);
 
        //发布
        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }
    }

4.AMCL包发布最合适的位姿(the only one pose)

在amcl_node.cpp中的laserReceived函数,if(resampled || forcepublication) 条件下表达：

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;
      }
      //读出每个粒子sample簇（cluster）的权重，均值，协方差；
      //选出其中权重最大的粒子簇；接下来将它均值和协方差初始化要发布的位姿
      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]);

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

      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 利用最大权重的粒子簇cluster的均值和协方差初始化位姿p
      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
      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];

      /*
         printf("cov:\n");
         for(int i=0; i<6; i++)
         {
         for(int j=0; j<6; j++)
         printf("%6.3f ", p.covariance[6*i+j]);
         puts("");
         }
       */
      //然后将p发布
      pose_pub_.publish(p);
      //并将p赋值给last_published_pose,后面有妙用
      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]);

5.AMCL包发布粒子云(particle cloud)

在amcl_node.cpp中的laserReceived函数,if(lasers_update_[laser_index])条件下表达：

// If the robot has moved, update the filter如果机器人已经动了，更新粒子滤波器
//这里主要处理激光数据
  if(lasers_update_[laser_index])
  {
    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);
    }
     //使用激光数据更新粒子滤波器
    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);
    }
  }

6.粒子滤波器接收外界给定的位姿后的处理

处理初始位姿的函数：handleInitialPoseMessage()

void
AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg)
{
  boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
  if(msg.header.frame_id == "")
  {
    // This should be removed at some point
    ROS_WARN("Received initial pose with empty frame_id.  You should always supply a frame_id.");
  }
  // We only accept initial pose estimates in the global frame, #5148.
  else if(stripSlash(msg.header.frame_id) != global_frame_id_)
  {
    ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
             stripSlash(msg.header.frame_id).c_str(),
             global_frame_id_.c_str());
    return;
  }

  // In case the client sent us a pose estimate in the past, integrate the
  // intervening odometric change.
  geometry_msgs::TransformStamped tx_odom;
  try
  {
    ros::Time now = ros::Time::now();
    // wait a little for the latest tf to become available
    tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,
                                   base_frame_id_, ros::Time::now(),
                                   odom_frame_id_, ros::Duration(0.5));
  }
  catch(tf2::TransformException e)
  {
    // If we've never sent a transform, then this is normal, because the
    // global_frame_id_ frame doesn't exist.  We only care about in-time
    // transformation for on-the-move pose-setting, so ignoring this
    // startup condition doesn't really cost us anything.
    if(sent_first_transform_)
      ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
    tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform);
  }

  tf2::Transform tx_odom_tf2;
  tf2::convert(tx_odom.transform, tx_odom_tf2);
  tf2::Transform pose_old, pose_new;
  tf2::convert(msg.pose.pose, pose_old);
  pose_new = pose_old * tx_odom_tf2;

  // Transform into the global frame

  ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
           ros::Time::now().toSec(),
           pose_new.getOrigin().x(),
           pose_new.getOrigin().y(),
           tf2::getYaw(pose_new.getRotation()));
  // Re-initialize the filter
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
  pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  // Copy in the covariance, converting from 6-D to 3-D
  for(int i=0; i<2; i++)
  {
    for(int j=0; j<2; j++)
    {
      pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];
    }
  }
  pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];

  delete initial_pose_hyp_;
  initial_pose_hyp_ = new amcl_hyp_t();
  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
  applyInitialPose();
}

其中提到的applyInitialPose(),用于设置位姿：

void
AmclNode::applyInitialPose()
{
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
  if( initial_pose_hyp_ != NULL && map_ != NULL ) {
    pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
    pf_init_ = false;

    delete initial_pose_hyp_;
    initial_pose_hyp_ = NULL;
  }
}

7.核心函数:laserReceived()的关键条件分支

下面贴的代码片是laserReceived()函数的关键条件分支，下一讲我们再对laserReceived函数进行分析。

图6.laserReceived函数关键条件分支展示

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

posted on 2022-09-14 13:57 tdyizhen1314 阅读(386) 评论(0) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部