autoware.universe源码略读(2.4)--localization:pose_initializer/pose2twist/stop_filter/twist2accel

Overview

在我所在的这个galatic分支，pose_initializer是分为了两个部分，分别是map_height_fitter和pose_initializer，其中，前者是对地图高度的拟合，后者会将位置发送给了ekf_localizer，下面的流程图应该是最新版的，不过相对于galatic这个版改动还是有点多的。下面分来看一下

map_height_fitter

这个也是通过实例化一个类来实现节点调用的。在构造函数里，订阅的消息是pointcloud_map，也就是地图，然后回生成的服务就是拟合高度

  sub_map_ = create_subscription<sensor_msgs::msg::PointCloud2>(
    "pointcloud_map", durable_qos, std::bind(&MapHeightFitter::on_map, this, _1));
  srv_fit_ = create_service<RequestHeightFitting>(
    "fit_map_height", std::bind(&MapHeightFitter::on_fit, this, _1, _2));

订阅没什么好说的，就是把ROS格式的点云信息转成了pcl的格式。至于拟合高度的部分，是需要有外部点输入的

  const auto & position = req->pose_with_covariance.pose.pose.position;
  tf2::Vector3 point(position.x, position.y, position.z);
  std::string req_frame = req->pose_with_covariance.header.frame_id;

然后如果有地图的话，会对高度进行估计，具体的估计是通过get_ground_height函数实现的。这里是先找到地图点云距离输入点最近的距离，然后以距离+1为半径画⚪，把范围内最小的z作为最终的高度

double MapHeightFitter::get_ground_height(const tf2::Vector3 & point) const
{
  const double x = point.getX();
  const double y = point.getY();

  // find distance d to closest point
  double min_dist2 = INFINITY;
  for (const auto & p : map_cloud_->points) {
    const double dx = x - p.x;
    const double dy = y - p.y;
    const double sd = (dx * dx) + (dy * dy);
    min_dist2 = std::min(min_dist2, sd);
  }

  // find lowest height within radius (d+1.0)
  const double radius2 = std::pow(std::sqrt(min_dist2) + 1.0, 2.0);
  double height = INFINITY;
  for (const auto & p : map_cloud_->points) {
    const double dx = x - p.x;
    const double dy = y - p.y;
    const double sd = (dx * dx) + (dy * dy);
    if (sd < radius2) {
      height = std::min(height, static_cast<double>(p.z));
    }
  }

  return std::isfinite(height) ? height : point.getZ();
}

所以这一步的目的我的理解就是：GNSS算出来的Z确实应该是表现最差的，或者像仿真的时候直接在rviz界面上画出来的位置Z并不准，根据点云地图这么来修正一下？

---

最新版本的代码好像把这里的内容删除了

---

pose_initializer

这个里面的节点也是通过实例化类实现的PoseInitializer

pose_initializer_core

在构造函数里，分别根据两个参数配置选项，gnss_enabled以及ndt_enabled分别进行GNSS模块的和NDT模块的配置，然后还有一个stop_check_enabled应该是车辆停止检测的部分。而在这里初始化的服务，感觉就是发布初始化的位置，并且把状态设置到初始化或者初始化失败的部分。接下来分开看下其他几个文件。

copy_vector_to_array

这个也是个工具性的文件，核心思想就是将协方差信息，以std::array的类型得到，因为const auto parameter = node->template declare_parameter<std::vector<double>>(name);这里得到的类型的是std::vector，因为理论上协方差的大小是固定的，所以这里就给转成array了。

gnss_module

这个模块只有两个函数，一个是构造函数，还有一个get_pose就是单纯的一个位置接口。这里的cli_map_fit变量是一个服务客户端对象的成员变量，用于与服务端进行通信。也就是说这里是和上面提到的RequestHeightFitting进行通信的一个过程，我觉得这里可以理解为取GNSS位置得时候进行一下高度的拟合？在get_pose函数里，会发送这个client的请求

  const auto req = std::make_shared<RequestHeightFitting::Request>();
  req->pose_with_covariance = *pose_;

  auto future = cli_map_fit_->async_send_request(req);
  return future.get()->pose_with_covariance;

localization_trigger_module

这是一个定位模块的触发器，类中有两个client，感觉可以简单理解client_ekf_trigger_是请求ekf定位模块的，然后client_ndt_trigger_就是用来请求ndt定位模块的。然后两个函数，deactivate用来停止两个模块，activate激活两个模块。

• 这里应该能解释之前看的疑问了？模块定位的服务是在这里以请求的形式被调用了的（应该？）

ndt_module

这个和GNSS那个模块很像，感觉也是发送调用NDT定位模块的请求，也就是align_pose函数，具体操作也是差不多的

stop_check_module

这个模块看起来是用来做车辆停止检测的，在构造函数里创建了一个订阅话题

  sub_twist_ = node->create_subscription<TwistWithCovarianceStamped>(
    "stop_check_twist", 1, std::bind(&StopCheckModule::on_twist, this, std::placeholders::_1));

但是在这个on_twist函数里，似乎又只是把解析出来的msg消息addTwist了，然后具体的静止检测的地方在PoseInitializer::on_initialize中

  if (stop_check_ && !stop_check_->isVehicleStopped(stop_check_duration_)) {
    throw ServiceException(
      Initialize::Service::Response::ERROR_UNSAFE, "The vehicle is not stopped.");
  }

所以根据这里的逻辑，这里应该是要求整个定位模块初始化的时候，车辆是保持静止状态的。

pose2twist

这个里面，是根据位置信息，来进行运动量(线速度和角速度)的计算。首先在构造函数中，有一个ROS2的新特性：

  rclcpp::QoS durable_qos(queue_size);
  durable_qos.transient_local();

这里我搜索了一下，发现这里设置的是用发布者和订阅者之间的通信质量，像下面的transient_local对应的就是一种策略，根据ChatGPT的解释：

Transient Local：
持久性：消息在发布者的生命周期内持久化，即使发布者在发布后立即退出，消息仍然会保留在系统中。
订阅者接收历史消息：当新的订阅者连接时，它们会收到发布者在其生命周期内发布的最后一条消息。这对于一些需要保留最新状态的应用场景非常有用，比如在系统启动时，新的订阅者需要了解最新的系统状态。
应用场景
设置 transient_local() 的场景包括：
状态信息：发布者发布系统的最新状态，当新的订阅者加入时，需要立即获取最新状态信息。
配置参数：发布者发布一些配置参数，当新的订阅者加入时，需要获取这些参数来正确配置自己。

具体的速度计算是通过回调函数callbackPose实现的，里面的线速度是根据三维的位置然后归到x轴一个轴上，角速度倒是会三个轴分开来算

  diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x;
  diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y;
  diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z;
  diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x);
  diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y);
  diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z);

  geometry_msgs::msg::TwistStamped twist;
  twist.header = pose_b->header;
  twist.twist.linear.x =
    std::sqrt(std::pow(diff_xyz.x, 2.0) + std::pow(diff_xyz.y, 2.0) + std::pow(diff_xyz.z, 2.0)) /
    dt;
  twist.twist.linear.y = 0;
  twist.twist.linear.z = 0;
  twist.twist.angular.x = diff_rpy.x / dt;
  twist.twist.angular.y = diff_rpy.y / dt;
  twist.twist.angular.z = diff_rpy.z / dt;

最后发布的，是只有x轴线速度和z轴角速度的（和车体模型是对应的）

stop_filter

这个就是很简单的一个“滤波”，和什么均值滤波、高斯滤波都不太一样，这个就是根据速度阈值判断一下是不是处在停止状态(这和之前的stop_check不重复么？)，然后如果小于阈值的话，就强行设置为0了，并且把最新的odom消息发布出去。

twist2accel

这里是根据速度信息反算加速度信息，除了线速度的加速度信息，这里连角速度的加速度信息都给算了，这里还用到了下一维的低通滤波，来把加速度的值平滑一下

// 先在构造函数里定义低通滤波器
lpf_alx_ptr_ = std::make_shared<LowpassFilter1d>(accel_lowpass_gain_);
// 具体的计算函数里回计算加速度值，并执行滤波操作
double alx = (msg->twist.linear.x - prev_twist_ptr_->twist.linear.x) / dt;
accel_msg.accel.accel.linear.x = lpf_alx_ptr_->filter(alx);