Autoware 定位之数据稳定处理（十）

0. 简介

这一讲按照《Autoware 技术代码解读（三）》梳理的顺序，我们来说一说Autoware中的数据稳定处理操作，这一讲的内容比较多，主要分为：

pose_instability_detector 节点，旨在监测 /localization/kinematic_state 的稳定性，该主题是扩展卡尔曼滤波器（EKF）的输出。
pose2twist 节点从输入的姿态历史中计算速度。除了计算出的twist之外，该节点还输出线性-x和角度-z分量作为浮点消息，以简化调试。
stop_filter 当这个功能不存在时，每个节点都使用不同的标准来确定车辆是否停止，导致一些节点在停车模式下运行，而另一些节点继续以驾驶模式运行。
tree_structured_parzen_estimator 是一个用于黑盒优化的软件包。这个软件包没有节点，它只是一个库。
twist2accel 这个软件包负责利用ekf_localizer的输出来估计加速度。它使用低通滤波器来减少噪音。

1. pose_instability_detector节点代码阅读

这段代码实现了一个姿态不稳定性检测器，用于监测机器人姿态变化是否超出设定阈值。在初始化函数中，节点名称和参数被设置，并创建了订阅器、定时器和发布器。订阅器用于接收里程计和速度信息，定时器用于定时触发回调函数，发布器用于发布调试信息和诊断信息。

当接收到里程计和速度信息时，会分别调用回调函数，将最新的信息存储起来。定时器触发的回调函数会计算姿态变化，并发布诊断信息和调试信息。首先判断是否有足够的数据进行计算，然后将四元数转换为欧拉角，比较姿态信息和最新的里程计信息，计算出姿态差异。接着根据设定的阈值对姿态差异进行判断，并发布诊断信息和调试信息。当中比较两个姿态：

通过在/localization/kinematic_state接收到的最后一条消息中积分得到的扭矩数值，积分的持续时间由interval_sec指定。
来自/localization/kinematic_state的最新姿态。
然后将比较结果输出到/diagnostics话题。

如果这个节点向/diagnostics输出警告消息，意味着EKF输出与积分得到的扭矩数值存在显著差异。这种差异表明可能存在估计姿态或输入扭矩方面的问题。

以下图表概述了这个过程的时间线是什么样子的：

在这里插入图片描述

/// @brief 初始化了节点名称和参数，并创建了订阅器、定时器和发布器
/// @param options 节点选项
PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options)
: Node("pose_instability_detector", options),
  threshold_diff_position_x_(this->declare_parameter<double>("threshold_diff_position_x")),
  threshold_diff_position_y_(this->declare_parameter<double>("threshold_diff_position_y")),
  threshold_diff_position_z_(this->declare_parameter<double>("threshold_diff_position_z")),
  threshold_diff_angle_x_(this->declare_parameter<double>("threshold_diff_angle_x")),
  threshold_diff_angle_y_(this->declare_parameter<double>("threshold_diff_angle_y")),
  threshold_diff_angle_z_(this->declare_parameter<double>("threshold_diff_angle_z"))
{
  odometry_sub_ = this->create_subscription<Odometry>(
    "~/input/odometry", 10,
    std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1));//接收Odometry消息的订阅器

  twist_sub_ = this->create_subscription<TwistWithCovarianceStamped>(
    "~/input/twist", 10,
    std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1));//接收TwistWithCovarianceStamped消息的订阅器

  const double interval_sec = this->declare_parameter<double>("interval_sec");
  timer_ = rclcpp::create_timer(
    this, this->get_clock(), std::chrono::duration<double>(interval_sec),
    std::bind(&PoseInstabilityDetector::callback_timer, this));//定时触发回调函数

  diff_pose_pub_ = this->create_publisher<PoseStamped>("~/debug/diff_pose", 10);
  diagnostics_pub_ = this->create_publisher<DiagnosticArray>("/diagnostics", 10);
}

/// @brief 当接收到odometry消息时被调用，将最新的odometry消息存储在latest_odometry_中
/// @param odometry_msg_ptr 指向接收到的Odometry消息的指针
void PoseInstabilityDetector::callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr)
{
  latest_odometry_ = *odometry_msg_ptr;
}

/// @brief 当接收到twist消息时被调用，将最新的twist消息存储在twist_buffer_中
/// @param twist_msg_ptr 指向接收到的TwistWithCovarianceStamped消息的指针
void PoseInstabilityDetector::callback_twist(
  TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr)
{
  twist_buffer_.push_back(*twist_msg_ptr);
}

/// @brief 定时器触发的回调函数，用于计算姿态变化并发布诊断信息和调试信息
void PoseInstabilityDetector::callback_timer()
{
    //首先判断是否有足够的数据进行计算，若无则返回
  if (latest_odometry_ == std::nullopt) {
    return;
  }
  if (prev_odometry_ == std::nullopt) {
    prev_odometry_ = latest_odometry_;
    return;
  }

//将四元数转换为欧拉角函数
  auto quat_to_rpy = [](const Quaternion & quat) {
    tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w);
    tf2::Matrix3x3 mat(tf2_quat);
    double roll{};
    double pitch{};
    double yaw{};
    mat.getRPY(roll, pitch, yaw);
    return std::make_tuple(roll, pitch, yaw);
  };

    //比较姿态信息和最新的odometry信息，计算出姿态差异。
  Pose pose = prev_odometry_->pose.pose;
  rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp);
  for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) {
    const Twist twist = twist_with_cov.twist.twist;

    const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp);
    if (curr_time > latest_odometry_->header.stamp) {
      break;
    }

    const rclcpp::Duration time_diff = curr_time - prev_time;
    const double time_diff_sec = time_diff.seconds();
    if (time_diff_sec < 0.0) {
      continue;
    }

    // quat to rpy
    auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation);

    // 更新欧拉角
    ang_x += twist.angular.x * time_diff_sec;
    ang_y += twist.angular.y * time_diff_sec;
    ang_z += twist.angular.z * time_diff_sec;
    tf2::Quaternion quat;
    quat.setRPY(ang_x, ang_y, ang_z);

    // Convert twist to world frame
    // 将twist转换到世界坐标系中
    tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z);
    linear_velocity = tf2::quatRotate(quat, linear_velocity);

    // update
    pose.position.x += linear_velocity.x() * time_diff_sec;
    pose.position.y += linear_velocity.y() * time_diff_sec;
    pose.position.z += linear_velocity.z() * time_diff_sec;
    pose.orientation.x = quat.x();
    pose.orientation.y = quat.y();
    pose.orientation.z = quat.z();
    pose.orientation.w = quat.w();
    prev_time = curr_time;
  }

  // 比较姿态信息和最新的odometry信息，计算出姿态差异。
  const Pose latest_ekf_pose = latest_odometry_->pose.pose;
  const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose);
  const geometry_msgs::msg::Point pos = ekf_to_odom.position;
  const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation);
  const std::vector<double> values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z};

  const rclcpp::Time stamp = latest_odometry_->header.stamp;

  // 发布调试信息
  PoseStamped diff_pose;
  diff_pose.header = latest_odometry_->header;
  diff_pose.pose = ekf_to_odom;
  diff_pose_pub_->publish(diff_pose);

//阈值
  const std::vector<double> thresholds = {threshold_diff_position_x_, threshold_diff_position_y_,
                                          threshold_diff_position_z_, threshold_diff_angle_x_,
                                          threshold_diff_angle_y_,    threshold_diff_angle_z_};

  const std::vector<std::string> labels = {"diff_position_x", "diff_position_y", "diff_position_z",
                                           "diff_angle_x",    "diff_angle_y",    "diff_angle_z"};

  // 发布诊断信息
  DiagnosticStatus status;
  status.name = "localization: pose_instability_detector";
  status.hardware_id = this->get_name();
  bool all_ok = true;

  for (size_t i = 0; i < values.size(); ++i) {
    const bool ok = (std::abs(values[i]) < thresholds[i]);
    all_ok &= ok;
    diagnostic_msgs::msg::KeyValue kv;
    kv.key = labels[i] + ":threshold";
    kv.value = std::to_string(thresholds[i]);
    status.values.push_back(kv);
    kv.key = labels[i] + ":value";
    kv.value = std::to_string(values[i]);
    status.values.push_back(kv);
    kv.key = labels[i] + ":status";
    kv.value = (ok ? "OK" : "WARN");
    status.values.push_back(kv);
  }
  status.level = (all_ok ? DiagnosticStatus::OK : DiagnosticStatus::WARN);
  status.message = (all_ok ? "OK" : "WARN");

  DiagnosticArray diagnostics;
  diagnostics.header.stamp = stamp;
  diagnostics.status.emplace_back(status);
  diagnostics_pub_->publish(diagnostics);

  // prepare for next loop
  prev_odometry_ = latest_odometry_;
  twist_buffer_.clear();
}

2. pose2twist代码阅读

这里将位姿信息转换为速度信息。在构造函数中，初始化了ROS节点，并创建了发布者和订阅者。代码中包含了一些帮助函数，用于计算两个弧度之间的差值，获取位姿信息中的roll、pitch和yaw角度，并根据两个位姿消息计算线速度和角速度。回调函数处理接收到的位姿消息，计算并发布相应的速度消息。

当中twist.linear.x被计算为 $sqrt(d_x * d_x + d_y * d_y + d_z * d_z) / dt$ ，而y和z字段中的值为零。twist.angular被计算为每个字段的d_roll / dt，d_pitch / dt和d_yaw / dt。

/// @brief 是类的构造函数，初始化了ROS节点，并创建了发布者和订阅者
Pose2Twist::Pose2Twist() : Node("pose2twist_core")
{
  using std::placeholders::_1;

  static constexpr std::size_t queue_size = 1;
  rclcpp::QoS durable_qos(queue_size);
  durable_qos.transient_local();

  twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", durable_qos);
  linear_x_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>("linear_x", durable_qos);
  angular_z_pub_ =
    create_publisher<tier4_debug_msgs::msg::Float32Stamped>("angular_z", durable_qos);
  // Note: this callback publishes topics above
  pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
    "pose", queue_size, std::bind(&Pose2Twist::callbackPose, this, _1));
}

/// @brief 函数计算两个弧度之间的差值，确保结果在-pi到pi之间
/// @param lhs_rad 左边的弧度
/// @param rhs_rad 右边的弧度
/// @return 
double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
{
  double diff_rad = lhs_rad - rhs_rad;//计算两个弧度之间的差值
  if (diff_rad > M_PI) {
    diff_rad = diff_rad - 2 * M_PI;
  } else if (diff_rad < -M_PI) {
    diff_rad = diff_rad + 2 * M_PI;
  }
  return diff_rad;
}

// x: roll, y: pitch, z: yaw
/// @brief 根据输入的位姿信息，返回roll、pitch和yaw角度。
/// @param pose 输入的位姿信息
/// @return 
geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose)
{
  geometry_msgs::msg::Vector3 rpy;
  tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
  return rpy;
}

/// @brief 调用上面的getRPY函数，获取传入位姿消息指针对应的roll、pitch和yaw角度。
/// @param pose 位姿消息指针
/// @return 
geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose)
{
  return getRPY(pose->pose);
}

/// @brief 根据两个位姿消息计算得到线速度和角速度，然后封装成TwistStamped消息返回
/// @param pose_a 位置a
/// @param pose_b 位置b
/// @return 
geometry_msgs::msg::TwistStamped calcTwist(
  geometry_msgs::msg::PoseStamped::SharedPtr pose_a,
  geometry_msgs::msg::PoseStamped::SharedPtr pose_b)
{
  const double dt =
    (rclcpp::Time(pose_b->header.stamp) - rclcpp::Time(pose_a->header.stamp)).seconds();

  if (dt == 0) {
    geometry_msgs::msg::TwistStamped twist;
    twist.header = pose_b->header;
    return twist;
  }

  const auto pose_a_rpy = getRPY(pose_a);
  const auto pose_b_rpy = getRPY(pose_b);

  geometry_msgs::msg::Vector3 diff_xyz;
  geometry_msgs::msg::Vector3 diff_rpy;

//计算两个位姿之间的差值
  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;

  return twist;
}
/// @brief 回调函数，处理接收到的位姿消息，计算并发布相应的速度消息。其中包括计算两次位姿之间的变化量，更新上一次的位姿消息，计算速度消息并发布。
/// @param pose_msg_ptr 指向接收到的位姿消息的指针
void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
{
  // TODO(YamatoAndo) check time stamp diff
  // TODO(YamatoAndo) check suddenly move
  // TODO(YamatoAndo) apply low pass filter

  geometry_msgs::msg::PoseStamped::SharedPtr current_pose_msg = pose_msg_ptr;
  static geometry_msgs::msg::PoseStamped::SharedPtr prev_pose_msg = current_pose_msg;
  //计算两次位姿之间的变化量
  geometry_msgs::msg::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg);
  prev_pose_msg = current_pose_msg;
  twist_msg.header.frame_id = "base_link";
  twist_pub_->publish(twist_msg);

  tier4_debug_msgs::msg::Float32Stamped linear_x_msg;
  linear_x_msg.stamp = this->now();
  linear_x_msg.data = twist_msg.twist.linear.x;
  linear_x_pub_->publish(linear_x_msg);

  tier4_debug_msgs::msg::Float32Stamped angular_z_msg;
  angular_z_msg.stamp = this->now();
  angular_z_msg.data = twist_msg.twist.angular.z;
  angular_z_pub_->publish(angular_z_msg);
}

3. stop_filter代码阅读

这段代码是一个停止过滤器的类，它订阅里程计消息，并根据预设的速度和角速度阈值来判断车辆是否停止。在构造函数中，它初始化了节点并创建了订阅器、发布器以及其他参数。当收到里程计消息时，回调函数会判断车辆的线速度和角速度是否小于预设的阈值，如果是，则将车辆的速度和角速度设为0，并发布一个布尔型的停止标志消息以及处理后的里程计消息。